ROS中用rosbag记录的数据仿真时发布TF在RVIZ中遇到“Message removed because it is too old”的问题解决

用rosbag的里程计和激光数据mapping，想在RVIZ中不仅显示occupancy grid map，也显示激光数据，为此需要自己编写TF的broadcaster来发布scan到odom的坐标变换，代码如下

其中scan的坐标系名字（frame_id）叫“laser”，这是因为rosbag中scan的数据里面header.frame_id 就定义为了“laser”

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>


static double x_r_l, y_r_l, theta_r_l;

void odomCallback ( const nav_msgs::OdometryConstPtr& odom )
{
    /* 获取机器人姿态 */
    double x_r = odom->pose.pose.position.x;
    double y_r = odom->pose.pose.position.y;
    double theta_r = tf::getYaw ( odom->pose.pose.orientation );

    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(x_r+x_r_l, y_r+y_r_l, 0.0));
    transform.setRotation(tf::createQuaternionFromYaw(theta_r+theta_r_l));
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "laser"));
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf_broadcaster");
    ros::NodeHandle nh1;

    // 从yaml文件中读取laser相对于robot的pose
    nh1.getParam ( "/tf_things/robot_laser/x", x_r_l );
    nh1.getParam ( "/tf_things/robot_laser/y", y_r_l );
    nh1.getParam ( "/tf_things/robot_laser/theta", theta_r_l );

    ros::Subscriber g_odom_sub;
    g_odom_sub = nh1.subscribe ( "/mbot/odometry", 1, odomCallback );

    ros::spin();

    return 0;
}

 

但这样做之后，在RVIZ中add一个LaserScan后，选择topic为/scan，结果报错“Message removed because it is too old”，用 $rosrun tf view_frames 

$rosrun tf echo odom laser

检查都没问题，说明代码没问题，TF可以正常发送。

最后找到了解决方法：https://answers.ros.org/question/52788/error-in-display-of-pointcloud-in-base_link-frame/

即在launch文件顶部加入：<param name="use_sim_time" value="true"/>

然后在rosbag 节点加入：--clock

这样做的含义是所有node都使用仿真时间而不是ros time

如下所示

<launch>

  <param name="use_sim_time" value="true"/>

  <node pkg="occ_grid_mapping" type="odometry" name="odometry" output="screen" clear_params="true">
						<rosparam file="$(find occ_grid_mapping)/config/default.yaml" command="load" />
  </node>

  <node pkg="occ_grid_mapping" type="mapping" name="mapping" output="screen" clear_params="true">
						<rosparam file="$(find occ_grid_mapping)/config/default.yaml" command="load" />
  </node>

  <node pkg="occ_grid_mapping" type="tf_things" name="tf_things" output="screen" clear_params="true">
						<rosparam file="$(find occ_grid_mapping)/config/default.yaml" command="load" />
  </node>
  
	<node pkg="rviz" type="rviz" name="rviz" output="screen" 
      args="-d $(find occ_grid_mapping)/rviz/default.rviz" required="true">
  </node>
  
  <!-- -r 5: replay with rate = 5hz 即以记录时的5倍速回放 -->
  <node pkg="rosbag" type="play" name="rosbag" args="--clock -r 5 $(find occ_grid_mapping)/laser_dataset/laser2_2018-07-14-18-41-42.bag"/>

</launch>