amcl定位过程中发布机器人在地图中位姿的总结

「这是我参与2022首次更文挑战的第6天，活动详情查看：2022首次更文挑战」。

目前要实现的应用是amcl匹配后，得到车体位姿，然后发布到外部使用

起初我是采用订阅amcl发布的amcl_pose话题的方法

代码如下

参考ROS：订阅话题并发布（订阅amcl_pose数据并发布）_Better-ing的博客-CSDN博客

class amcl_pose_sub_pub { private: ros::NodeHandle nh_; // 定义ROS句柄 ros::Publisher msgPointPub; ros::Subscriber sub;

public: amcl_pose_sub_pub() { msgPointPub = nh_.advertise<geometry_msgs::PointStamped>("amcl_pose_", 1000); sub = nh_.subscribe("/amcl_pose", 1, &amcl_pose_sub_pub::amcl_pose_CallBack,this); }

~amcl_pose_sub_pub(){}

void amcl_pose_CallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
    geometry_msgs::PointStamped msgPointStamped;
    msgPointStamped.point=msg->pose.pose.position;
    msgPointStamped.header.stamp = msg->header.stamp;
    msgPointStamped.header.frame_id = "map";
    msgPointPub.publish(msgPointStamped);
    std::cout<<"amcl"<<std::endl;
}

}; int main(int argc, char** argv) { ros::init(argc, argv,"amcl_pose_sub_pub"); amcl_pose_sub_pub obj; ros::spin(); return 0; }

效果如图 （rviz里面添加pointstanted订阅发布的amcl_pose话题，图中紫色的点）

但是发现amcl_pose的频率很低。amcl_pose话题的发布需要经过重采样，而重采样的触发条件是里程计显示小车平移了0.2m(参数update_min_d)或者旋转了30度（参数update_min_a）。而且我在rviz里点击了重新设置车体坐标的按钮之后才能显示出来

但是这两个参数阈值如果改的很小，虽然amcl_pose可以很快发布的，但是这就会导致例子很快收敛而不能定位正确。所以直接获取amcl_pose的方法是不可行的。

参考之前使用cartographer纯定位的时候，通过tf树获取车体位姿的办法

// //---------------------------------------------------------- #include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> #include <turtlesim/Spawn.h> #include #include using namespace std; double x,y,z,ww,zz,hh,ii,Aww,Azz,Ahh,Aii; double theta; int main(int argc, char** argv){ ros::init(argc, argv, "tf_subscriber");

ros::NodeHandle node; tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()){ tf::StampedTransform transform; try{ //得到坐标odom和坐标base_link之间的关系 listener.waitForTransform("map","base_footprint", ros::Time(0), ros::Duration(3.0)); listener.lookupTransform("map", "base_footprint", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); }

x=transform.getOrigin().x();
y=transform.getOrigin().y();
z=transform.getOrigin().z();

tf::Quaternion q = transform.getRotation();
double qx = q.x();
double qy = q.y();
double qz = q.z();
double qw = q.w();

printf("x: %f, y: %f, z: %f, qx: %f,qy: %f,qz: %f, qw: %f\n",x,y,z,qx,qy,qz,qw);

rate.sleep();

} return 0; };

效果如下