From 3b4325fa6b8bd279b990e7b052a70230874395ef Mon Sep 17 00:00:00 2001 From: "Eurico F. Pedrosa" Date: Fri, 19 Nov 2021 11:45:25 +0000 Subject: [PATCH] [loc2d] Add option to disable map->odom tf (closes #37). --- README.md | 1 + include/lama/ros/loc2d_ros.h | 11 +++++---- src/loc2d_ros.cpp | 46 ++++++++++++++++++++---------------- 3 files changed, 32 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index aaf094a..34b5547 100644 --- a/README.md +++ b/README.md @@ -117,6 +117,7 @@ Please use `rviz` to set the initial pose. Global localization is not yet implem * `~scan_topic`: Laser scan topic to subscribe (default: "/scan"). * `~mrange`: Maximum laser scan range. When 0, maximum range defaults to the sensor maximum range. (default: 0 meters). * `~transform_tolerance`: Defines how long map->odom transform is good for by [future dating tf](https://answers.ros.org/question/218864/why-does-amcl-post-date-tf-transform_tolerance/) (default: 0.1) +* `~publish_tf`: Publish TF correction from `global_frame_id` to `odom_frame_id` (default: true). * `~beam_step`: Number of beams to step (or skip) in each scan (default: 1). * `~initial_pos_x`: Initial x position (default: 0 meters). * `~initial_pos_y`: Initial y position (default: 0 meters). diff --git a/include/lama/ros/loc2d_ros.h b/include/lama/ros/loc2d_ros.h index 3bcb5de..e908b29 100644 --- a/include/lama/ros/loc2d_ros.h +++ b/include/lama/ros/loc2d_ros.h @@ -131,11 +131,12 @@ class Loc2DROS { std::string scan_topic_; ///< LaserScan message topic. - bool use_map_topic_; ///< True to subscribe to the map topic instead of requesting the map through the "static_map" service - bool first_map_only_; ///< True to use only the first map ever received - bool first_map_received_; ///< True if the first map has already been received - bool use_pose_on_new_map_; ///< True to use the current algorithm pose when the map changes - bool force_update_; ///< True to force an update when a new laser scan is received + bool publish_tf_; ///< True to publish the transformations. + bool use_map_topic_; ///< True to subscribe to the map topic instead of requesting the map through the "static_map" service + bool first_map_only_; ///< True to use only the first map ever received + bool first_map_received_; ///< True if the first map has already been received + bool use_pose_on_new_map_; ///< True to use the current algorithm pose when the map changes + bool force_update_; ///< True to force an update when a new laser scan is received bool force_update_on_initial_pose_; ///< True to trigger a forced updated when an initial pose is received // == Inner state == diff --git a/src/loc2d_ros.cpp b/src/loc2d_ros.cpp index 0541226..c459775 100644 --- a/src/loc2d_ros.cpp +++ b/src/loc2d_ros.cpp @@ -52,6 +52,7 @@ lama::Loc2DROS::Loc2DROS() pnh_.param("transform_tolerance", tmp, 0.1); transform_tolerance_.fromSec(tmp); + pnh_.param("publish_tf", publish_tf_, true); pnh_.param("use_map_topic", use_map_topic_, false); pnh_.param("first_map_only", first_map_only_, false); pnh_.param("use_pose_on_new_map", use_pose_on_new_map_, false); @@ -315,30 +316,33 @@ void lama::Loc2DROS::onLaserScan(const sensor_msgs::LaserScanConstPtr& laser_sca } current_orientation_ = tf::createQuaternionFromYaw(loc2d_.getPose().rotation()); - // subtracting base to odom from map to base and send map to odom instead - tf::Stamped odom_to_map; - try{ - tf::Transform tmp_tf(current_orientation_, tf::Vector3(loc2d_.getPose().x(), loc2d_.getPose().y(), 0)); - tf::Stamped tmp_tf_stamped (tmp_tf.inverse(), laser_scan->header.stamp, base_frame_id_); - tf_->transformPose(odom_frame_id_, tmp_tf_stamped, odom_to_map); - - }catch(tf::TransformException){ - ROS_WARN("Failed to subtract base to odom transform"); - return; - } - latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), - tf::Point(odom_to_map.getOrigin())); + if (publish_tf_){ + // subtracting base to odom from map to base and send map to odom instead + tf::Stamped odom_to_map; + try{ + tf::Transform tmp_tf(current_orientation_, tf::Vector3(loc2d_.getPose().x(), loc2d_.getPose().y(), 0)); + tf::Stamped tmp_tf_stamped (tmp_tf.inverse(), laser_scan->header.stamp, base_frame_id_); + tf_->transformPose(odom_frame_id_, tmp_tf_stamped, odom_to_map); + + }catch(tf::TransformException){ + ROS_WARN("Failed to subtract base to odom transform"); + return; + } + + latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), + tf::Point(odom_to_map.getOrigin())); + + // We want to send a transform that is good up until a + // tolerance time so that odom can be used + ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_); + tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), transform_expiration, + global_frame_id_, odom_frame_id_); + tfb_->sendTransform(tmp_tf_stamped); + } - // We want to send a transform that is good up until a - // tolerance time so that odom can be used - ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_); - tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), - transform_expiration, - global_frame_id_, odom_frame_id_); - tfb_->sendTransform(tmp_tf_stamped); publishCurrentPose(); - } else { + } else if (publish_tf_) { // Nothing has change, therefore, republish the last transform. ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_); tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), transform_expiration,