cartographer输出机器人相对地图位姿

 cartographer（0.3.0版本）中机器人位姿是以tf的格式发布的。cartographer在关于位姿的tf_tree为：map->odom->base_footprint。其中odom->base_footprint理解为激光模拟的里程计信息，map->odom理解为对里程计数据的补偿，如消除累积误差等。在应用中推荐把provide_odom_frame参数置为false。当该参数为true时，其发布的数据为map->odom的tf，而非机器人的位姿。当将provide_odom_frame参数置为false时，可以获得map->base_footprint的tf数据，即机器人相对于map的位姿。

       但是，有时会希望在工程中将机器人相对于map的位姿以其他msgs格式发布。这种情况需要对cartographer_ros的源码进行修改。下面以将机器人相对于map的位姿数据封装成odometry的格式发布为例。

       改动主要在node.cc中。

    void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
    laser_odom_pub_ = node_handle_.advertise<nav_msgs::Odometry>("laser_odom",1000);
    ...
    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
      if (trajectory_state.published_to_tracking != nullptr) {
      nav_msgs::Odometry laser_odom_;
      ...
      if (trajectory_state.trajectory_options.provide_odom_frame) {
      ...  //参数provide_odom_frame为true时的情况
       }
      else{
            stamped_transform.header.frame_id = node_options_.map_frame;
            stamped_transform.child_frame_id =
                trajectory_state.trajectory_options.published_frame;
            stamped_transform.transform = ToGeometryMsgTransform(
                tracking_to_map * (*trajectory_state.published_to_tracking));
            tf_broadcaster_.sendTransform(stamped_transform);
     
            laser_odom_.header.stamp = stamped_transform.header.stamp;
            laser_odom_.header.frame_id = "laser_odom";
            laser_odom_.child_frame_id = "base_footprint";
            laser_odom_.pose.pose.position.x = stamped_transform.transform.translation.x;
            laser_odom_.pose.pose.position.y = stamped_transform.transform.translation.y;
            laser_odom_.pose.pose.position.z = stamped_transform.transform.translation.z;
            laser_odom_.pose.pose.orientation = stamped_transform.transform.rotation;
            laser_odom_pub_.publish(laser_odom_);
          }
        }}}

       如此机器人将发布一个名为laser_odom的topic，topic的内容为机器人相对于map的位姿。

       为了在rviz中观察到laser_odom，在launch文件中加入map到laser_odom的静态tf即可。

<node pkg="tf" type="static_transform_publisher" name="map_to_laser_odom" args="0.0 0.0 0.0 0.0 0.0 0.0 /map /laser_odom 100" />

       若想封装为其他格式的数据流程类似。