[loc2d] Add option to disable map->odom tf (closes #37).

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).

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 ==

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<tf::Pose> odom_to_map;
-        try{
-            tf::Transform tmp_tf(current_orientation_, tf::Vector3(loc2d_.getPose().x(), loc2d_.getPose().y(), 0));
-            tf::Stamped<tf::Pose> 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<tf::Pose> odom_to_map;
+            try{
+                tf::Transform tmp_tf(current_orientation_, tf::Vector3(loc2d_.getPose().x(), loc2d_.getPose().y(), 0));
+                tf::Stamped<tf::Pose> 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,