Reimplement tf_prefix in Noetic exactly as it was in Melodic

CMakeLists.txt

@@ -69,13 +69,16 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest_gtest(test_two_links_moving_joint ${CMAKE_CURRENT_SOURCE_DIR}/test/test_two_links_moving_joint.launch test/test_two_links_moving_joint.cpp)
   target_link_libraries(test_two_links_moving_joint ${catkin_LIBRARIES} ${PROJECT_NAME}_solver)
 
+  add_rostest_gtest(test_frames_and_slashes ${CMAKE_CURRENT_SOURCE_DIR}/test/test_frames_and_slashes.launch test/test_frames_and_slashes.cpp)
+  target_link_libraries(test_frames_and_slashes ${catkin_LIBRARIES})
+
   add_rostest_gtest(test_joint_states_bag ${CMAKE_CURRENT_SOURCE_DIR}/test/test_joint_states_bag.launch test/test_joint_states_bag.cpp)
   target_link_libraries(test_joint_states_bag ${catkin_LIBRARIES} ${PROJECT_NAME}_solver)
 
   add_rostest_gtest(test_subclass ${CMAKE_CURRENT_SOURCE_DIR}/test/test_subclass.launch test/test_subclass.cpp)
   target_link_libraries(test_subclass ${catkin_LIBRARIES} ${PROJECT_NAME}_solver joint_state_listener)
 
-  install(FILES test/one_link.urdf test/pr2.urdf test/two_links_fixed_joint.urdf test/two_links_moving_joint.urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test)
+  install(FILES test/one_link.urdf test/pr2.urdf test/two_links_fixed_joint.urdf test/two_links_moving_joint.urdf test/frames_and_slashes.urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test)
 
 endif()
 

include/robot_state_publisher/joint_state_listener.h

@@ -71,6 +71,9 @@ class JointStateListener {
   /// Destructor
   ~JointStateListener();
 
+private:
+  std::string getTFPrefix();
+
 protected:
   virtual void callbackJointState(const JointStateConstPtr& state);
   virtual void callbackFixedJoint(const ros::TimerEvent& e);

include/robot_state_publisher/robot_state_publisher.h

@@ -83,6 +83,11 @@ class RobotStatePublisher
   virtual void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
   virtual void publishFixedTransforms(bool use_tf_static = false);
 
+  /** Publish transforms with tf_prefix
+   */
+  void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time, const std::string & tf_prefix);
+  void publishFixedTransforms(const std::string & tf_prefix, bool use_tf_static = false);
+
 protected:
   virtual void addChildren(const KDL::SegmentMap::const_iterator segment);
 

src/joint_state_listener.cpp

@@ -90,11 +90,23 @@ JointStateListener::JointStateListener(const std::shared_ptr<RobotStatePublisher
 JointStateListener::~JointStateListener()
 {}
 
+std::string JointStateListener::getTFPrefix()
+{
+  ros::NodeHandle n_tilde("~");
+  std::string tf_prefix;
+
+  // get the tf_prefix parameter from the closest namespace
+  std::string tf_prefix_key;
+  n_tilde.searchParam("tf_prefix", tf_prefix_key);
+  n_tilde.param(tf_prefix_key, tf_prefix, std::string(""));
+
+  return tf_prefix;
+}
 
 void JointStateListener::callbackFixedJoint(const ros::TimerEvent& e)
 {
   (void)e;
-  state_publisher_->publishFixedTransforms(use_tf_static_);
+  state_publisher_->publishFixedTransforms(getTFPrefix(), use_tf_static_);
 }
 
 void JointStateListener::callbackJointState(const JointStateConstPtr& state)
@@ -150,7 +162,7 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state)
       }
     }
 
-    state_publisher_->publishTransforms(joint_positions, state->header.stamp);
+    state_publisher_->publishTransforms(joint_positions, state->header.stamp, getTFPrefix());
 
     // store publish time in joint map
     for (size_t i = 0; i<state->name.size(); ++i) {

src/robot_state_publisher.cpp

@@ -91,8 +91,35 @@ std::string stripSlash(const std::string & in)
   return in;
 }
 
