Skip to content

Commit e72adfc

Browse files
doisygGuillaume Doisy
doisyg
and
Guillaume Doisy
authored
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]>
1 parent f96c09b commit e72adfc

5 files changed

+208
-2
lines changed

‎CMakeLists.txt

+12-1
Original file line numberDiff line numberDiff line change
@@ -95,9 +95,20 @@ if(BUILD_TESTING)
9595

9696
add_executable(test_two_links_change_fixed_joint test/test_two_links_change_fixed_joint.cpp)
9797
target_include_directories(test_two_links_change_fixed_joint PRIVATE ${GTEST_INCLUDE_DIRS})
98-
target_link_libraries(test_two_links_change_fixed_joint PRIVATE ${GTEST_LIBRARIES} rclcpp::rclcpp tf2_ros::tf2_ros)
98+
target_link_libraries(test_two_links_change_fixed_joint PRIVATE
99+
${GTEST_LIBRARIES}
100+
${rcl_interfaces_TARGETS}
101+
rclcpp::rclcpp
102+
tf2_ros::tf2_ros
103+
)
99104
add_launch_test(test/two_links_change_fixed_joint-launch.py
100105
ARGS "test_exe:=$<TARGET_FILE:test_two_links_change_fixed_joint>")
106+
107+
add_executable(test_change_mimic_joint test/test_change_mimic_joint.cpp)
108+
target_include_directories(test_change_mimic_joint PRIVATE ${GTEST_INCLUDE_DIRS})
109+
target_link_libraries(test_change_mimic_joint PRIVATE ${GTEST_LIBRARIES} rclcpp::rclcpp ${rcl_interfaces_TARGETS})
110+
add_launch_test(test/change_mimic_joint-launch.py
111+
ARGS "test_exe:=$<TARGET_FILE:test_change_mimic_joint>")
101112
endif()
102113

