diff --git a/gmapping/CMakeLists.txt b/gmapping/CMakeLists.txt index 097d0916..58a2b9f6 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 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) diff --git a/gmapping/package.xml b/gmapping/package.xml index 59939dfb..1219391d 100644 --- a/gmapping/package.xml +++ b/gmapping/package.xml @@ -15,17 +15,21 @@ catkin nav_msgs + nodelet openslam_gmapping roscpp rostest - tf - nodelet + tf2 + tf2_geometry_msgs + tf2_ros nav_msgs + nodelet openslam_gmapping roscpp - tf - nodelet + tf2 + tf2_geometry_msgs + tf2_ros diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp index bd9977c3..247cce49 100644 --- a/gmapping/src/slam_gmapping.cpp +++ b/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 +#include +#include #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("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_)); @@ -287,15 +307,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"); } } @@ -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,22 +385,22 @@ 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 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; } @@ -392,18 +408,20 @@ SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t 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 ident; - tf::Stamped 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()); @@ -411,16 +429,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()); @@ -428,10 +447,10 @@ 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; } @@ -439,20 +458,25 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) 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::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::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(); } diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h index ae622b95..cfc0c5c4 100644 --- a/gmapping/src/slam_gmapping.h +++ b/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 +#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" @@ -54,10 +56,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_; @@ -65,7 +68,7 @@ class SlamGMapping // symmetrical bounds as that's what gmapping expects) std::vector laser_angles_; // The pose, in the original laser frame, of the corresponding centered laser with z facing up - tf::Stamped 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_; diff --git a/gmapping/test/basic_localization_stage_replay.launch b/gmapping/test/basic_localization_stage_replay.launch index 83963805..19fa5a1b 100644 --- a/gmapping/test/basic_localization_stage_replay.launch +++ b/gmapping/test/basic_localization_stage_replay.launch @@ -4,5 +4,5 @@ - + diff --git a/gmapping/test/basic_localization_upside_down.launch b/gmapping/test/basic_localization_upside_down.launch index 052468f3..998678f2 100644 --- a/gmapping/test/basic_localization_upside_down.launch +++ b/gmapping/test/basic_localization_upside_down.launch @@ -1,5 +1,5 @@ - +