Make diff drive plugin tf broadcasting a configurable option

* Allows turning off the tf broadcaster of base_link ("body" link)
  to odom in cases where the non-Flatland code provides this
  transform.
* Improved debugging output of FlatlandViz::addTool

docs/included_plugins/diff_drive.rst

@@ -21,10 +21,10 @@ velocities and odometries are w.r.t. the robot origin
   plugins:
 
       # required, specify DiffDrive type to load the plugin
-    - type: DiffDrive 
+    - type: DiffDrive
 
       # required, name of the plugin
-      name: turtlebot_drive 
+      name: turtlebot_drive
 
       # required, body of a model to set velocities and obtain odometry
       body: base
@@ -50,29 +50,34 @@ velocities and odometries are w.r.t. the robot origin
       # optional, defaults to "twist", the topic to publish noisy local frame velocity
       # that simulates encoder readings
       twist_pub: twist
-      
+
       # optional, defaults to true, enables the advertising and publishing of both
       # ground truth and noisy odometry
       enable_odom_pub: true
-      
+
+      # optional, defaults to true, enables the publishing of the TF between
+      # 'base_link' (or link specified in "body") and 'odom' links.
+      # Disable if your code has its own TF publisher for these two links.
+      enable_odom_tf_pub: true
+
       # optional, defaults to true, enables the advertising and publishing of noisy local
       # frame velocity
       enable_twist_pub: true
 
-      # optional, defaults to [0, 0, 0], corresponds to noise on [x, y, yaw], 
+      # optional, defaults to [0, 0, 0], corresponds to noise on [x, y, yaw],
       # the variances of gaussian noise to apply to the pose components of the
       # odometry message
       odom_pose_noise: [0, 0, 0]
 
-      # optional, defaults to [0, 0, 0], corresponds to noise on 
+      # optional, defaults to [0, 0, 0], corresponds to noise on
       # [x velocity, y velocity, yaw rate], the variances of gaussian noise to
       # apply to the twist components of the odometry message
       odom_twist_noise: [0, 0, 0]
 
-      # optional, defaults to the diagonal [x, y, yaw] components replaced by 
-      # odom_pose_noise with all other values equals zero, must have length of 36, 
-      # represents a 6x6 covariance matrix for x, y, z, roll, pitch, yaw. 
-      # This does not involve in any of the noise calculation, it is simply 
+      # optional, defaults to the diagonal [x, y, yaw] components replaced by
+      # odom_pose_noise with all other values equals zero, must have length of 36,
+      # represents a 6x6 covariance matrix for x, y, z, roll, pitch, yaw.
+      # This does not involve in any of the noise calculation, it is simply
       # the output values of odometry pose covariance
       odom_pose_covariance: [0, 0, 0, 0, 0, 0,
                              0, 0, 0, 0, 0, 0,
@@ -81,10 +86,10 @@ velocities and odometries are w.r.t. the robot origin
                              0, 0, 0, 0, 0, 0,
                              0, 0, 0, 0, 0, 0]
 
-      # optional, defaults to the diagonal [x velocity, y velocity, yaw rate] 
+      # optional, defaults to the diagonal [x velocity, y velocity, yaw rate]
       # components replaced by odom_twist_noise with all other values equals zero,
-      # must have length of 36, represents a 6x6 covariance matrix for rates x, 
-      # y, z, roll, pitch, yaw. This does not involve in any of the noise 
+      # must have length of 36, represents a 6x6 covariance matrix for rates x,
+      # y, z, roll, pitch, yaw. This does not involve in any of the noise
       # calculation, it is simply the output values of odometry twist covariance
       odom_twist_covariance: [0, 0, 0, 0, 0, 0
                              0, 0, 0, 0, 0, 0

flatland_plugins/include/flatland_plugins/diff_drive.h

@@ -73,8 +73,9 @@ class DiffDrive : public flatland_server::ModelPlugin {
   nav_msgs::Odometry ground_truth_msg_;
   UpdateTimer update_timer_;
   tf::TransformBroadcaster tf_broadcaster;  ///< For publish ROS TF
-  bool enable_odom_pub_;   ///< YAML parameter to enable odom publishing
-  bool enable_twist_pub_;  ///< YAML parameter to enable twist publishing
+  bool enable_odom_pub_;            ///< YAML parameter to enable odom publishing
+  bool enable_odom_tf_pub_;         ///< YAML parameter to enable odom tf publishing
+  bool enable_twist_pub_;           ///< YAML parameter to enable twist publishing
   DynamicsLimits angular_dynamics_; ///< Angular dynamics constraints
   DynamicsLimits linear_dynamics_;  ///< Linear dynamics constraints
   double angular_velocity_ = 0.0;

flatland_plugins/src/diff_drive.cpp

@@ -62,6 +62,7 @@ void DiffDrive::TwistCallback(const geometry_msgs::Twist& msg) {
 void DiffDrive::OnInitialize(const YAML::Node& config) {
   YamlReader reader(config);
   enable_odom_pub_ = reader.Get<bool>("enable_odom_pub", true);
+  enable_odom_tf_pub_ = reader.Get<bool>("enable_odom_tf_pub", true);
   enable_twist_pub_ = reader.Get<bool>("enable_twist_pub", true);
   std::string body_name = reader.Get<std::string>("body");
   std::string odom_frame_id = reader.Get<std::string>("odom_frame_id", "odom");
@@ -260,15 +261,17 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
       twist_pub_.publish(twist_pub_msg);
     }
 
-    // publish odom tf
-    geometry_msgs::TransformStamped odom_tf;
-    odom_tf.header = odom_msg_.header;
-    odom_tf.child_frame_id = odom_msg_.child_frame_id;
-    odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x;
-    odom_tf.transform.translation.y = odom_msg_.pose.pose.position.y;
-    odom_tf.transform.translation.z = 0;
-    odom_tf.transform.rotation = odom_msg_.pose.pose.orientation;
-    tf_broadcaster.sendTransform(odom_tf);
+    if (enable_odom_tf_pub_) {
+      // publish odom tf
+      geometry_msgs::TransformStamped odom_tf;
+      odom_tf.header = odom_msg_.header;
+      odom_tf.child_frame_id = odom_msg_.child_frame_id;
+      odom_tf.transform.translation.x = odom_msg_.pose.pose.position.x;
+      odom_tf.transform.translation.y = odom_msg_.pose.pose.position.y;
+      odom_tf.transform.translation.z = 0;
+      odom_tf.transform.rotation = odom_msg_.pose.pose.orientation;
+      tf_broadcaster.sendTransform(odom_tf);
+    }
   }
 
 }

flatland_viz/src/flatland_viz.cpp

@@ -179,7 +179,7 @@ void FlatlandViz::setDisplayConfigModified() {
 }
 
 void FlatlandViz::addTool(rviz::Tool* tool) {
-  ROS_ERROR("addTool called");
+  ROS_ERROR("addTool called: %s", tool->getName().toStdString().c_str());
   QAction* action = new QAction(tool->getName(), toolbar_actions_);
   action->setIcon(tool->getIcon());
   action->setIconText(tool->getName());