Fix reload after a description with a mimic joint (#212)

* fix mimic_.clear(); * Make the copy explicit. * Add in a test for the mimic change crash. Signed-off-by: Chris Lalancette <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]>
ros · Jan 4, 2024 · e72adfc · e72adfc
1 parent f96c09b
commit e72adfc
Show file tree

Hide file tree

Showing 5 changed files with 208 additions and 2 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -95,9 +95,20 @@ if(BUILD_TESTING)
 
   add_executable(test_two_links_change_fixed_joint test/test_two_links_change_fixed_joint.cpp)
   target_include_directories(test_two_links_change_fixed_joint PRIVATE ${GTEST_INCLUDE_DIRS})
-  target_link_libraries(test_two_links_change_fixed_joint PRIVATE ${GTEST_LIBRARIES} rclcpp::rclcpp tf2_ros::tf2_ros)
+  target_link_libraries(test_two_links_change_fixed_joint PRIVATE
+    ${GTEST_LIBRARIES}
+    ${rcl_interfaces_TARGETS}
+    rclcpp::rclcpp
+    tf2_ros::tf2_ros
+  )
   add_launch_test(test/two_links_change_fixed_joint-launch.py
     ARGS "test_exe:=$<TARGET_FILE:test_two_links_change_fixed_joint>")
+
+  add_executable(test_change_mimic_joint test/test_change_mimic_joint.cpp)
+  target_include_directories(test_change_mimic_joint PRIVATE ${GTEST_INCLUDE_DIRS})
+  target_link_libraries(test_change_mimic_joint PRIVATE ${GTEST_LIBRARIES} rclcpp::rclcpp ${rcl_interfaces_TARGETS})
+  add_launch_test(test/change_mimic_joint-launch.py
+    ARGS "test_exe:=$<TARGET_FILE:test_change_mimic_joint>")
 endif()
 
 ament_export_dependencies(

diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp
@@ -186,7 +186,13 @@ void RobotStatePublisher::setupURDF(const std::string & urdf_xml)
   mimic_.clear();
   for (const std::pair<const std::string, urdf::JointSharedPtr> & i : model.joints_) {
     if (i.second->mimic) {
-      mimic_.insert(std::make_pair(i.first, i.second->mimic));
+      // Just taking a reference to the model shared pointers ends up in a crash.
+      // Explicitly make a copy of the JointMimic.
+      auto jm = std::make_shared<urdf::JointMimic>();
+      jm->offset = i.second->mimic->offset;
+      jm->multiplier = i.second->mimic->multiplier;
+      jm->joint_name = i.second->mimic->joint_name;
+      mimic_[i.first] = jm;
     }
   }
 

diff --git a/test/change_mimic_joint-launch.py b/test/change_mimic_joint-launch.py
@@ -0,0 +1,88 @@
+# Copyright (c) 2024 Open Source Robotics Foundation, Inc.
+#
+# 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 copyright holder 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 HOLDER 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.
+
+import os
+import unittest
+
+import launch
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+
+
+def generate_test_description():
+    test_exe_arg = launch.actions.DeclareLaunchArgument(
+        'test_exe',
+        description='Path to executable test',
+    )
+
+    process_under_test = launch.actions.ExecuteProcess(
+        cmd=[launch.substitutions.LaunchConfiguration('test_exe')],
+        output='screen',
+    )
+
+    urdf_file = os.path.join(os.path.dirname(__file__), 'mimic_joint.urdf')
+    with open(urdf_file, 'r') as infp:
+        robot_desc = infp.read()
+
+    params = {'robot_description': robot_desc}
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    return LaunchDescription([
+        node_robot_state_publisher,
+        test_exe_arg,
+        process_under_test,
+        launch_testing.util.KeepAliveProc(),
+        launch_testing.actions.ReadyToTest(),
+    ]), locals()
+
+
+class TestChangeFixedJoint(unittest.TestCase):
+
+    def test_termination(self, process_under_test, proc_info):
+        proc_info.assertWaitForShutdown(process=process_under_test, timeout=(10))
+
+
+@launch_testing.post_shutdown_test()
+class ChangeFixedJointTestAfterShutdown(unittest.TestCase):
+
+    def test_exit_code(self, process_under_test, proc_info):
+        # Check that all processes in the launch (in this case, there's just one) exit
+        # with code 0
+        launch_testing.asserts.assertExitCodes(
+            proc_info,
+            [launch_testing.asserts.EXIT_OK],
+            process_under_test
+        )
diff --git a/test/mimic_joint.urdf b/test/mimic_joint.urdf
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<robot name="test">
+  <link name="base_link">
+  </link>
+
+  <link name="second_link">
+  </link>
+
+  <link name="third_link">
+  </link>
+
+  <joint name="base_to_second_joint" type="fixed">
+    <origin rpy="1 0 0" xyz="0 1 0"/>
+    <parent link="base_link"/>
+    <child link="second_link"/>
+  </joint>
+
+  <joint name="base_to_third_joint" type="fixed">
+    <origin rpy="1 0 0" xyz="0 1 0"/>
+    <parent link="base_link"/>
+    <child link="third_link"/>
+    <mimic joint="base_to_second_joint" multiplier="1" offset="0"/>
+  </joint>
+
+</robot>
diff --git a/test/test_change_mimic_joint.cpp b/test/test_change_mimic_joint.cpp
@@ -0,0 +1,76 @@
+// Copyright (c) 2023, Open Source Robotics Foundation, Inc.
+//
+// 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 copyright holder 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 HOLDER 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 <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+TEST(test_publisher, test_two_joints)
+{
+  auto node = rclcpp::Node::make_shared("rsp_test_two_links_change_fixed_joint");
+
+  // OK, now publish a new URDF to the parameter and ensure that the link has been updated
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(
+    node,
+    "robot_state_publisher");
+  unsigned int i;
+  for (i = 0; i < 100 && !parameters_client->wait_for_service(std::chrono::milliseconds(100));
+    ++i)
+  {
+    ASSERT_TRUE(rclcpp::ok());
+  }
+
+  ASSERT_LT(i, 100u);
+
+  std::string new_robot_description("<robot name='test'><link name='base_link'/></robot>");
+  std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters_result =
+    parameters_client->set_parameters(
+  {
+    rclcpp::Parameter("robot_description", new_robot_description)
+  }, std::chrono::milliseconds(500));
+  ASSERT_EQ(set_parameters_result.size(), 1u);
+  ASSERT_TRUE(set_parameters_result[0].successful);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  int res = RUN_ALL_TESTS();
+
+  rclcpp::shutdown();
+
+  return res;
+}