From 05df138fbe8a998995ebfb32cfda9d337a9b3db9 Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Sat, 25 Apr 2015 23:33:17 +0200 Subject: [PATCH] switch to tf2 --- gmapping/CMakeLists.txt | 2 +- gmapping/package.xml | 8 ++- gmapping/src/slam_gmapping.cpp | 103 +++++++++++++++++++-------------- gmapping/src/slam_gmapping.h | 16 ++--- gmapping/src/tftest.cpp | 9 +-- 5 files changed, 80 insertions(+), 58 deletions(-) diff --git a/gmapping/CMakeLists.txt b/gmapping/CMakeLists.txt index ef19b698..32ff0352 100644 --- a/gmapping/CMakeLists.txt +++ b/gmapping/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8) project(gmapping) -find_package(catkin REQUIRED nav_msgs openslam_gmapping roscpp rostest tf rosbag_storage) +find_package(catkin REQUIRED nav_msgs openslam_gmapping roscpp rostest tf2 tf2_geometry_msgs tf2_ros rosbag_storage) find_package(Boost REQUIRED signals) diff --git a/gmapping/package.xml b/gmapping/package.xml index 87faa04d..8b86eeb9 100644 --- a/gmapping/package.xml +++ b/gmapping/package.xml @@ -18,10 +18,14 @@ openslam_gmapping roscpp rostest - tf + tf2 + tf2_geometry_msgs + tf2_ros nav_msgs openslam_gmapping roscpp - tf + tf2 + tf2_geometry_msgs + tf2_ros diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp index c6b89388..1931b69a 100644 --- a/gmapping/src/slam_gmapping.cpp +++ b/gmapping/src/slam_gmapping.cpp @@ -115,6 +115,9 @@ Initial map dimensions and resolution: #include "ros/ros.h" #include "ros/console.h" #include "nav_msgs/MapMetaData.h" +#include +#include +#include #include "gmapping/sensor/sensor_range/rangesensor.h" #include "gmapping/sensor/sensor_odometry/odometrysensor.h" @@ -128,18 +131,19 @@ Initial map dimensions and resolution: #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 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(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(); } @@ -153,7 +157,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; @@ -254,7 +258,7 @@ void SlamGMapping::startLiveSlam() sstm_ = node_.advertise("map_metadata", 1, true); ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this); scan_filter_sub_ = new message_filters::Subscriber(node_, "scan", 5); - scan_filter_ = new tf::MessageFilter(*scan_filter_sub_, tf_, odom_frame_, 5); + scan_filter_ = new tf2_ros::MessageFilter(*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_)); @@ -280,15 +284,11 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t std::queue s_queue; foreach(rosbag::MessageInstance const m, viewall) { - tf::tfMessage::ConstPtr cur_tf = m.instantiate(); + tf2_msgs::TFMessage::ConstPtr cur_tf = m.instantiate(); 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"); } } @@ -309,18 +309,19 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t { try { - tf::StampedTransform t; - tf_.lookupTransform(s_queue.front()->header.frame_id, odom_frame_, s_queue.front()->header.stamp, t); + tf_.lookupTransform(s_queue.front()->header.frame_id, odom_frame_, s_queue.front()->header.stamp); this->laserCallback(s_queue.front()); s_queue.pop(); } // If tf does not have the data yet - catch(tf::ExtrapolationException& e) + catch(tf2::ExtrapolationException& e) { ROS_WARN("Wait for TF data: %s", e.what()); break; } - catch(tf::LookupException& e) + // Try again next time if tf does not have the data for the laser frame yet + // (should only happen at startup) + catch(tf2::LookupException& e) { ROS_WARN("TF data incomplete, waiting until data is extracted from the bag (%s)", e.what()); break; @@ -364,22 +365,25 @@ bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t) { // Get the laser's pose - tf::Stamped ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0), - tf::Vector3(0,0,0)), t, laser_frame_); - tf::Stamped odom_pose; + geometry_msgs::PoseStamped ident, odom_pose; + ident.header.stamp = t; + ident.header.frame_id = laser_frame_; + tf2::Transform transform; + transform.setIdentity(); + tf2::toMsg(transform, ident.pose); try { - tf_.transformPose(odom_frame_, ident, odom_pose); + tf_.transform(ident, 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; } @@ -389,16 +393,17 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) { laser_frame_ = scan.header.frame_id; // Get the laser's pose, relative to base. - tf::Stamped ident; - tf::Stamped laser_pose; - ident.setIdentity(); - ident.frame_id_ = laser_frame_; - ident.stamp_ = scan.header.stamp; + geometry_msgs::PoseStamped ident, laser_pose; + ident.header.stamp = scan.header.stamp; + ident.header.frame_id = laser_frame_; + tf2::Transform transform; + transform.setIdentity(); + tf2::toMsg(transform, ident.pose); try { - tf_.transformPose(base_frame_, ident, laser_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()); @@ -413,16 +418,17 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) } // 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 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()); @@ -430,17 +436,17 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) } // 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(); int orientationFactor; - if (up.z() > 0) + if (up.point.z > 0) { orientationFactor = 1; ROS_INFO("Laser is mounted upwards."); @@ -606,8 +612,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(); @@ -756,7 +765,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); @@ -780,6 +789,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(); } diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h index 7e1ec250..ca392f3e 100644 --- a/gmapping/src/slam_gmapping.h +++ b/gmapping/src/slam_gmapping.h @@ -20,10 +20,11 @@ #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 +#include +#include #include "message_filters/subscriber.h" -#include "tf/message_filter.h" +#include #include "gmapping/gridfastslam/gridslamprocessor.h" #include "gmapping/sensor/sensor_base/sensor.h" @@ -53,10 +54,11 @@ class SlamGMapping ros::Publisher sst_; ros::Publisher sstm_; ros::ServiceServer ss_; - tf::TransformListener tf_; + tf2_ros::Buffer tf_; + boost::shared_ptr tfL_; message_filters::Subscriber* scan_filter_sub_; - tf::MessageFilter* scan_filter_; - tf::TransformBroadcaster* tfB_; + tf2_ros::MessageFilter* scan_filter_; + tf2_ros::TransformBroadcaster* tfB_; GMapping::GridSlamProcessor* gsp_; GMapping::RangeSensor* gsp_laser_; @@ -72,7 +74,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_; diff --git a/gmapping/src/tftest.cpp b/gmapping/src/tftest.cpp index 965c10ef..747e4e90 100644 --- a/gmapping/src/tftest.cpp +++ b/gmapping/src/tftest.cpp @@ -1,7 +1,7 @@ #include #include "sensor_msgs/LaserScan.h" #include "ros/node.h" -#include "tf/transform_listener.h" +#include class Test { @@ -9,8 +9,8 @@ class Test 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 tf_; sensor_msgs::LaserScan scan_; };