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>