diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 29947e9..6f7688c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,17 +1,3 @@ -# To use: -# -# pre-commit run -a -# -# Or: -# -# pre-commit install # (runs every time you commit in git) -# -# To update this file: -# -# pre-commit autoupdate -# -# See https://github.com/pre-commit/pre-commit - repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks @@ -43,10 +29,12 @@ repos: - id: requirements-txt-fixer - id: sort-simple-yaml - id: trailing-whitespace - - repo: https://github.com/psf/black - rev: 22.12.0 + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.6.9 hooks: - - id: black + - id: ruff + args: ['--output-format=full', '--fix', '--config', 'pyproject.toml'] + - id: ruff-format - repo: https://github.com/codespell-project/codespell rev: v2.0.0 hooks: @@ -71,3 +59,11 @@ repos: rev: v2.10.0 hooks: - id: hadolint-docker + - repo: https://github.com/pre-commit/mirrors-prettier + rev: "v3.1.0" + hooks: + - id: prettier + additional_dependencies: + - "prettier@3.1.0" + - "@prettier/plugin-xml@3.3.1" + files: \.(xml|xacro|srdf)$ diff --git a/.prettierrc.cjs b/.prettierrc.cjs new file mode 100644 index 0000000..4e04acc --- /dev/null +++ b/.prettierrc.cjs @@ -0,0 +1,5 @@ +module.exports = { + plugins: [require.resolve("@prettier/plugin-xml")], + xmlWhitespaceSensitivity: "ignore", + xmlQuoteAttributes: "double" +} diff --git a/CMakeLists.txt b/CMakeLists.txt index 30f5487..3a8e525 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,9 +56,12 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(ros_testing REQUIRED) # ROS2 linters, but disable copyright test. PickNik's copyright's may not conform # to this test + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_pep257_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) ament_lint_auto_find_test_dependencies() @@ -68,6 +71,14 @@ if(BUILD_TESTING) target_link_libraries(topic_based_system_test ${PROJECT_NAME}) ament_target_dependencies(topic_based_system_test ${THIS_PACKAGE_INCLUDE_DEPENDS} ros2_control_test_assets) + + # Integration tests + add_ros_test( + test/ros2_control.test.py + TIMEOUT + 120 + ARGS + test_file:=${CMAKE_CURRENT_SOURCE_DIR}/test/test_topic_based_robot.py) endif() pluginlib_export_plugin_description_file(hardware_interface topic_based_ros2_control_plugin_description.xml) diff --git a/package.xml b/package.xml index 427f3c6..cd3082d 100644 --- a/package.xml +++ b/package.xml @@ -1,32 +1,48 @@ - + - topic_based_ros2_control - 0.2.0 - ros2 control hardware interface for topic_based sim + topic_based_ros2_control + 0.2.0 + ros2 control hardware interface for topic_based sim - Marq Rasmussen - Jafar - Jafar + Marq Rasmussen + Jafar + Jafar - BSD + BSD - https://github.com/PickNikRobotics/topic_based_ros2_control - https://github.com/PickNikRobotics/topic_based_ros2_control/issues - https://github.com/PickNikRobotics/topic_based_ros2_control/ + + https://github.com/PickNikRobotics/topic_based_ros2_control + + + https://github.com/PickNikRobotics/topic_based_ros2_control/issues + + + https://github.com/PickNikRobotics/topic_based_ros2_control/ + - ament_cmakes + ament_cmake - angles - rclcpp - hardware_interface - sensor_msgs + angles + rclcpp + hardware_interface + sensor_msgs - ament_lint_auto - picknik_ament_copyright - ros2_control_test_assets - ament_lint_common + ament_lint_auto + ament_lint_common + controller_manager + joint_state_broadcaster + joint_trajectory_controller + launch + launch_ros + launch_testing + picknik_ament_copyright + rclpy + robot_state_publisher + ros2_control_test_assets + ros_testing + xacro - - ament_cmake - + + ament_cmake + diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..8d38d34 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,20 @@ +[tool.ruff] +extend-select = [ + # Enabled by default + # pyflakes + # "F", + # pycodestyle + # "E", + "W", + # isort + "I", + # NumPy-specific rules + "NPY", + # Ruff-specific rules + "RUF", +] +# line-length = 88 +ignore = ["E501"] +target-version = "py310" +[tool.ruff.pydocstyle] +convention = "google" diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index ee506dd..67a9ec9 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -91,6 +91,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& // Check the initial_value param is used if (!interface.initial_value.empty()) { + joint_states_[index][i] = std::stod(interface.initial_value); joint_commands_[index][i] = std::stod(interface.initial_value); } } diff --git a/test/control.launch.py b/test/control.launch.py new file mode 100644 index 0000000..7b44508 --- /dev/null +++ b/test/control.launch.py @@ -0,0 +1,70 @@ +# -*- coding: utf-8 -*- + +# Copyright 2024 PickNik 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 PickNik Inc. 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 +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 = Path(SCRIPT_PATH / "ros2_controllers.yaml") + robot_description = { + "robot_description": xacro.process_file(SCRIPT_PATH / "rrr.urdf.xacro").toxml(), + } + controllers = ["joint_state_broadcaster", "joint_trajectory_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], + ) + for controller in controllers + ], + ) diff --git a/test/ros2_control.test.py b/test/ros2_control.test.py new file mode 100644 index 0000000..be42148 --- /dev/null +++ b/test/ros2_control.test.py @@ -0,0 +1,92 @@ +# -*- coding: utf-8 -*- + +# Copyright 2024 PickNik 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 s 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 +from pathlib import Path + +import launch_testing +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + TimerAction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node + +SCRIPT_PATH = Path(os.path.realpath(__file__)).parent + + +def generate_test_description(): + test_node = Node( + executable=LaunchConfiguration("test_file"), + ) + + return LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + str(SCRIPT_PATH), + "control.launch.py", + ], + ), + ], + ), + ), + DeclareLaunchArgument("test_file"), + TimerAction(period=2.0, actions=[test_node]), + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ], + ), { + "test_node": test_node, + } + + +class TestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_run_complete(self, test_node): + self.proc_info.assertWaitForShutdown(test_node, timeout=4000.0) + + +@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, test_node): + launch_testing.asserts.assertExitCodes(proc_info, process=test_node) diff --git a/test/ros2_controllers.yaml b/test/ros2_controllers.yaml new file mode 100644 index 0000000..acdf09c --- /dev/null +++ b/test/ros2_controllers.yaml @@ -0,0 +1,25 @@ +controller_manager: + ros__parameters: + update_rate: 50 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_trajectory_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 diff --git a/test/rrr.urdf.xacro b/test/rrr.urdf.xacro new file mode 100644 index 0000000..c198417 --- /dev/null +++ b/test/rrr.urdf.xacro @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + topic_based_ros2_control/TopicBasedSystem + /topic_based_joint_commands + /topic_based_joint_states + + + + + 0.2 + + + + + + + 0.3 + + + + + + + 0.1 + + + + + diff --git a/test/test_topic_based_robot.py b/test/test_topic_based_robot.py new file mode 100755 index 0000000..7f2b600 --- /dev/null +++ b/test/test_topic_based_robot.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright 2024 PickNik 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 s 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 rclpy +from topic_based_robot import TopicBasedRobot + +rclpy.init() + +robot = TopicBasedRobot(["joint_1", "joint_2", "joint_3"]) +# By default the joint_states should have the values from initial_value from rrr.urdf.xacro +current_joint_state = robot.get_current_joint_state() +urdf_initial_values = [0.2, 0.3, 0.1] +assert ( + current_joint_state == urdf_initial_values +), f"{current_joint_state=} != {urdf_initial_values=}" + +# Test setting the robot joint states +joint_state = [0.1, 0.2, 0.3] +robot.set_joint_positions(joint_state) +current_joint_state = robot.get_current_joint_state() +assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" diff --git a/test/topic_based_robot.py b/test/topic_based_robot.py new file mode 100644 index 0000000..fc33a83 --- /dev/null +++ b/test/topic_based_robot.py @@ -0,0 +1,132 @@ +# -*- coding: utf-8 -*- + +# Copyright 2024 PickNik 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 s 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. + + +from collections import OrderedDict + +import numpy as np +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data, qos_profile_system_default +from sensor_msgs.msg import JointState + + +class TopicBasedRobot(Node): + """Topic Based Robot to test topic_based_ros2_control interface""" + + def __init__(self, joint_names: list[str]) -> None: + super().__init__("topic_based_robot") + self.joint_names = joint_names.copy() + self.last_joint_command = [] + self.current_joint_state = [] + self.callback_group = ReentrantCallbackGroup() + # Publisher for the robot internal joint state (Ground truth) + self.actual_joint_state_publisher = self.create_publisher( + JointState, + "topic_based_joint_states", + qos_profile=qos_profile_sensor_data, + callback_group=self.callback_group, + ) + # Subscriber for the desired joint state from the controller + self.desired_joint_state_subscriber = self.create_subscription( + JointState, + "topic_based_joint_commands", + self.command_callback, + QoSProfile(depth=1), + callback_group=self.callback_group, + ) + # Reported joint state from ros2_control + self.current_joint_state_subscriber = self.create_subscription( + JointState, + "joint_states", + self.joint_states_callback, + qos_profile_system_default, + callback_group=self.callback_group, + ) + + 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 + + def command_callback(self, msg: JointState): + self.last_joint_command = self.filter_joint_state_msg(msg) + + def joint_states_callback(self, msg: JointState): + self.current_joint_state = self.filter_joint_state_msg(msg) + + def get_current_joint_command(self) -> OrderedDict[str, float]: + """Get the last joint command sent to the robot.""" + self.last_joint_command = [] + while len(self.last_joint_command) == 0: + self.get_logger().warning( + f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.last_joint_command + + 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.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, timeout_sec=0.0) + return self.current_joint_state + + def set_joint_positions( + self, + joint_positions: list[float] | np.ndarray, + ) -> None: + """Set the joint positions of the robot.""" + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.joint_names), + position=joint_positions, + ), + ) + while not np.allclose( + self.get_current_joint_state(), + joint_positions, + atol=1e-3, + ): + rclpy.spin_once(self, timeout_sec=0.0) diff --git a/topic_based_ros2_control_plugin_description.xml b/topic_based_ros2_control_plugin_description.xml index 280f4dc..cbcd003 100644 --- a/topic_based_ros2_control_plugin_description.xml +++ b/topic_based_ros2_control_plugin_description.xml @@ -1,9 +1,9 @@ - - - - A topic based ros2_control hardware interface. - + + A topic based ros2_control hardware interface. -