+// Replicate behavior of tf 1 tf_prefix
+std::string prefix_frame(const std::string & tf_prefix, const std::string & frame)
+{
+  // If the frame begins with a slash, remove the first slash and don't add prefix at all
+  if (frame.size() && frame[0] == '/') {
+    return frame.substr(1);
+  }
+
+  std::string prefixed_frame;
+
+  // If the prefix begins with a slash, remove it
+  if (tf_prefix.size() && tf_prefix[0] == '/') {
+    prefixed_frame = tf_prefix.substr(1);
+  } else {
+    prefixed_frame = tf_prefix;
+  }
+
+  // Add a slash after a non-empty prefix
+  if (!tf_prefix.empty()) {
+    prefixed_frame += '/';
+  }
+
+  prefixed_frame += frame;
+
+  return prefixed_frame;
+}
+
 // publish moving transforms
-void RobotStatePublisher::publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time)
+void RobotStatePublisher::publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time, const std::string & tf_prefix)
 {
   ROS_DEBUG("Publishing transforms for moving joints");
   std::vector<geometry_msgs::TransformStamped> tf_transforms;
@@ -103,8 +130,8 @@ void RobotStatePublisher::publishTransforms(const std::map<std::string, double>&
     if (seg != segments_.end()) {
       geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt->second));
       tf_transform.header.stamp = time;
-      tf_transform.header.frame_id = stripSlash(seg->second.root);
-      tf_transform.child_frame_id = stripSlash(seg->second.tip);
+      tf_transform.header.frame_id = prefix_frame(tf_prefix, seg->second.root);
+      tf_transform.child_frame_id = prefix_frame(tf_prefix, seg->second.tip);
       tf_transforms.push_back(tf_transform);
     }
     else {
@@ -115,7 +142,7 @@ void RobotStatePublisher::publishTransforms(const std::map<std::string, double>&
 }
 
 // publish fixed transforms
-void RobotStatePublisher::publishFixedTransforms(bool use_tf_static)
+void RobotStatePublisher::publishFixedTransforms(const std::string & tf_prefix, bool use_tf_static)
 {
   ROS_DEBUG("Publishing transforms for fixed joints");
   std::vector<geometry_msgs::TransformStamped> tf_transforms;
@@ -128,8 +155,8 @@ void RobotStatePublisher::publishFixedTransforms(bool use_tf_static)
     if (!use_tf_static) {
       tf_transform.header.stamp += ros::Duration(0.5);
     }
-    tf_transform.header.frame_id = stripSlash(seg->second.root);
-    tf_transform.child_frame_id = stripSlash(seg->second.tip);
+    tf_transform.header.frame_id = prefix_frame(tf_prefix, seg->second.root);
+    tf_transform.child_frame_id = prefix_frame(tf_prefix, seg->second.tip);
     tf_transforms.push_back(tf_transform);
   }
   if (use_tf_static) {
@@ -140,4 +167,14 @@ void RobotStatePublisher::publishFixedTransforms(bool use_tf_static)
   }
 }
 
+void RobotStatePublisher::publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time)
+{
+  publishTransforms(joint_positions, time, "");
+}
+
+void RobotStatePublisher::publishFixedTransforms(bool use_tf_static)
+{
+  publishFixedTransforms("", use_tf_static);
+}
+
 }

test/frames_and_slashes.urdf

@@ -0,0 +1,77 @@
+<robot name="frames_and_slashes">
+  <link name="link1" />
+  <link name="/link2" />
+  <link name="//link3" />
+  <link name="link4/" />
+  <link name="link5//" />
+  <link name="/link6/" />
+  <link name="//link7/" />
+  <link name="/link8//" />
+  <link name="//link9//" />
+  <link name="//li/nk10//" />
+  <link name="//li//nk11//" />
+  <link name="li/nk12" />
+  <link name="li//nk13" />
+
+  <joint name="joint2" type="fixed">
+    <parent link="link1"/>
+    <child link="/link2"/>
+    <origin xyz="2 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint3" type="fixed">
+    <parent link="link1"/>
+    <child link="//link3"/>
+    <origin xyz="3 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint4" type="fixed">
+    <parent link="link1"/>
+    <child link="link4/"/>
+    <origin xyz="4 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint5" type="fixed">
+    <parent link="link1"/>
+    <child link="link5//"/>
+    <origin xyz="5 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint6" type="fixed">
+    <parent link="link1"/>
+    <child link="/link6/"/>
+    <origin xyz="6 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint7" type="fixed">
+    <parent link="link1"/>
+    <child link="//link7/"/>
+    <origin xyz="7 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint8" type="fixed">
+    <parent link="link1"/>
+    <child link="/link8//"/>
+    <origin xyz="8 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint9" type="fixed">
+    <parent link="link1"/>
+    <child link="//link9//"/>
+    <origin xyz="9 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint10" type="fixed">
+    <parent link="link1"/>
+    <child link="//li/nk10//"/>
+    <origin xyz="10 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint11" type="fixed">
+    <parent link="link1"/>
+    <child link="//li//nk11//"/>
+    <origin xyz="11 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint12" type="fixed">
+    <parent link="link1"/>
+    <child link="li/nk12"/>
+    <origin xyz="12 0 0" rpy="0 0 1.57" />
+  </joint>
+  <joint name="joint13" type="fixed">
+    <parent link="link1"/>
+    <child link="li//nk13"/>
+    <origin xyz="13 0 0" rpy="0 0 1.57" />
+  </joint>
+</robot>
+

