diff --git a/joint_state_topic_hardware_interface/CMakeLists.txt b/joint_state_topic_hardware_interface/CMakeLists.txt index 69d3148..6c161a9 100644 --- a/joint_state_topic_hardware_interface/CMakeLists.txt +++ b/joint_state_topic_hardware_interface/CMakeLists.txt @@ -81,12 +81,19 @@ if(BUILD_TESTING) rclcpp::rclcpp rclcpp_action::rclcpp_action ) + add_executable(test_gripper test/gripper/test_gripper.cpp) + target_link_libraries(test_gripper PUBLIC + ${std_msgs_TARGETS} + rclcpp::rclcpp + ) function(add_ros_isolated_launch_test path) set(RUNNER "${ament_cmake_ros_DIR}/run_test_isolated.py") add_launch_test("${path}" RUNNER "${RUNNER}" ${ARGN}) endfunction() add_ros_isolated_launch_test( test/rrr/position.test.py) + add_ros_isolated_launch_test( + test/gripper/position.test.py) endif() pluginlib_export_plugin_description_file(hardware_interface joint_state_topic_hardware_interface_plugin_description.xml) diff --git a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp index b8a8062..51482a8 100644 --- a/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp +++ b/joint_state_topic_hardware_interface/include/joint_state_topic_hardware_interface/joint_state_topic_hardware_interface.hpp @@ -38,10 +38,6 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; hardware_interface::return_type write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override; @@ -52,31 +48,9 @@ class JointStateTopicSystem : public hardware_interface::SystemInterface sensor_msgs::msg::JointState latest_joint_state_; bool sum_wrapped_joint_states_{ false }; - /// Use standard interfaces for joints because they are relevant for dynamic behavior - std::array standard_interfaces_ = { - { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, - hardware_interface::HW_IF_EFFORT } - }; - - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_command_values_; - std::vector> joint_state_values_; - // If the difference between the current joint state and joint command is less than this value, // the joint command will not be published. double trigger_joint_command_threshold_ = 1e-5; - - template - bool getInterface(const std::string& name, const std::string& interface_name, const size_t vector_index, - std::vector>& values, std::vector& interfaces); }; } // namespace joint_state_topic_hardware_interface diff --git a/joint_state_topic_hardware_interface/package.xml b/joint_state_topic_hardware_interface/package.xml index 212d542..b6e5cd8 100644 --- a/joint_state_topic_hardware_interface/package.xml +++ b/joint_state_topic_hardware_interface/package.xml @@ -30,6 +30,7 @@ ament_cmake_ros control_msgs controller_manager + forward_command_controller joint_state_broadcaster joint_trajectory_controller launch_ros diff --git a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp index 6d09455..b2b9bf5 100644 --- a/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp +++ b/joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp @@ -29,13 +29,14 @@ namespace { /** @brief Sums the total rotation for joint states that wrap from 2*pi to -2*pi when rotating in the positive direction */ -void sumRotationFromMinus2PiTo2Pi(const double current_wrapped_rad, double& total_rotation) +double sumRotationFromMinus2PiTo2Pi(const double current_wrapped_rad, double total_rotation_in) { double delta = 0; - angles::shortest_angular_distance_with_large_limits(total_rotation, current_wrapped_rad, 2 * M_PI, -2 * M_PI, delta); + angles::shortest_angular_distance_with_large_limits(total_rotation_in, current_wrapped_rad, 2 * M_PI, -2 * M_PI, + delta); // Add the corrected delta to the total rotation - total_rotation += delta; + return total_rotation_in + delta; } } // namespace @@ -54,64 +55,6 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::ERROR; } - // Initialize storage for all joints' standard interfaces, regardless of their existence and set all values to nan - joint_command_values_.resize(standard_interfaces_.size()); - joint_state_values_.resize(standard_interfaces_.size()); - for (auto i = 0u; i < standard_interfaces_.size(); i++) - { - joint_command_values_[i].resize(get_hardware_info().joints.size(), 0.0); - joint_state_values_[i].resize(get_hardware_info().joints.size(), 0.0); - } - - // Initial command values - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto& component = get_hardware_info().joints[i]; - for (const auto& interface : component.state_interfaces) - { - auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name); - // If interface name is found in the interfaces list - if (it != standard_interfaces_.end()) - { - auto index = static_cast(std::distance(standard_interfaces_.begin(), it)); - // Check the initial_value param is used - if (!interface.initial_value.empty()) - { - joint_state_values_[index][i] = std::stod(interface.initial_value); - joint_command_values_[index][i] = std::stod(interface.initial_value); - } - } - } - } - - // Search for mimic joints - for (auto i = 0u; i < get_hardware_info().joints.size(); ++i) - { - const auto& joint = get_hardware_info().joints.at(i); - if (joint.parameters.find("mimic") != joint.parameters.cend()) - { - const auto mimicked_joint_it = std::find_if( - get_hardware_info().joints.begin(), get_hardware_info().joints.end(), - [&mimicked_joint = joint.parameters.at("mimic")](const hardware_interface::ComponentInfo& joint_info) { - return joint_info.name == mimicked_joint; - }); - if (mimicked_joint_it == get_hardware_info().joints.cend()) - { - throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); - } - MimicJoint mimic_joint; - mimic_joint.joint_index = i; - mimic_joint.mimicked_joint_index = - static_cast(std::distance(get_hardware_info().joints.begin(), mimicked_joint_it)); - auto param_it = joint.parameters.find("multiplier"); - if (param_it != joint.parameters.end()) - { - mimic_joint.multiplier = std::stod(joint.parameters.at("multiplier")); - } - mimic_joints_.push_back(mimic_joint); - } - } - const auto get_hardware_parameter = [this](const std::string& parameter_name, const std::string& default_value) { if (auto it = get_hardware_info().hardware_parameters.find(parameter_name); it != get_hardware_info().hardware_parameters.end()) @@ -143,164 +86,130 @@ CallbackReturn JointStateTopicSystem::on_init(const hardware_interface::Hardware return CallbackReturn::SUCCESS; } -std::vector JointStateTopicSystem::export_state_interfaces() -{ - std::vector state_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto& joint = get_hardware_info().joints[i]; - for (const auto& interface : joint.state_interfaces) - { - // Add interface: if not in the standard list then use "other" interface list - if (!getInterface(joint.name, interface.name, i, joint_state_values_, state_interfaces)) - { - throw std::runtime_error("Interface is not found in the standard list."); - } - } - } - - return state_interfaces; -} - -std::vector JointStateTopicSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - // Joints' state interfaces - for (auto i = 0u; i < get_hardware_info().joints.size(); i++) - { - const auto& joint = get_hardware_info().joints[i]; - for (const auto& interface : joint.command_interfaces) - { - if (!getInterface(joint.name, interface.name, i, joint_command_values_, command_interfaces)) - { - throw std::runtime_error("Interface is not found in the standard list."); - } - } - } - - return command_interfaces; -} - hardware_interface::return_type JointStateTopicSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + const auto& joints = get_hardware_info().joints; for (std::size_t i = 0; i < latest_joint_state_.name.size(); ++i) { - const auto& joints = get_hardware_info().joints; - auto it = std::find_if(joints.begin(), joints.end(), - [&joint_name = std::as_const(latest_joint_state_.name[i])]( - const hardware_interface::ComponentInfo& info) { return joint_name == info.name; }); + const auto it = std::find_if(joints.begin(), joints.end(), + [name = latest_joint_state_.name[i]](const hardware_interface::ComponentInfo& joint) { + return joint.name == name; + }); if (it != joints.end()) { - auto j = static_cast(std::distance(joints.begin(), it)); - if (sum_wrapped_joint_states_) + if (std::find_if(get_hardware_info().mimic_joints.begin(), get_hardware_info().mimic_joints.end(), + [idx = static_cast(std::distance(joints.begin(), it))]( + const hardware_interface::MimicJoint& mimic_joint) { + return idx == mimic_joint.joint_index; + }) != get_hardware_info().mimic_joints.end()) { - sumRotationFromMinus2PiTo2Pi(latest_joint_state_.position[i], joint_state_values_[POSITION_INTERFACE_INDEX][j]); + // mimic joints are updated at the end of this function + continue; } - else + + if (!latest_joint_state_.position.empty() && std::isfinite(latest_joint_state_.position.at(i))) { - joint_state_values_[POSITION_INTERFACE_INDEX][j] = latest_joint_state_.position[i]; + if (sum_wrapped_joint_states_) + { + auto name = latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_POSITION; + + set_state(name, sumRotationFromMinus2PiTo2Pi(latest_joint_state_.position.at(i), get_state(name))); + } + else + { + set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_POSITION, + latest_joint_state_.position.at(i)); + } } - if (!latest_joint_state_.velocity.empty()) + if (!latest_joint_state_.velocity.empty() && std::isfinite(latest_joint_state_.velocity.at(i))) { - joint_state_values_[VELOCITY_INTERFACE_INDEX][j] = latest_joint_state_.velocity[i]; + set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_VELOCITY, + latest_joint_state_.velocity.at(i)); } - if (!latest_joint_state_.effort.empty()) + if (!latest_joint_state_.effort.empty() && std::isfinite(latest_joint_state_.effort.at(i))) { - joint_state_values_[EFFORT_INTERFACE_INDEX][j] = latest_joint_state_.effort[i]; + set_state(latest_joint_state_.name[i] + "/" + hardware_interface::HW_IF_EFFORT, + latest_joint_state_.effort.at(i)); } } } - for (const auto& mimic_joint : mimic_joints_) + // Update mimic joints + for (const auto& mimic_joint : get_hardware_info().mimic_joints) { - for (auto& joint_state : joint_state_values_) + const auto& mimic_joint_name = joints.at(mimic_joint.joint_index).name; + const auto& mimicked_joint_name = joints.at(mimic_joint.mimicked_joint_index).name; + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION)) { - joint_state[mimic_joint.joint_index] = mimic_joint.multiplier * joint_state[mimic_joint.mimicked_joint_index]; + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_POSITION, + mimic_joint.offset + + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_POSITION)); + } + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_VELOCITY, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_VELOCITY)); + } + if (has_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)) + { + set_state(mimic_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION, + mimic_joint.multiplier * get_state(mimicked_joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)); } } return hardware_interface::return_type::OK; } -template -bool JointStateTopicSystem::getInterface(const std::string& name, const std::string& interface_name, - const size_t vector_index, std::vector>& values, - std::vector& interfaces) -{ - auto it = std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface_name); - if (it != standard_interfaces_.end()) - { - auto j = static_cast(std::distance(standard_interfaces_.begin(), it)); - interfaces.emplace_back(name, *it, &values[j][vector_index]); - return true; - } - return false; -} - hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + const auto& joints = get_hardware_info().joints; // To avoid spamming TopicBased's joint command topic we check the difference between the joint states and // the current joint commands, if it's smaller than a threshold we don't publish it. - const auto diff = std::transform_reduce( - joint_state_values_[POSITION_INTERFACE_INDEX].cbegin(), joint_state_values_[POSITION_INTERFACE_INDEX].cend(), - joint_command_values_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, - [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); + auto diff = 0.0; + for (std::size_t i = 0; i < joints.size(); ++i) + { + for (const auto& interface : joints[i].command_interfaces) + { + if (interface.name != hardware_interface::HW_IF_POSITION) + { + continue; + } + // sum the absolute difference for all joints + diff += std::abs(get_state(joints[i].name + "/" + interface.name) - + get_command(joints[i].name + "/" + interface.name)); + } + } if (diff <= trigger_joint_command_threshold_) { return hardware_interface::return_type::OK; } sensor_msgs::msg::JointState joint_state; - for (std::size_t i = 0; i < get_hardware_info().joints.size(); ++i) + for (std::size_t i = 0; i < joints.size(); ++i) { - joint_state.name.push_back(get_hardware_info().joints[i].name); + joint_state.name.push_back(joints[i].name); joint_state.header.stamp = get_node()->now(); // only send commands to the interfaces that are defined for this joint - for (const auto& interface : get_hardware_info().joints[i].command_interfaces) + for (const auto& interface : joints[i].command_interfaces) { if (interface.name == hardware_interface::HW_IF_POSITION) { - joint_state.position.push_back(joint_command_values_[POSITION_INTERFACE_INDEX][i]); + joint_state.position.push_back(get_command(joints[i].name + "/" + interface.name)); } else if (interface.name == hardware_interface::HW_IF_VELOCITY) { - joint_state.velocity.push_back(joint_command_values_[VELOCITY_INTERFACE_INDEX][i]); + joint_state.velocity.push_back(get_command(joints[i].name + "/" + interface.name)); } else if (interface.name == hardware_interface::HW_IF_EFFORT) { - joint_state.effort.push_back(joint_command_values_[EFFORT_INTERFACE_INDEX][i]); + joint_state.effort.push_back(get_command(joints[i].name + "/" + interface.name)); } else { RCLCPP_WARN_ONCE(get_node()->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.", - get_hardware_info().joints[i].name.c_str(), interface.name.c_str()); - } - } - } - - for (const auto& mimic_joint : mimic_joints_) - { - for (const auto& interface : get_hardware_info().joints[mimic_joint.mimicked_joint_index].command_interfaces) - { - if (interface.name == hardware_interface::HW_IF_POSITION) - { - joint_state.position[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index]; - } - else if (interface.name == hardware_interface::HW_IF_VELOCITY) - { - joint_state.velocity[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index]; - } - else if (interface.name == hardware_interface::HW_IF_EFFORT) - { - joint_state.effort[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index]; + joints[i].name.c_str(), interface.name.c_str()); } } } diff --git a/joint_state_topic_hardware_interface/test/gripper/gripper.launch.py b/joint_state_topic_hardware_interface/test/gripper/gripper.launch.py new file mode 100644 index 0000000..3f1f7e4 --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/gripper.launch.py @@ -0,0 +1,56 @@ +# Copyright 2025 ros2_control Development Team +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path + +import xacro +from launch import LaunchDescription +from launch_ros.actions import Node + +SCRIPT_PATH = Path(os.path.realpath(__file__)).parent + + +def generate_launch_description(): + ros2_controllers_file = str(Path(SCRIPT_PATH / "gripper_controller_position.yaml")) + robot_description = { + "robot_description": xacro.process_file( + SCRIPT_PATH / "gripper.urdf.xacro" + ).toxml(), + } + controllers = ["joint_state_broadcaster", "gripper_controller"] + return LaunchDescription( + [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[robot_description], + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_file], + output="screen", + ), + ] + + [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "--param-file", ros2_controllers_file], + ) + for controller in controllers + ], + ) diff --git a/joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro b/joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro new file mode 100644 index 0000000..b00412d --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/gripper.urdf.xacro @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_joint_states + + + + + 0.15 + + + + + + + + + + + diff --git a/joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml b/joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml new file mode 100644 index 0000000..e8bedb3 --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/gripper_controller_position.yaml @@ -0,0 +1,13 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +gripper_controller: + ros__parameters: + type: forward_command_controller/ForwardCommandController + joints: + - right_finger_joint + interface_name: position diff --git a/joint_state_topic_hardware_interface/test/gripper/position.test.py b/joint_state_topic_hardware_interface/test/gripper/position.test.py new file mode 100644 index 0000000..45c5e3c --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/position.test.py @@ -0,0 +1,174 @@ +# Copyright 2025 ros2_control Development Team +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest +from collections import OrderedDict +from pathlib import Path + +import launch_testing +import launch_testing.markers +import numpy as np +import pytest +import rclpy +from ament_index_python.packages import get_package_prefix +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from rclpy.qos import qos_profile_system_default +from sensor_msgs.msg import JointState + + +# This function specifies the processes to be run for our test +@pytest.mark.rostest +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env["PYTHONUNBUFFERED"] = "1" + + return LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + str(Path(os.path.realpath(__file__)).parent), + "gripper.launch.py", + ], + ), + ), + ), + Node( + package="topic_tools", + executable="relay", + output="screen", + arguments=["/topic_based_joint_commands", "/topic_based_joint_states"], + ), + KeepAliveProc(), + ReadyToTest(), + ], + ) + + +class TestFixture(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_node") + + # Reported joint state from joint_state_broadcaster + self.current_joint_state_subscriber = self.node.create_subscription( + JointState, + "joint_states", + self.joint_states_callback, + qos_profile_system_default, + ) + + self.joint_names = ["right_finger_joint", "left_finger_joint"] + + def tearDown(self): + self.node.destroy_node() + + def test_controller_running(self, proc_output): + cnames = ["gripper_controller", "joint_state_broadcaster"] + check_node_running(self.node, "relay") + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", self.joint_names) + + def test_main(self, launch_service, proc_info, proc_output): + # By default the joint_states should have the initial_value from rrr.urdf.xacro + self.node.get_logger().info("Checking initial joint states...") + current_joint_state = self.get_current_joint_state() + urdf_initial_values = [0.15, 0.15] + assert current_joint_state == urdf_initial_values, ( + f"{current_joint_state=} != {urdf_initial_values=}" + ) + + # Test setting the robot joint states via controller + # test_gripper was not installed, call it directly from build space + pkg_name = "joint_state_topic_hardware_interface" + proc_action = Node( + executable=os.path.join( + get_package_prefix(pkg_name).replace("install", "build"), + "test_gripper", + ), + output="screen", + ) + + with launch_testing.tools.launch_process( + launch_service, proc_action, proc_info, proc_output + ): + proc_info.assertWaitForShutdown(process=proc_action, timeout=300) + launch_testing.asserts.assertExitCodes( + proc_info, process=proc_action, allowable_exit_codes=[0] + ) + + self.node.get_logger().info("Checking final joint states...") + current_joint_state = self.get_current_joint_state() + final_values = [0.09, 0.09] + assert np.allclose( + current_joint_state, + final_values, + atol=1e-3, + ), f"{current_joint_state=} != {final_values=}" + + def joint_states_callback(self, msg: JointState): + self.current_joint_state = self.filter_joint_state_msg(msg) + + def get_current_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_joint_state = [] + while len(self.current_joint_state) == 0: + self.node.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self.node, timeout_sec=1.0) + return self.current_joint_state + + def filter_joint_state_msg(self, msg: JointState): + joint_states = [] + for joint_name in self.joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + raise ValueError(msg) from None + joint_states.append(msg.position[index]) + return joint_states + + +@launch_testing.post_shutdown_test() +class TestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_pass(self, proc_info): + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp b/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp new file mode 100644 index 0000000..e149639 --- /dev/null +++ b/joint_state_topic_hardware_interface/test/gripper/test_gripper.cpp @@ -0,0 +1,57 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Denis Štogl (Stogl Robotics Consulting) +// + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node = std::make_shared("gripper_test_node"); + + auto publisher = node->create_publisher("/gripper_controller/commands", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + std_msgs::msg::Float64MultiArray commands; + + using namespace std::chrono_literals; + + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.38; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.19; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.09; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + rclcpp::shutdown(); + + return 0; +} diff --git a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp index 5816585..51755e0 100644 --- a/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp +++ b/joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp @@ -13,23 +13,194 @@ // limitations under the License. #include +#include #include -#include #include #include -#if __has_include() -#include + +#if __has_include("hardware_interface/hardware_interface/version.h") +#include "hardware_interface/hardware_interface/version.h" +#else +#include "hardware_interface/version.h" +#endif +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +namespace +{ +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +} // namespace + +// Forward declaration +class TestableResourceManager; + +class TestTopicBasedSystem : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() override + { + executor_ = std::make_shared(); + node_ = std::make_shared(::testing::UnitTest::GetInstance()->current_test_info()->name()); + + js_publisher_ = node_->create_publisher("/topic_based_custom_joint_states", + rclcpp::SystemDefaultsQoS()); + + INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(node_, hardware_interface::DEFAULT_INTROSPECTION_TOPIC, + hardware_interface::DEFAULT_REGISTRY_KEY); + } + + void TearDown() override + { + node_.reset(); + } + + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::unique_ptr rm_; + rclcpp::Publisher::SharedPtr js_publisher_; + + /// Publish joint_state_message + /** + * names - names of joints + * points - vector of trajectory-velocities. One point per controlled joint + */ + void publish(const std::vector& joint_names = {}, const std::vector& points_positions = {}, + const std::vector& points_velocities = {}, const std::vector& points_effort = {}) + { + int wait_count = 0; + const auto topic = js_publisher_->get_topic_name(); + while (node_->count_subscribers(topic) == 0) + { + if (wait_count >= 5) + { + auto error_msg = std::string("publishing to ") + topic + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + + sensor_msgs::msg::JointState state_msg; + state_msg.name = joint_names; + + state_msg.position = points_positions; + state_msg.velocity = points_velocities; + state_msg.effort = points_effort; + + js_publisher_->publish(state_msg); + } + + /** + * @brief wait_for_msg block until a new JointState is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function + */ + void wait_for_msg(const std::chrono::milliseconds& timeout = std::chrono::milliseconds{ 10 }) + { + auto until = node_->get_clock()->now() + timeout; + while (node_->get_clock()->now() < until) + { + executor_->spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void init_rm(const std::string& urdf) + { + // The API of the ResourceManager has changed in hardware_interface 4.13.0 +#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + hardware_interface::ResourceManagerParams params; + params.robot_description = urdf; + params.clock = node_->get_clock(); + params.logger = node_->get_logger(); + params.executor = executor_; + rm_ = std::make_unique(params, true); #else -#include + rm_ = std::make_unique(urdf, true, false); #endif -#include -#include -#include -#include -#include + } +}; + +class TestableResourceManager : public hardware_interface::ResourceManager +{ +public: + friend TestTopicBasedSystem; + + // The API of the ResourceManager has changed in hardware_interface 4.13.0 +#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + explicit TestableResourceManager(const hardware_interface::ResourceManagerParams& params, bool load) + : hardware_interface::ResourceManager(params, load) + { + } +#else + explicit TestableResourceManager(const std::string& urdf, bool validate_interfaces = true, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + { + } +#endif + + explicit TestableResourceManager(rclcpp::Node::SharedPtr node) + : hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()) + { + } + + explicit TestableResourceManager(rclcpp::Node::SharedPtr node, const std::string& urdf, bool activate_all = false, + unsigned int cm_update_rate = 100) + : hardware_interface::ResourceManager(urdf, node->get_node_clock_interface(), node->get_node_logging_interface(), + activate_all, cm_update_rate) + { + } +}; + +void set_components_state(TestableResourceManager& rm, const std::vector& components, + const uint8_t state_id, const std::string& state_name) +{ + for (const auto& component : components) + { + rclcpp_lifecycle::State state(state_id, state_name); + rm.set_component_state(component, state); + } +} + +void configure_components(TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) +{ + set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); +}; + +void activate_components(TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) +{ + set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); +}; + +void deactivate_components(TestableResourceManager& rm, + const std::vector& components = { "GenericSystem2dof" }) +{ + set_components_state(rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); +}; -TEST(TestTopicBasedSystem, load_topic_based_system_2dof) +TEST_F(TestTopicBasedSystem, load_topic_based_system_2dof) { const std::string hardware_system_2dof_topic_based = R"( @@ -42,46 +213,441 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) - + + 1.57 + - + + -1.57 + )"; auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; - auto node = std::make_shared("test_topic_based_system"); -// The API of the ResourceManager has changed in hardware_interface 4.13.0 -#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) - hardware_interface::ResourceManagerParams params; - params.robot_description = urdf; - params.clock = node->get_clock(); - params.logger = node->get_logger(); - params.executor = std::make_shared(); - try - { - hardware_interface::ResourceManager rm(params, true); - } - catch (const std::exception& e) - { - std::cerr << "Exception caught: " << e.what() << std::endl; - FAIL() << "Exception thrown during ResourceManager construction: " << e.what(); - } -#else - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false)); -#endif + init_rm(urdf); + + // Activate components to get all interfaces available + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); + + // Check interfaces + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(4u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(4u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_p_s = rm_->claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_p_s = rm_->claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position"); + hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_p_c = rm_->claim_command_interface("joint2/position"); + hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity"); + + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_EQ(j2_p_s.get_optional().value(), -1.57); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_p_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_p_c.set_value(0.11)); + ASSERT_TRUE(j1_v_c.set_value(0.12)); + ASSERT_TRUE(j2_p_c.set_value(0.13)); + ASSERT_TRUE(j2_v_c.set_value(0.14)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_EQ(j2_p_s.get_optional().value(), -1.57); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_p_c.get_optional().value(), 0.13); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_p_s.get_optional().value(), 0.21); + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_p_s.get_optional().value(), 0.23); + EXPECT_EQ(j2_v_s.get_optional().value(), 0.24); + + // commands should remain unchanged + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_p_c.get_optional().value(), 0.13); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); } -int main(int argc, char** argv) +TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only) { - testing::InitGoogleMock(&argc, argv); - rclcpp::init(argc, argv); + const std::string hardware_system_2dof_topic_based = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + + + + + + +)"; + auto urdf = + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; + + init_rm(urdf); + + // Activate components to get all interfaces available + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); + + // Check interfaces + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(2u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(2u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity"); + + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_v_c.set_value(0.12)); + ASSERT_TRUE(j2_v_c.set_value(0.14)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + + publish({ "joint1", "joint2" }, {}, { 0.22, 0.24 }); + wait_for_msg(std::chrono::milliseconds{ 100 }); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_v_s.get_optional().value(), 0.24); + + // commands should remain unchanged + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); +} + +TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only_inconsistent_topic) +{ + const std::string hardware_system_2dof_topic_based = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + + + + + + +)"; + auto urdf = + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; + + init_rm(urdf); + + // Activate components to get all interfaces available + activate_components(*rm_, { "JointStateTopicBasedSystem2dof" }); + + // Check interfaces + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(2u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(2u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity"); + hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity"); + + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_v_c.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_v_c.set_value(0.12)); + ASSERT_TRUE(j2_v_c.set_value(0.14)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + + EXPECT_EQ(j1_v_c.get_optional().value(), 0.12); + EXPECT_EQ(j2_v_c.get_optional().value(), 0.14); + + // Reading should fail as position interface is missing + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + wait_for_msg(); + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::ERROR); +} + +TEST_F(TestTopicBasedSystem, topic_based_system_with_mimic_joint) +{ + const std::string hardware_system_2dof_with_mimic_joint = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + 1.57 + + + + + + + + +)"; + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint + + ros2_control_test_assets::urdf_tail; + + init_rm(urdf); + + // Activate components to get all interfaces available + activate_components(*rm_, { "JointStateTopicBasedSystem2dofMimic" }); + + // Check interfaces + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(4u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(1u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_p_s = rm_->claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_p_s = rm_->claim_state_interface("joint2/position"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position"); + + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_p_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_p_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_p_c.set_value(0.11)); + + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + // but mimic joint should update its state based on the first joint state + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_EQ(j2_p_s.get_optional().value(), -2 * 1.57); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_p_s.get_optional().value(), 0.21); + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_p_s.get_optional().value(), -2 * 0.21); // received value ignored due to mimic + EXPECT_EQ(j2_v_s.get_optional().value(), -2 * 0.22); // received value ignored due to mimic + + // commands should remain unchanged + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); +} + +TEST_F(TestTopicBasedSystem, topic_based_system_with_mimic_joint_missing_position) +{ + const std::string hardware_system_2dof_with_mimic_joint = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + 1.57 + + + + + + + +)"; + auto urdf = ros2_control_test_assets::urdf_head_mimic + hardware_system_2dof_with_mimic_joint + + ros2_control_test_assets::urdf_tail; + + init_rm(urdf); + + // Activate components to get all interfaces available + activate_components(*rm_, { "JointStateTopicBasedSystem2dofMimic" }); + + // Check interfaces + EXPECT_EQ(1u, rm_->system_components_size()); + ASSERT_EQ(3u, rm_->state_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint1/velocity")); + EXPECT_FALSE(rm_->state_interface_exists("joint2/position")); + EXPECT_TRUE(rm_->state_interface_exists("joint2/velocity")); + + ASSERT_EQ(1u, rm_->command_interface_keys().size()); + EXPECT_TRUE(rm_->state_interface_exists("joint1/position")); + + // Check initial values + hardware_interface::LoanedStateInterface j1_p_s = rm_->claim_state_interface("joint1/position"); + hardware_interface::LoanedStateInterface j1_v_s = rm_->claim_state_interface("joint1/velocity"); + hardware_interface::LoanedStateInterface j2_v_s = rm_->claim_state_interface("joint2/velocity"); + hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position"); + + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j1_p_c.get_optional().value())); + + // set some new values in commands + ASSERT_TRUE(j1_p_c.set_value(0.11)); + + // should not throw even if mimic joint is missing position interface + hardware_interface::return_type ret; + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // command is not propagated to state until topic from robot is received + // same holds for velocity + EXPECT_EQ(j1_p_s.get_optional().value(), 1.57); + EXPECT_TRUE(std::isnan(j1_v_s.get_optional().value())); + EXPECT_TRUE(std::isnan(j2_v_s.get_optional().value())); + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); + + publish({ "joint1", "joint2" }, { 0.21, 0.23 }, { 0.22, 0.24 }); + + wait_for_msg(); + + ASSERT_NO_THROW(ret = rm_->read(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result); + ASSERT_EQ(ret, hardware_interface::return_type::OK); + + // new states should have been updated from topic + EXPECT_EQ(j1_p_s.get_optional().value(), 0.21); + EXPECT_EQ(j1_v_s.get_optional().value(), 0.22); + EXPECT_EQ(j2_v_s.get_optional().value(), -2 * 0.22); // received value ignored due to mimic - return RUN_ALL_TESTS(); + // commands should remain unchanged + EXPECT_EQ(j1_p_c.get_optional().value(), 0.11); }