switch to tf2

gmapping/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8)
 project(gmapping)
 
-find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf rosbag_storage)
+find_package(catkin REQUIRED nav_msgs nodelet openslam_gmapping roscpp tf2 tf2_geometry_msgs tf2_ros rosbag_storage)
 
 find_package(Boost REQUIRED signals)
 

gmapping/package.xml

@@ -15,17 +15,21 @@
  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
  <build_depend>nav_msgs</build_depend>
+ <build_depend>nodelet</build_depend>
  <build_depend>openslam_gmapping</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rostest</build_depend>
- <build_depend>tf</build_depend>
- <build_depend>nodelet</build_depend>
+ <build_depend>tf2</build_depend>
+ <build_depend>tf2_geometry_msgs</build_depend>
+ <build_depend>tf2_ros</build_depend>
 
  <run_depend>nav_msgs</run_depend>
+ <run_depend>nodelet</run_depend>
  <run_depend>openslam_gmapping</run_depend>
  <run_depend>roscpp</run_depend>
- <run_depend>tf</run_depend>
- <run_depend>nodelet</run_depend>
+ <run_depend>tf2</run_depend>
+ <run_depend>tf2_geometry_msgs</run_depend>
+ <run_depend>tf2_ros</run_depend>
 
  <export>
  <nodelet plugin="${prefix}/nodelet_plugins.xml" />

gmapping/src/slam_gmapping.cpp

@@ -114,6 +114,9 @@ Initial map dimensions and resolution:
 #include "ros/ros.h"
 #include "ros/console.h"
 #include "nav_msgs/MapMetaData.h"
+#include <tf2/utils.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_msgs/TFMessage.h>
 
 #include "gmapping/sensor/sensor_range/rangesensor.h"
 #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
@@ -126,27 +129,41 @@ Initial map dimensions and resolution:
 // compute linear index for given map coords
 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
 
+/* This function is only useful to have the whole code work
+ * with old rosbags that have trailing slashes for their frames
+ */
+inline
+std::string stripSlash(const std::string& in)
+{
+ std::string out = in;
+ if ( ( !in.empty() ) && (in[0] == '/') )
+ out.erase(0,1);
+ return out;
+}
+
 SlamGMapping::SlamGMapping():
- map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
  laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
 {
+ tfL_.reset(new tf2_ros::TransformListener(tf_));
+ map_to_odom_.setIdentity();
  seed_ = time(NULL);
  init();
 }
 
 SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
- map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
  laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
 {
+ tfL_.reset(new tf2_ros::TransformListener(tf_));
+ map_to_odom_.setIdentity();
  seed_ = time(NULL);
  init();
 }
 
 SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
- map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
  laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL),
- seed_(seed), tf_(ros::Duration(max_duration_buffer))
+ seed_(seed), tf_(ros::Duration(max_duration_buffer)), tfL_(new tf2_ros::TransformListener(tf_))
 {
+ map_to_odom_.setIdentity();
  init();
 }
 
@@ -160,7 +177,7 @@ void SlamGMapping::init()
  gsp_ = new GMapping::GridSlamProcessor();
  ROS_ASSERT(gsp_);
 
- tfB_ = new tf::TransformBroadcaster();
+ tfB_ = new tf2_ros::TransformBroadcaster();
  ROS_ASSERT(tfB_);
 
  gsp_laser_ = NULL;
@@ -176,10 +193,13 @@ void SlamGMapping::init()
  throttle_scans_ = 1;
  if(!private_nh_.getParam("base_frame", base_frame_))
  base_frame_ = "base_link";
+ base_frame_ = stripSlash(base_frame_);
  if(!private_nh_.getParam("map_frame", map_frame_))
  map_frame_ = "map";
+ map_frame_ = stripSlash(map_frame_);
  if(!private_nh_.getParam("odom_frame", odom_frame_))
  odom_frame_ = "odom";
+ odom_frame_ = stripSlash(odom_frame_);
 
  private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
 
@@ -260,7 +280,7 @@ void SlamGMapping::startLiveSlam()
  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
- scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
+ scan_filter_ = new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5, node_);
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
 
  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
