cartographer_ros定位功能位姿獲取與重定位設置

本人小白，項目中使用cartographer進行機器人的定位。cartographer_ros中沒有發佈機器人在地圖座標系中的位姿topic，只發布各frame間的tf變換。若通過tf變換獲取位姿座標，測試發現不管激光頻率多高（20~100hz），獲取的位姿頻率只有5h左右。因此對源碼進行簡單修改，獲取高頻率的位姿topic並在rviz中根據機器人當前位置進行重定位。

準備工作

源碼安裝

首先對cartographer進行源碼安裝，相關鏈接如下：

• cartographer項目源碼cartographer

• cartographer_ros項目源碼cartographer_ros

• 源碼安裝教程源碼安裝

demo運行

cartographer_rosd的demo運行demo，主要參考.launch運行文件和.lua配置文件。需要注意的是需要開啓pure localization功能。

位姿獲取

tf座標變換-不修改源碼

通過tf變換獲取位姿座標，獲取的位姿頻率只有5hz左右，但不用修改源碼。

建立tf變換節點，發佈2D位姿topic。

ros::NodeHandle nh;
ros::Publisher _pose_pub;
_pose_pub = nh.advertise<geometry_msgs::Pose2D>("pose_nav", 10);

tf::StampedTransform transform;
    tf::TransformListener listener;
    try
    {
        listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
    }
    catch (tf::TransformException &ex)
    {
        ROS_ERROR("%s", ex.what());
    }
    //輸出位置信息
    pos_now.x = static_cast<double>(transform.getOrigin().x());
    pos_now.y = static_cast<double>(transform.getOrigin().y());
    pos_now.theta = tf::getYaw(transform.getRotation());
    _pose_pub.publish(pos_now);

修改源碼

在cartographer_ros/cartographer_ros/cartographer_ros/node.h添加::ros::Publisher _pose_pub。對node.cc進行修改：

_pose_pub = node_handle_.advertise<geometry_msgs::Pose2D>("pose_nav", 10);//100hz  
geometry_msgs::Pose2D pos_now;

if (trajectory_data.published_to_tracking != nullptr) {
      if (trajectory_data.trajectory_options.provide_odom_frame) {
        std::vector<geometry_msgs::TransformStamped> stamped_transforms;

        stamped_transform.header.frame_id = node_options_.map_frame;
        stamped_transform.child_frame_id =
            trajectory_data.trajectory_options.odom_frame;
        stamped_transform.transform =
            ToGeometryMsgTransform(trajectory_data.local_to_map);
        stamped_transforms.push_back(stamped_transform);

        stamped_transform.header.frame_id =
            trajectory_data.trajectory_options.odom_frame;
        stamped_transform.child_frame_id =
            trajectory_data.trajectory_options.published_frame;
        stamped_transform.transform = ToGeometryMsgTransform(
            tracking_to_local * (*trajectory_data.published_to_tracking));
        stamped_transforms.push_back(stamped_transform);

        tf_broadcaster_.sendTransform(stamped_transforms);

        geometry_msgs::Transform transform = ToGeometryMsgTransform(
            tracking_to_map * (*trajectory_data.published_to_tracking));
        //********修改部分：添加位姿發佈**********/
        pos_now.x = static_cast<double>(transform.translation.x);
        pos_now.y = static_cast<double>(transform.translation.y);
        pos_now.theta = tf::getYaw(transform.rotation);
        _pose_pub.publish(pos_now);
      } 

rviz重定位設置

訂閱/initialpose話題

rviz中的“2D Pose Estimate”可發佈/initialpose話題，通過點擊地圖位置可以發佈相應位置的topic，包括x,y和theta。

_pose_init_sub = nh.subscribe("/initialpose", 1000, &NavNode::init_pose_callback, this);

重定位設置

重定位功能通過調用API設置，參考API。

void NavNode::init_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
    double x = msg->pose.pose.position.x;
    double y = msg->pose.pose.position.y;
    double theta = tf2::getYaw(msg->pose.pose.orientation);
    ros::NodeHandle nh;

    ros::ServiceClient client_traj_finish = nh.serviceClient<cartographer_ros_msgs::FinishTrajectory>("finish_trajectory");
    cartographer_ros_msgs::FinishTrajectory srv_traj_finish;
    srv_traj_finish.request.trajectory_id = traj_id;
    if (client_traj_finish.call(srv_traj_finish))
    {
        ROS_INFO("Call finish_trajectory %d success!", traj_id);
    }
    else
    {
        ROS_INFO("Failed to call finish_trajectory service!");
    }

    ros::ServiceClient client_traj_start = nh.serviceClient<cartographer_ros_msgs::StartTrajectory>("start_trajectory");
    cartographer_ros_msgs::StartTrajectory srv_traj_start;
    srv_traj_start.request.configuration_directory = "xxx";//.lua文件所在路徑
    srv_traj_start.request.configuration_basename = "xxx.lua";//lua文件
    srv_traj_start.request.use_initial_pose = 1;
    srv_traj_start.request.initial_pose = msg->pose.pose;
    srv_traj_start.request.relative_to_trajectory_id = 0;
    if (client_traj_start.call(srv_traj_start))
    {
        // ROS_INFO("Status ", srv_traj_finish.response.status)
        ROS_INFO("Call start_trajectory %d success!", traj_id);
        traj_id++;
    }
    else
    {
        ROS_INFO("Failed to call start_trajectory service!");
    }
}

完畢，若有問題，歡迎溝通，大家共同學習。