From 52d894eb3d322e7d86b380db6d8c4432a69ab8ab Mon Sep 17 00:00:00 2001
From: rocket42 <hung.rocket42@gmail.com>
Date: Wed, 8 Apr 2020 09:43:00 +0700
Subject: [PATCH] FIX Invalid argument ERROR WHEN MIGRATION TF TO TF2

- fixed error **Invalid argument passed to lookupTransform argument source_frame in tf2** which cased by **migration from tf to tf2**
- teb_local_planner_tutorials can run ok
- tested on ubuntu 18.04 LTS + ros-melodic
---
 readme.md        | 18 ++++++++++++++++++
 src/stageros.cpp |  8 ++++----
 2 files changed, 22 insertions(+), 4 deletions(-)
 create mode 100644 readme.md

diff --git a/readme.md b/readme.md
new file mode 100644
index 0000000..97acb2e
--- /dev/null
+++ b/readme.md
@@ -0,0 +1,18 @@
+# Introduction
+This Fork is to fix error **Invalid argument passed to lookupTransform argument source_frame in tf2** which cased by **migration from tf to tf2**
+
+This issues was described [here](https://github.com/rst-tu-dortmund/teb_local_planner_tutorials/issues/9)
+# Using
+- Create catkit workspace:
+```
+$ mkdir -p ~/catkin_ws/src
+$ cd ~/catkin_ws/
+$ catkin_make
+```
+- Build only stage_ros and it's dependencies:
+```
+$ cd ~/catkin_ws/src
+$ git clone https://github.com/AMRobots/stage_ros.git
+$ cd ../
+catkin_make --only-pkg-with-deps stage_ros
+```
diff --git a/src/stageros.cpp b/src/stageros.cpp
index 39d040a..b1c85b1 100644
--- a/src/stageros.cpp
+++ b/src/stageros.cpp
@@ -183,11 +183,11 @@ StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
 
         if ((found==std::string::npos) && umn)
         {
-            snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
+            snprintf(buf, sizeof(buf), "%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
         }
         else
         {
-            snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
+            snprintf(buf, sizeof(buf), "robot_%u/%s", (unsigned int)robotID, name);
         }
 
         return buf;
@@ -209,11 +209,11 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model
 
         if ((found==std::string::npos) && umn)
         {
-            snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
+            snprintf(buf, sizeof(buf), "%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
         }
         else
         {
-            snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
+            snprintf(buf, sizeof(buf), "robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
         }
 
         return buf;