From 6ff26263102bfe478afcc47b15fc87fbbb1af3fb Mon Sep 17 00:00:00 2001
From: Shohei Fujii <fujii.shohei@gmail.com>
Date: Tue, 20 Dec 2016 13:29:46 +0900
Subject: [PATCH] add param to use robot_description tf

---
 src/stageros.cpp | 48 ++++++++++++++++++++++++++++--------------------
 1 file changed, 28 insertions(+), 20 deletions(-)

diff --git a/src/stageros.cpp b/src/stageros.cpp
index b6a5869..71fe485 100644
--- a/src/stageros.cpp
+++ b/src/stageros.cpp
@@ -108,6 +108,7 @@ class StageNode
     
     bool isDepthCanonical;
     bool use_model_names;
+    bool use_robot_description_tf;
 
     // A helper function that is executed for each stage model.  We use it
     // to search for models of interest.
@@ -280,6 +281,7 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us
     if(!localn.getParam("is_depth_canonical", isDepthCanonical))
         isDepthCanonical = true;
 
+    localn.param("use_robot_description_tf", use_robot_description_tf, false);
 
     // We'll check the existence of the world file, because libstage doesn't
     // expose its failure to open it.  Could go further with checks (e.g., is
@@ -476,21 +478,25 @@ StageNode::WorldCallback()
             laserQ.setRPY(0.0, 0.0, lp.a);
             tf::Transform txLaser =  tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z));
 
-            if (robotmodel->lasermodels.size() > 1)
-                tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
-                                                      mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
-                                                      mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
-            else
-                tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
-                                                      mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
-                                                      mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+            if (!this->use_robot_description_tf) {
+                if (robotmodel->lasermodels.size() > 1)
+                    tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
+                                                          mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
+                                                          mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+                else
+                    tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
+                                                          mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
+                                                          mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+            }
         }
 
         //the position of the robot
-        tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
-                                              sim_time,
-                                              mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
-                                              mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+        if (!this->use_robot_description_tf) {
+            tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
+                                                  sim_time,
+                                                  mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
+                                                  mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+        }
 
         // Get latest odometry data
         // Translate into ROS message format and publish
@@ -669,14 +675,16 @@ StageNode::WorldCallback()
 
                 tf::Transform tr =  tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z));
 
-                if (robotmodel->cameramodels.size() > 1)
-                    tf.sendTransform(tf::StampedTransform(tr, sim_time,
-                                                          mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
-                                                          mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
-                else
-                    tf.sendTransform(tf::StampedTransform(tr, sim_time,
-                                                          mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
-                                                          mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+                if (!this->use_robot_description_tf) {
+                    if (robotmodel->cameramodels.size() > 1)
+                        tf.sendTransform(tf::StampedTransform(tr, sim_time,
+                                                              mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
+                                                              mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+                    else
+                        tf.sendTransform(tf::StampedTransform(tr, sim_time,
+                                                              mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
+                                                              mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
+                }
 
                 sensor_msgs::CameraInfo camera_msg;
                 if (robotmodel->cameramodels.size() > 1)