test/test_frames_and_slashes.cpp

@@ -0,0 +1,135 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2021, Open Source Robotics Foundation, Inc.
+*  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 Willow Garage 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.
+*********************************************************************/
+
+
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <geometry_msgs/Transform.h>
+#include <tf2_msgs/TFMessage.h>
+
+#define EPS 0.01
+
+class TestFramesAndSlashes : public testing::Test
+{
+protected:
+  TestFramesAndSlashes()
+  {
+    tf_static_sub_ = n_.subscribe(
+      "/tf_static", 1, &TestFramesAndSlashes::tf_static_callback, this);
+  }
+
+  ~TestFramesAndSlashes() = default;
+
+  void
+  tf_static_callback(const tf2_msgs::TFMessage::ConstPtr msg)
+  {
+    transforms_received_ = *msg;
+    // avoid writing another message while test is reading the one received
+    tf_static_sub_.shutdown();
+    got_transforms_ = true;
+  }
+
+  ros::NodeHandle n_;
+  ros::Subscriber tf_sub_;
+  ros::Subscriber tf_static_sub_;
+  tf2_msgs::TFMessage transforms_received_;
+  bool got_transforms_ = false;
+};
+
+TEST_F(TestFramesAndSlashes, test)
+{
+  std::vector<std::string> expected_links(13);
+
+  {
+    ros::NodeHandle n_tilde;
+    std::string robot_description;
+    ASSERT_TRUE(n_tilde.getParam("robot_description", robot_description));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_1", expected_links[0]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_2", expected_links[1]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_3", expected_links[2]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_4", expected_links[3]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_5", expected_links[4]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_6", expected_links[5]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_7", expected_links[6]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_8", expected_links[7]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_9", expected_links[8]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_10", expected_links[9]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_11", expected_links[10]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_12", expected_links[11]));
+    ASSERT_TRUE(n_tilde.getParam("expected_name_link_13", expected_links[12]));
+  }
+
+  for (int i = 0; i < 100 && !got_transforms_; ++i) {
+    ros::Duration(0.1).sleep();
+    ros::spinOnce();
+  }
+  ASSERT_TRUE(got_transforms_);
+
+  for (size_t n = 1; n < expected_links.size(); ++n) {
+    const std::string & link_1 = expected_links.at(0);
+    const std::string & link_n = expected_links.at(n);
+
+    bool found_transform_ = false;
+    geometry_msgs::Transform transform;
+    for (const geometry_msgs::TransformStamped & tf_stamped : transforms_received_.transforms) {
+      if (tf_stamped.header.frame_id == link_1 &&
+          tf_stamped.child_frame_id == link_n)
+      {
+        transform = tf_stamped.transform;
+        found_transform_ = true;
+      }
+    }
+
+    EXPECT_TRUE(found_transform_) << "expected " << link_1 << " and " << link_n;
+    if (found_transform_) {
+      EXPECT_NEAR(transform.translation.x, n + 1, EPS) << "frames " << link_1 << " and " << link_n;
+    }
+  }
+
+  // Print transforms received for easier debugging
+  EXPECT_FALSE(testing::Test::HasFailure()) << transforms_received_;
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "test_frames_and_slashes");
+
+  int res = RUN_ALL_TESTS();
+
+  return res;
+}