Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix reload after a description with a mimic joint #212

Merged
merged 3 commits into from
Jan 4, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
8 changes: 7 additions & 1 deletion src/robot_state_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down
88 changes: 88 additions & 0 deletions test/change_mimic_joint-launch.py
Original file line number Diff line number Diff line change
@@ -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
)
25 changes: 25 additions & 0 deletions test/mimic_joint.urdf
Original file line number Diff line number Diff line change
@@ -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>
76 changes: 76 additions & 0 deletions test/test_change_mimic_joint.cpp
Original file line number Diff line number Diff line change
@@ -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;
}