Sync problems between ROS and Gazebo

edit

Hi all,

I built a simple simulated environment and a diff-drive robot (with an Hokuyo and a Kinect) in Gazebo to perform some mapping experiments. All the config files are in this repository and you may test it by running:

roslaunch lucrezio_simulation_environments empty_world_with_apartment_and_robot.launch

After running the experiments I noticed strange behaviors. This is what happens when I try to localize the robot on a map that I previously built and, at the same time, on RViz I want to visualize the colored point cloud returned by the Kinect:

https://youtu.be/v7DAr12hxO0

The only interpretation I can give is that there is some delay in the published transforms.

To check this, I wrote a minimal node that does the following:

• listens to /scan topic to receive laser messages

• uses tf::TransformListener to receive the TF between /odom and /base_footprint frames

this is the code:

void laserCallback(const sensor_msgs::LaserScan::ConstPtr &laser_msg){
  ros::Time stamp = laser_msg->header.stamp;
  ROS_INFO("Laser msg timestamp: %f",stamp.toSec());

  tf::TransformListener odom_listener;
  tf::StampedTransform odom_tf;

  try{
    odom_listener.waitForTransform("/odom",
                                   "/base_footprint",
                                   stamp,
                                   ros::Duration(1.0));
    odom_listener.lookupTransform("/odom",
                                  "/base_footprint",
                                  stamp,
                                  odom_tf);
  } catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
  }
  ROS_INFO("Odom tf timestamp: %f",odom_tf.stamp_.toSec());
}

when I execute it, this is what I get:

dede@srrg-02:~$ rosrun lucrezio_semantic_mapper pose_broadcaster_node 
[ INFO] [1529497453.859222896, 17.178000000]: Laser msg timestamp: 17.145000
[ERROR] [1529497454.891297562, 18.207000000]: Lookup would require extrapolation into the past.  Requested time 17.145000000 but the earliest data is at time 17.356000000, when looking up transform from frame [base_footprint] to frame [odom]
[ INFO] [1529497454.891428167, 18.207000000]: Odom tf timestamp: 0.000000
[ INFO] [1529497454.902422031, 18.218000000]: Laser msg timestamp: 18.196000
[ERROR] [1529497455.937621922, 19.248000000]: Lookup would require extrapolation into the past.  Requested time 18.196000000 but the earliest data is at time 18.356000000, when looking up transform from frame [base_footprint] to frame [odom]
[ INFO] [1529497455.937735340, 19.248000000]: Odom tf timestamp: 0.000000

if, instead of stamp, I pass ros::Time(0) as argument to waitForTransform() and lookupTransform() I get no error but I see that the laser message stamp and the tf stamp have a not negligible difference:

dede@srrg-02:~$ rosrun lucrezio_semantic_mapper pose_broadcaster_node 
[ INFO] [1529497759.281287706, 321.114000000]: Laser msg timestamp: 321.107000
[ INFO] [1529497759.484725587, 321.317000000]: Odom tf timestamp: 321.316000
[ INFO] [1529497759.499681070, 321.331000000]: Laser msg timestamp: 321.308000
[ INFO] [1529497759.685763872, 321.516000000]: Odom tf timestamp: 321.516000

Please, can someone explain me what is happening and how to tackle this problem?

Thanks.