How to attach the exact timestamp to "/gazebo/link_states" msg?

edit

Hi all,

I made a simulated environment and robot in Gazebo. Since Gazebo publishes the robot pose w.r.t. the world, I'd like to use this information to "simulate" a localizer.

So far, this is what I did:

#include <iostream>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <gazebo_msgs/LinkStates.h>
#include <Eigen/Geometry>
#include <sensor_msgs/LaserScan.h>

class PoseBroadcaster{
public:
  PoseBroadcaster(ros::NodeHandle nh_):
    _nh(nh_){

    _camera_pose_sub = _nh.subscribe("/gazebo/link_states",
                                     1000,
                                     &PoseBroadcaster::cameraPoseCallback,
                                     this);
    _scan_sub = _nh.subscribe("/scan",
                              1000,
                              &PoseBroadcaster::scanCallback,
                              this);
  }

  void cameraPoseCallback(const gazebo_msgs::LinkStates::ConstPtr& camera_pose_msg){
    const std::vector<std::string> &names = camera_pose_msg->name;
    for(size_t i=0; i<names.size(); ++i){
      if(names[i].compare("robot::camera_link") == 0){
        const geometry_msgs::Pose camera_pose = camera_pose_msg->pose[i];
        _camera_transform=poseMsg2eigen(camera_pose);
        break;
      }
    }
  }

  void scanCallback(const sensor_msgs::LaserScan::ConstPtr &laser_msg){

    ros::Time stamp = laser_msg->header.stamp;

    tf::StampedTransform camera_tf;

    try{
      _listener.waitForTransform("/odom",
                                 "/camera_link",
                                 stamp,
                                 ros::Duration(0.5));
      _listener.lookupTransform("/odom",
                                "/camera_link",
                                stamp,
                                camera_tf);
    } catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
    }
    Eigen::Isometry3f camera_transform = tfTransform2eigen(camera_tf);

    tf::Transform odom_to_map_tf = eigen2tfTransform(_camera_transform*camera_transform.inverse());
    _br.sendTransform(tf::StampedTransform(odom_to_map_tf, stamp, "/map", "/odom"));
  }

  Eigen::Isometry3f tfTransform2eigen(const tf::Transform& p){
  ...
  }

  Eigen::Isometry3f poseMsg2eigen(const geometry_msgs::Pose &p){
  ...
  }

  tf::Transform eigen2tfTransform(const Eigen::Isometry3f& T){
  ...
  }

protected:
  ros::NodeHandle _nh;
  tf::TransformListener _listener;

  tf::TransformBroadcaster _br;

  ros::Subscriber _scan_sub;

  ros::Subscriber _camera_pose_sub;
  Eigen::Isometry3f _camera_transform;

};

int main(int argc, char **argv){

  ros::init(argc, argv, "pose_broadcaster_node");
  ros::NodeHandle nh;

  PoseBroadcaster dummy(nh);

  ros::spin();

  return 0;
}

In few words, what it does is to:

• listen to "/gazebo/link_states" and store the last pose

• subscribe to "/scan" topic

• when a scan is received, listen to the camera pose at that time istant and publish the odom->map transform to "close" the tree

The main problem is that there is a delay between the moment the callback is executed and the laser scan timestamp. So, what should happen is that the pose that I get from Gazebo is more new w.r.t. to camera_link->odom transform.

A better solution would be to use message_filters::Synchronizer, but since gazebo_msgs/LinkStates is not stamped, this is not possible.

My question is: is it possible (perhaps through a Gazebo plugin) to attach a timestamp to the gazebo_msgs/LinkStates message?

Of course also better solutions are welcome :)

Thanks.