FIX Invalid argument ERROR migration from tf to tf2

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
+```

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;