Fix resolution of frames with tf_prefix

Author: rhaschke

src/rviz/default_plugin/effort_display.cpp

@@ -7,6 +7,7 @@
 #include <rviz/properties/int_property.h>
 #include <rviz/frame_manager.h>
 #include <rviz/validate_floats.h>
+#include <rviz/helpers/tf_prefix.h>
 
 #include "effort_visual.h"
 #include "effort_display.h"
@@ -278,8 +279,7 @@ void EffortDisplay::processMessage(const sensor_msgs::JointState::ConstPtr& msg)
     int joint_type = joint->type;
     if (joint_type == urdf::Joint::REVOLUTE)
     {
-      std::string tf_prefix = tf_prefix_property_->getStdString();
-      std::string tf_frame_id = (tf_prefix.empty() ? "" : tf_prefix + "/") + joint->child_link_name;
+      std::string tf_frame_id = concat(tf_prefix_property_->getStdString(), joint->child_link_name);
       Ogre::Quaternion orientation;
       Ogre::Vector3 position;
 

src/rviz/helpers/tf_prefix.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2020, Bielefeld University
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Bielefeld University nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#pragma once
+#include <string>
+
+namespace rviz
+{
+
+std::string concat(const std::string& prefix, const std::string& frame)
+{
+  if (prefix.empty())
+    return frame;
+
+  std::string composite = prefix;
+  composite.append("/");
+  composite.append(frame);
+  return composite;
+}
+
+} // namespace rviz

src/rviz/robot/tf_link_updater.cpp

@@ -29,8 +29,7 @@
 
 #include "tf_link_updater.h"
 #include <rviz/frame_manager.h>
-
-#include <tf/tf.h>
+#include <rviz/helpers/tf_prefix.h>
 
 #include <OgreVector3.h>
 #include <OgreQuaternion.h>
@@ -48,11 +47,7 @@ TFLinkUpdater::TFLinkUpdater(FrameManager* frame_manager, const StatusCallback&
 bool TFLinkUpdater::getLinkTransforms(const std::string& _link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation,
                                       Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const
 {
-  std::string link_name = _link_name;
-  if (!tf_prefix_.empty())
-  {
-    link_name = tf::resolve(tf_prefix_, link_name);
-  }
+  std::string link_name = concat(tf_prefix_, _link_name);
 
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;