@@ -287,15 +307,11 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
  std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
  foreach(rosbag::MessageInstance const m, viewall)
  {
- tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
+ tf2_msgs::TFMessage::ConstPtr cur_tf = m.instantiate<tf2_msgs::TFMessage>();
  if (cur_tf != NULL) {
  for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
  {
- geometry_msgs::TransformStamped transformStamped;
- tf::StampedTransform stampedTf;
- transformStamped = cur_tf->transforms[i];
- tf::transformStampedMsgToTF(transformStamped, stampedTf);
- tf_.setTransform(stampedTf);
+ tf_.setTransform(cur_tf->transforms[i], "slam_gmapping");
  }
  }
 
@@ -318,8 +334,8 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
  {
  try
  {
- tf::StampedTransform t;
- tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
+ tf_.lookupTransform(stripSlash(s_queue.front().first->header.frame_id),
+  odom_frame_, s_queue.front().first->header.stamp);
  this->laserCallback(s_queue.front().first);
  s_queue.pop();
  }
@@ -369,90 +385,98 @@ bool
 SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
 {
  // Get the pose of the centered laser at the right time
- centered_laser_pose_.stamp_ = t;
+ centered_laser_pose_.header.stamp = t;
  // Get the laser's pose that is centered
- tf::Stamped<tf::Transform> odom_pose;
+ geometry_msgs::PoseStamped odom_pose;
  try
  {
- tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
+ tf_.transform(centered_laser_pose_, odom_pose, odom_frame_);
  }
- catch(tf::TransformException e)
+ catch(tf2::TransformException e)
  {
  ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
  return false;
  }
- double yaw = tf::getYaw(odom_pose.getRotation());
+ double yaw = tf2::getYaw(odom_pose.pose.orientation);
 
- gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
- odom_pose.getOrigin().y(),
+ gmap_pose = GMapping::OrientedPoint(odom_pose.pose.position.x,
+ odom_pose.pose.position.y,
  yaw);
  return true;
 }
 
 bool
 SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
 {
- laser_frame_ = scan.header.frame_id;
+ laser_frame_ = stripSlash(scan.header.frame_id);
  // Get the laser's pose, relative to base.
- tf::Stamped<tf::Pose> ident;
- tf::Stamped<tf::Transform> laser_pose;
- ident.setIdentity();
- ident.frame_id_ = laser_frame_;
- ident.stamp_ = scan.header.stamp;
+ geometry_msgs::PoseStamped laser_pose;
  try
  {
- tf_.transformPose(base_frame_, ident, laser_pose);
+ geometry_msgs::PoseStamped ident;
+ ident.header.stamp = scan.header.stamp;
+ ident.header.frame_id = laser_frame_;
+ tf2::Transform transform;
+ transform.setIdentity();
+ tf2::toMsg(transform, ident.pose);
+ tf_.transform(ident, laser_pose, base_frame_);
  }
- catch(tf::TransformException e)
+ catch(tf2::TransformException e)
  {
  ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
  e.what());
  return false;
  }
 
  // create a point 1m above the laser position and transform it into the laser-frame
- tf::Vector3 v;
- v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
- tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
- base_frame_);
+ geometry_msgs::PointStamped up;
+ up.header.stamp = scan.header.stamp;
+ up.header.frame_id = base_frame_;
+ up.point.x = up.point.y = 0;
+ up.point.z = 1 + laser_pose.pose.position.z;
  try
  {
- tf_.transformPoint(laser_frame_, up, up);
- ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
+ tf_.transform(up, up, laser_frame_);
+ ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.point.z);
  }
- catch(tf::TransformException& e)
+ catch(tf2::TransformException& e)
  {
  ROS_WARN("Unable to determine orientation of laser: %s",
  e.what());
  return false;
  }
 
  // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
- if (fabs(fabs(up.z()) - 1) > 0.001)
+ if (fabs(fabs(up.point.z) - 1) > 0.001)
  {
  ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
- up.z());
+ up.point.z);
  return false;
  }
 
  gsp_laser_beam_count_ = scan.ranges.size();
 
  double angle_center = (scan.angle_min + scan.angle_max)/2;
 
- if (up.z() > 0)
+ centered_laser_pose_.header.frame_id = laser_frame_;
+ centered_laser_pose_.header.stamp = ros::Time::now();
+ tf2::Quaternion q;
+ if (up.point.z > 0)
  {
  do_reverse_range_ = scan.angle_min > scan.angle_max;
- centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
- tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
+ q.setEuler(angle_center, 0, 0);
  ROS_INFO("Laser is mounted upwards.");
  }
  else
  {
  do_reverse_range_ = scan.angle_min < scan.angle_max;
- centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center),
- tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
+ q.setEuler(-angle_center, 0, M_PI);
  ROS_INFO("Laser is mounted upside down.");
  }
+ centered_laser_pose_.pose.position.x = 0;
+ centered_laser_pose_.pose.position.y = 0;
+ centered_laser_pose_.pose.position.z = 0;
+ convert(q, centered_laser_pose_.pose.orientation);
 
  // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
  laser_angles_.resize(scan.ranges.size());
