Ask Your Question
0

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

asked 2018-06-22 09:56:19 -0500

schizzz8 gravatar image

updated 2018-06-25 05:54:18 -0500

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-06-25 06:33:11 -0500

gvdhoorn gravatar image

updated 2018-06-26 04:19:15 -0500

Wouldn't this be a job for gazebo_ros_p3d? See #q222033.


Edit:

No. This publishes the robot odometry. Which is different from the robot pose wrt the gazebo world.

The code seems to indicate otherwise. See here. The pose.position.{x, y, z} elements do not get calculated, but are initialised from the world pose of the configured link.

If you set the gaussianNoise parameter to 0, it should be absolute ground truth (ie: pose in Gazebo world). not even strictly necessary: noise is only added to linear and angular velocity estimates. But might be nice to do still if you want to use this as input to another node that takes covariance into account.

Other answers (such as #q258748) also seem to confirm/suggest that.


Edit2:

I agree with you that this plugin is publishing the world pose of the robot. Nevertheless, if I run it (inserting the lines specified in #q258748 in my urdf) I don't have a tf between /map and /odom frame which is what I really need.

Getting the odometry data out of Gazebo is only the first step. If you got that working, you should be able to use fake_localization to do the rest.

The base_pose_ground_truth subscription would get its data from the p3d plugin.

edit flag offensive delete link more

Comments

No. This publishes the robot odometry. Which is different from the robot pose wrt the gazebo world.

schizzz8 gravatar imageschizzz8 ( 2018-06-25 07:26:15 -0500 )edit

I agree with you that this plugin is publishing the world pose of the robot. Nevertheless, if I run it (inserting the lines specified in #q258748 in my urdf) I don't have a tf between /map and /odom frame which is what I really need.

schizzz8 gravatar imageschizzz8 ( 2018-06-26 03:15:02 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-06-22 09:56:19 -0500

Seen: 270 times

Last updated: Jun 26 '18