103114
ament_export_dependencies(

‎src/robot_state_publisher.cpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -186,7 +186,13 @@ void RobotStatePublisher::setupURDF(const std::string & urdf_xml)
186186
mimic_.clear();
187187
for (const std::pair<const std::string, urdf::JointSharedPtr> & i : model.joints_) {
188188
if (i.second->mimic) {
189-
mimic_.insert(std::make_pair(i.first, i.second->mimic));
189+
// Just taking a reference to the model shared pointers ends up in a crash.
190+
// Explicitly make a copy of the JointMimic.
191+
auto jm = std::make_shared<urdf::JointMimic>();
192+
jm->offset = i.second->mimic->offset;
193+
jm->multiplier = i.second->mimic->multiplier;
194+
jm->joint_name = i.second->mimic->joint_name;
195+
mimic_[i.first] = jm;
190196
}
191197
}
192198

‎test/change_mimic_joint-launch.py

+88
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
# Copyright (c) 2024 Open Source Robotics Foundation, Inc.
2+
#
3+
# Redistribution and use in source and binary forms, with or without
4+
# modification, are permitted provided that the following conditions are met:
5+
#
6+
# * Redistributions of source code must retain the above copyright
7+
# notice, this list of conditions and the following disclaimer.
8+
#
9+
# * Redistributions in binary form must reproduce the above copyright
10+
# notice, this list of conditions and the following disclaimer in the
11+
# documentation and/or other materials provided with the distribution.
12+
#
13+
# * Neither the name of the copyright holder nor the names of its
14+
# contributors may be used to endorse or promote products derived from
15+
# this software without specific prior written permission.
16+
#
17+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
# POSSIBILITY OF SUCH DAMAGE.
28+
29+
import os
30+
import unittest
31+
32+
import launch
33+
from launch import LaunchDescription
34+
from launch_ros.actions import Node
35+
import launch_testing
36+
import launch_testing.actions
37+
import launch_testing.asserts
38+
39+
40+
def generate_test_description():
41+
test_exe_arg = launch.actions.DeclareLaunchArgument(
42+
'test_exe',
43+
description='Path to executable test',
44+
)
45+
46+
process_under_test = launch.actions.ExecuteProcess(
47+
cmd=[launch.substitutions.LaunchConfiguration('test_exe')],
48+
output='screen',
49+
)
50+
51+
urdf_file = os.path.join(os.path.dirname(__file__), 'mimic_joint.urdf')
52+
with open(urdf_file, 'r') as infp:
53+
robot_desc = infp.read()
54+
55+
params = {'robot_description': robot_desc}
56+
node_robot_state_publisher = Node(
57+
package='robot_state_publisher',
58+
executable='robot_state_publisher',
59+
output='screen',
60+
parameters=[params]
61+
)
62+
63+
return LaunchDescription([
64+
node_robot_state_publisher,
65+
test_exe_arg,
66+
process_under_test,
67+
launch_testing.util.KeepAliveProc(),
68+
launch_testing.actions.ReadyToTest(),
69+
]), locals()
70+
71+
72+
class TestChangeFixedJoint(unittest.TestCase):
73+
74+
def test_termination(self, process_under_test, proc_info):
75+
proc_info.assertWaitForShutdown(process=process_under_test, timeout=(10))
76+
77+
78+
@launch_testing.post_shutdown_test()
79+
class ChangeFixedJointTestAfterShutdown(unittest.TestCase):
80+
81+
def test_exit_code(self, process_under_test, proc_info):
82+
# Check that all processes in the launch (in this case, there's just one) exit
83+
# with code 0
84+
launch_testing.asserts.assertExitCodes(
85+
proc_info,
86+
[launch_testing.asserts.EXIT_OK],
87+
process_under_test
88+
)

‎test/mimic_joint.urdf

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
<?xml version="1.0"?>
2+
<robot name="test">
3+
<link name="base_link">
4+
</link>
5+
6+
<link name="second_link">
7+
</link>
8+
9+
<link name="third_link">
10+
</link>
11+
12+
<joint name="base_to_second_joint" type="fixed">
13+
<origin rpy="1 0 0" xyz="0 1 0"/>
14+
<parent link="base_link"/>
15+
<child link="second_link"/>
16+
</joint>
17+
18+
<joint name="base_to_third_joint" type="fixed">
19+
<origin rpy="1 0 0" xyz="0 1 0"/>
20+
<parent link="base_link"/>
21+
<child link="third_link"/>
22+
<mimic joint="base_to_second_joint" multiplier="1" offset="0"/>
23+
</joint>
24+
25+
</robot>

‎test/test_change_mimic_joint.cpp

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
// Copyright (c) 2023, Open Source Robotics Foundation, Inc.
2+
//
3+
// Redistribution and use in source and binary forms, with or without
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
12+
//
13+
// * Neither the name of the copyright holder nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
16+
//
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
// POSSIBILITY OF SUCH DAMAGE.
28+
29+
#include <chrono>
30+
#include <memory>
31+
#include <string>
32+
#include <vector>
33+
34+
#include "gtest/gtest.h"
35+
36+
#include "rcl_interfaces/msg/set_parameters_result.hpp"
37+
#include "rclcpp/rclcpp.hpp"
38+
39+
TEST(test_publisher, test_two_joints)
40+
{
41+
auto node = rclcpp::Node::make_shared("rsp_test_two_links_change_fixed_joint");
42+
43+
// OK, now publish a new URDF to the parameter and ensure that the link has been updated
44+
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(
45+
node,
46+
"robot_state_publisher");
47+
unsigned int i;
48+
for (i = 0; i < 100 && !parameters_client->wait_for_service(std::chrono::milliseconds(100));
49+
++i)
50+
{
51+
ASSERT_TRUE(rclcpp::ok());
52+
}
53+
54+
ASSERT_LT(i, 100u);
55+
56+
std::string new_robot_description("<robot name='test'><link name='base_link'/></robot>");
57+
std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters_result =
58+
parameters_client->set_parameters(
59+
{
60+
rclcpp::Parameter("robot_description", new_robot_description)
61+
}, std::chrono::milliseconds(500));
62+
ASSERT_EQ(set_parameters_result.size(), 1u);
63+
ASSERT_TRUE(set_parameters_result[0].successful);
64+
}
65+
66+
int main(int argc, char ** argv)
67+
{
68+
testing::InitGoogleTest(&argc, argv);
69+
rclcpp::init(argc, argv);
70+
71+
int res = RUN_ALL_TESTS();
72+
73+
rclcpp::shutdown();
74+
75+
return res;
76+
}

0 commit comments

Comments
 (0)