@@ -621,8 +645,11 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
  ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
  ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
 
- tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
- tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
+ tf2::Quaternion q;
+ q.setRPY(0, 0, mpose.theta);
+ tf2::Transform laser_to_map = tf2::Transform(q, tf2::Vector3(mpose.x, mpose.y, 0.0)).inverse();
+ q.setRPY(0, 0, odom_pose.theta);
+ tf2::Transform odom_to_laser = tf2::Transform(q, tf2::Vector3(odom_pose.x, odom_pose.y, 0.0));
 
  map_to_odom_mutex_.lock();
  map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
@@ -762,7 +789,7 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
 
  //make sure to set the header information on the map
  map_.map.header.stamp = ros::Time::now();
- map_.map.header.frame_id = tf_.resolve( map_frame_ );
+ map_.map.header.frame_id = map_frame_;
 
  sst_.publish(map_.map);
  sstm_.publish(map_.map.info);
@@ -786,6 +813,12 @@ void SlamGMapping::publishTransform()
 {
  map_to_odom_mutex_.lock();
  ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
- tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
+ geometry_msgs::TransformStamped transform;
+ transform.header.frame_id = map_frame_;
+ transform.header.stamp = tf_expiration;
+ transform.child_frame_id = odom_frame_;
+ transform.transform = tf2::toMsg(map_to_odom_);
+
+ tfB_->sendTransform( transform );
  map_to_odom_mutex_.unlock();
 }

gmapping/src/slam_gmapping.h

@@ -20,10 +20,12 @@
 #include "sensor_msgs/LaserScan.h"
 #include "std_msgs/Float64.h"
 #include "nav_msgs/GetMap.h"
-#include "tf/transform_listener.h"
-#include "tf/transform_broadcaster.h"
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include "message_filters/subscriber.h"
-#include "tf/message_filter.h"
+#include <tf2_ros/message_filter.h>
 
 #include "gmapping/gridfastslam/gridslamprocessor.h"
 #include "gmapping/sensor/sensor_base/sensor.h"
@@ -54,18 +56,19 @@ class SlamGMapping
  ros::Publisher sst_;
  ros::Publisher sstm_;
  ros::ServiceServer ss_;
- tf::TransformListener tf_;
+ tf2_ros::Buffer tf_;
+ boost::shared_ptr<tf2_ros::TransformListener> tfL_;
  message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
- tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
- tf::TransformBroadcaster* tfB_;
+ tf2_ros::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
+ tf2_ros::TransformBroadcaster* tfB_;
 
  GMapping::GridSlamProcessor* gsp_;
  GMapping::RangeSensor* gsp_laser_;
  // The angles in the laser, going from -x to x (adjustment is made to get the laser between
  // symmetrical bounds as that's what gmapping expects)
  std::vector<double> laser_angles_;
  // The pose, in the original laser frame, of the corresponding centered laser with z facing up
- tf::Stamped<tf::Pose> centered_laser_pose_;
+ geometry_msgs::PoseStamped centered_laser_pose_;
  // Depending on the order of the elements in the scan and the orientation of the scan frame,
  // We might need to change the order of the scan
  bool do_reverse_range_;
@@ -78,7 +81,7 @@ class SlamGMapping
  nav_msgs::GetMap::Response map_;
 
  ros::Duration map_update_interval_;
- tf::Transform map_to_odom_;
+ tf2::Transform map_to_odom_;
  boost::mutex map_to_odom_mutex_;
  boost::mutex map_mutex_;
 

gmapping/src/tftest.cpp

@@ -1,16 +1,16 @@
 #include <cstdio>
 #include "sensor_msgs/LaserScan.h"
 #include "ros/node.h"
-#include "tf/transform_listener.h"
+#include <tf2_ros/transform_listener.h>
 
 class Test
 {
  public:
  Test()
  {
  node_ = new ros::Node("test");
- tf_ = new tf::TransformListener(*node_, true, 
-  ros::Duration(10));
+ tf_.reset(new tf2::Buffer(ros::Duration(10)));
+ tfl_ = new tf2_ros::TransformListener(*tf_, *node_, true);
  tf_->setExtrapolationLimit( ros::Duration().fromSec(0.2));
 
  node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1);
@@ -52,7 +52,8 @@ class Test
 
  private:
  ros::Node* node_;
- tf::TransformListener* tf_;
+ tf::TransformListener* tfl_;
+ boost::shared_ptr<tf2_ros::Buffer> tf_;
  sensor_msgs::LaserScan scan_;
 };