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" />
若想封装为其他格式的数据流程类似。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
