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.
Originally posted by schizzz8 on ROS Answers with karma: 183 on 2018-06-22
Post score: 0
