diff --git a/README.md b/README.md index fa7a539c9..8633cd050 100644 --- a/README.md +++ b/README.md @@ -78,6 +78,10 @@ The following examples are part of this demo repository: This example shows how to integrate multiple robots under different controller manager instances. +* Example 16: ["DiffBot with Chained Controllers"](example_16) + + This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot. + ## Structure The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`. diff --git a/doc/index.rst b/doc/index.rst index bf5bb488e..1064bad96 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -85,6 +85,8 @@ Example 14: "Modular robots with actuators not providing states and with additio Example 15: "Using multiple controller managers" This example shows how to integrate multiple robots under different controller manager instances. +Example 16: "DiffBot with chained controllers" + This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot. .. _ros2_control_demos_install: @@ -280,3 +282,4 @@ Examples Example 13: Multiple robots <../example_13/doc/userdoc.rst> Example 14: Modular robots with actuators not providing states <../example_14/doc/userdoc.rst> Example 15: Using multiple controller managers <../example_15/doc/userdoc.rst> + Example 16: DiffBot with chained controllers <../example_16/doc/userdoc.rst> diff --git a/example_16/CMakeLists.txt b/example_16/CMakeLists.txt new file mode 100644 index 000000000..81ad5b39a --- /dev/null +++ b/example_16/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_16 LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# set the same behavior for windows as it is on linux +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# Specify the required version of ros2_control +find_package(controller_manager 4.0.0) +# Handle the case where the required version is not found +if(NOT controller_manager_FOUND) + message(FATAL_ERROR "ros2_control version 4.0.0 or higher is required. " + "Are you using the correct branch of the ros2_control_demos repository?") +endif() + +# find dependencies +find_package(backward_ros REQUIRED) +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +## COMPILE +add_library( + ros2_control_demo_example_16 + SHARED + hardware/diffbot_system.cpp +) +target_compile_features(ros2_control_demo_example_16 PUBLIC cxx_std_17) +target_include_directories(ros2_control_demo_example_16 PUBLIC +$ +$ +) +ament_target_dependencies( + ros2_control_demo_example_16 PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_16.xml) + +# INSTALL +install( + DIRECTORY hardware/include/ + DESTINATION include/ros2_control_demo_example_16 +) +install( + DIRECTORY description/launch description/ros2_control description/urdf + DESTINATION share/ros2_control_demo_example_16 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_16 +) +install(TARGETS ros2_control_demo_example_16 + EXPORT export_ros2_control_demo_example_16 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_16_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_16_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_16_launch test/test_diffbot_launch.py) +endif() + +## EXPORTS +ament_export_targets(export_ros2_control_demo_example_16 HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_16/README.md b/example_16/README.md new file mode 100644 index 000000000..fe2445172 --- /dev/null +++ b/example_16/README.md @@ -0,0 +1,6 @@ +# ros2_control_demo_example_16 + + *DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive. + The robot is basically a box moving according to differential drive kinematics. + +Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_16/doc/userdoc.html). diff --git a/example_16/bringup/config/diffbot_chained_controllers.yaml b/example_16/bringup/config/diffbot_chained_controllers.yaml new file mode 100644 index 000000000..991f517d0 --- /dev/null +++ b/example_16/bringup/config/diffbot_chained_controllers.yaml @@ -0,0 +1,92 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + pid_controller_left_wheel_joint: + type: pid_controller/PidController + + pid_controller_right_wheel_joint: + type: pid_controller/PidController + + diffbot_base_controller: + type: diff_drive_controller/DiffDriveController + + +pid_controller_left_wheel_joint: + ros__parameters: + + dof_names: + - left_wheel_joint + + command_interface: velocity + + reference_and_state_interfaces: + - velocity + + gains: + # control the velocity, no d term + left_wheel_joint: {"p": 0.5, "i": 2.5, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95} + +pid_controller_right_wheel_joint: + ros__parameters: + + dof_names: + - right_wheel_joint + + command_interface: velocity + + reference_and_state_interfaces: + - velocity + + gains: + # control the velocity, no d term + right_wheel_joint: {"p": 0.5, "i": 2.5, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95} + +diffbot_base_controller: + ros__parameters: + + left_wheel_names: ["pid_controller_left_wheel_joint/left_wheel_joint"] + right_wheel_names: ["pid_controller_right_wheel_joint/right_wheel_joint"] + + wheel_separation: 0.10 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.015 + + # we have velocity feedback + position_feedback: false + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + # set publish_limited_velocity to true for visualization + publish_limited_velocity: true + #velocity_rolling_window_size: 10 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 1.0 + linear.x.max_jerk: .NAN + linear.x.min_jerk: .NAN + + angular.z.max_velocity: 1.0 + angular.z.min_velocity: -1.0 + angular.z.max_acceleration: 1.0 + angular.z.min_acceleration: -1.0 + angular.z.max_jerk: .NAN + angular.z.min_jerk: .NAN diff --git a/example_16/bringup/config/plotjuggler.xml b/example_16/bringup/config/plotjuggler.xml new file mode 100644 index 000000000..0836de567 --- /dev/null +++ b/example_16/bringup/config/plotjuggler.xml @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_16/bringup/launch/demo_test.launch.py b/example_16/bringup/launch/demo_test.launch.py new file mode 100644 index 000000000..21d826c23 --- /dev/null +++ b/example_16/bringup/launch/demo_test.launch.py @@ -0,0 +1,40 @@ +# 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. + + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + + return LaunchDescription( + [ + ExecuteProcess( + cmd=[ + "python3", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_16"), + "launch", + "demo_test_helper.py", + ] + ), + ], + output="screen", + ) + ] + ) diff --git a/example_16/bringup/launch/demo_test_helper.py b/example_16/bringup/launch/demo_test_helper.py new file mode 100644 index 000000000..7bfc10b1d --- /dev/null +++ b/example_16/bringup/launch/demo_test_helper.py @@ -0,0 +1,80 @@ +# 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 time +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +from std_srvs.srv import SetBool + + +class DiffbotChainedControllersTest(Node): + def __init__(self): + super().__init__("diffbot_chained_controllers_demo_helper_node") + # Enable feedforward control via service call + self.client_left_ = self.create_client( + SetBool, "/pid_controller_left_wheel_joint/set_feedforward_control" + ) + self.client_right_ = self.create_client( + SetBool, "/pid_controller_right_wheel_joint/set_feedforward_control" + ) + self.publisher_ = self.create_publisher(TwistStamped, "/cmd_vel", 10) + + def set_feedforward_control(self): + while not self.client_left_.wait_for_service(timeout_sec=1.0): + self.get_logger().info( + "Waiting for left feedforward control service to be available..." + ) + while not self.client_right_.wait_for_service(timeout_sec=1.0): + self.get_logger().info( + "Waiting for right feedforward control service to be available..." + ) + + request_left = SetBool.Request() + request_left.data = True + future_left = self.client_left_.call_async(request_left) + + request_right = SetBool.Request() + request_right.data = True + future_right = self.client_right_.call_async(request_right) + + rclpy.spin_until_future_complete(self, future_left) + rclpy.spin_until_future_complete(self, future_right) + + self.get_logger().info("Enabled feedforward control for both wheels.") + + def publish_cmd_vel(self, delay=0.1): + + twist_msg = TwistStamped() + twist_msg.twist.linear.x = 0.7 + twist_msg.twist.linear.y = 0.0 + twist_msg.twist.linear.z = 0.0 + twist_msg.twist.angular.x = 0.0 + twist_msg.twist.angular.y = 0.0 + twist_msg.twist.angular.z = 1.0 + + while rclpy.ok(): + self.get_logger().info(f"Publishing twist message to cmd_vel: {twist_msg}") + self.publisher_.publish(twist_msg) + time.sleep(delay) + + +if __name__ == "__main__": + rclpy.init() + test_node = DiffbotChainedControllersTest() + test_node.set_feedforward_control() + test_node.publish_cmd_vel(delay=0.1) + rclpy.spin(test_node) + test_node.destroy_node() + rclpy.shutdown() diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py new file mode 100644 index 000000000..8552eb55a --- /dev/null +++ b/example_16/bringup/launch/diffbot.launch.py @@ -0,0 +1,171 @@ +# 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. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import ( + Command, + FindExecutable, + PathJoinSubstitution, + LaunchConfiguration, +) + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fixed_frame_id", + default_value="odom", + description="Fixed frame id of the robot.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + fixed_frame_id = LaunchConfiguration("fixed_frame_id") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_16"), "urdf", "diffbot.urdf.xacro"] + ), + " ", + "use_mock_hardware:=", + use_mock_hardware, + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_16"), + "config", + "diffbot_chained_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file, "-f", fixed_frame_id], + condition=IfCondition(gui), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ) + + pid_controllers_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "pid_controller_left_wheel_joint", + "pid_controller_right_wheel_joint", + "--param-file", + robot_controllers, + ], + ) + + robot_base_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "diffbot_base_controller", + "--param-file", + robot_controllers, + "--controller-ros-args", + "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", + ], + ) + + # Delay rviz start after `joint_state_broadcaster` + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + delay_robot_base_after_pid_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=pid_controllers_spawner, + on_exit=[robot_base_controller_spawner], + ) + ) + + # Delay start of joint_state_broadcaster after `robot_base_controller` + # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_robot_base_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_base_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + pid_controllers_spawner, + delay_robot_base_after_pid_controller_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_joint_state_broadcaster_after_robot_base_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_16/description/launch/view_robot.launch.py b/example_16/description/launch/view_robot.launch.py new file mode 100644 index 000000000..a48efbec7 --- /dev/null +++ b/example_16/description/launch/view_robot.launch.py @@ -0,0 +1,114 @@ +# 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. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="diffbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + gui = LaunchConfiguration("gui") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_16"), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + # start rviz2 with initial fixed frame id as base_link + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file, "-f", "base_link"], + condition=IfCondition(gui), + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_16/description/ros2_control/diffbot.ros2_control.xacro b/example_16/description/ros2_control/diffbot.ros2_control.xacro new file mode 100644 index 000000000..341df177e --- /dev/null +++ b/example_16/description/ros2_control/diffbot.ros2_control.xacro @@ -0,0 +1,34 @@ + + + + + + + + + ros2_control_demo_example_16/DiffBotSystemHardware + 0 + 3.0 + + + + + mock_components/GenericSystem + true + + + + + + + + + + + + + + + + + diff --git a/example_16/description/urdf/diffbot.urdf.xacro b/example_16/description/urdf/diffbot.urdf.xacro new file mode 100644 index 000000000..f11dc0844 --- /dev/null +++ b/example_16/description/urdf/diffbot.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + diff --git a/example_16/doc/diffbot_velocities.png b/example_16/doc/diffbot_velocities.png new file mode 100644 index 000000000..c4991b8e3 Binary files /dev/null and b/example_16/doc/diffbot_velocities.png differ diff --git a/example_16/doc/userdoc.rst b/example_16/doc/userdoc.rst new file mode 100644 index 000000000..0ef6b3740 --- /dev/null +++ b/example_16/doc/userdoc.rst @@ -0,0 +1,249 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_16/doc/userdoc.rst + +.. _ros2_control_demos_example_16_userdoc: + +******************************** +DiffBot with Chained Controllers +******************************** + +This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot. It extends *example_2*. If you haven't already, you can find the instructions for *example_2* in :ref:`ros2_control_demos_example_2_userdoc`. It is recommended to follow the steps given in that tutorial first before proceeding with this one. + +This example demonstrates controller chaining as described in :ref:`controller_chaining`. The control chain flows from the diff_drive_controller through two PID controllers to the DiffBot hardware. The diff_drive_controller converts desired robot twist into wheel velocity commands, which are then processed by the PID controllers to directly control the wheel velocities. Additionally, this example shows how to enable the feedforward mode for the PID controllers. + +Furthermore, this example shows how to use plotjuggler to visualize the controller states. + +The *DiffBot* URDF files can be found in ``description/urdf`` folder. + +.. include:: ../../doc/run_from_docker.rst + + +Tutorial steps +-------------------------- + +1. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_16 diffbot.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz*. + In the starting terminal you will see a lot of output from the hardware implementation showing its internal states. + This excessive printing is only added for demonstration. In general, printing to the terminal should be avoided as much as possible in a hardware interface implementation. + + If you can see an orange box in *RViz* everything has started properly. Let's introspect the control system before moving *DiffBot*. + +2. Check controllers + + .. code-block:: shell + + ros2 control list_controllers + + You should get + + .. code-block:: shell + + joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active + diffbot_base_controller diff_drive_controller/DiffDriveController active + pid_controller_right_wheel_joint pid_controller/PidController active + pid_controller_left_wheel_joint pid_controller/PidController active + +3. Check the hardware interface loaded by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + You should get + + .. code-block:: shell + + command interfaces + diffbot_base_controller/angular/velocity [unavailable] [unclaimed] + diffbot_base_controller/linear/velocity [unavailable] [unclaimed] + left_wheel_joint/velocity [available] [claimed] + pid_controller_left_wheel_joint/left_wheel_joint/velocity [available] [unclaimed] + pid_controller_right_wheel_joint/right_wheel_joint/velocity [available] [unclaimed] + right_wheel_joint/velocity [available] [claimed] + state interfaces + left_wheel_joint/position + left_wheel_joint/velocity + pid_controller_left_wheel_joint/left_wheel_joint/velocity + pid_controller_right_wheel_joint/right_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity + + + The ``[claimed]`` marker on command interfaces means that a controller has access to command *DiffBot*. There are two ``[claimed]`` interfaces from pid_controller, one for left wheel and one for right wheel. These interfaces are referenced by diff_drive_controller. By referencing them, diff_drive_controller can send commands to these interfaces. If you see these, we've successfully chained the controllers. + + There are also two ``[unclaimed]`` interfaces from diff_drive_controller, one for angular velocity and one for linear velocity. These are provided by the diff_drive_controller because it is chainable. You can ignore them since we don't use them in this example. + +4. We specified ``feedforward_gain`` as part of ``gains`` in diffbot_chained_controllers.yaml. To actually enable feedforward mode for the pid_controller, we need to use a service provided by pid_controller. Let's enable it. + + .. code-block:: shell + + ros2 service call /pid_controller_left_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" && \ + ros2 service call /pid_controller_right_wheel_joint/set_feedforward_control std_srvs/srv/SetBool "data: true" + + You should get + + .. code-block:: shell + + response: + std_srvs.srv.SetBool_Response(success=True, message='') + +5. To see the pid_controller in action, let's subscribe to the controler_state topic, e.g. pid_controller_left_wheel_joint/controller_state topic. + + .. code-block:: shell + + ros2 topic echo /pid_controller_left_wheel_joint/controller_state + +6. Now we are ready to send a command to move the robot. Send a command to *Diff Drive Controller* by opening another terminal and executing + + .. code-block:: shell + + ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " + twist: + linear: + x: 0.7 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 1.0" + + You should now see robot is moving in circles in *RViz*. + +7. In the terminal where launch file is started, you should see the commands being sent to the wheels and how they are gradually stabilizing to the target velocity similar to following output. + + .. code-block:: shell + + [ros2_control_node-1] [WARN] [1739860498.298085087] [diffbot_base_controller]: Received TwistStamped with zero timestamp, setting it to current time, this message will only be shown once + [ros2_control_node-1] [INFO] [1739860498.634431841] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Reading states: + [ros2_control_node-1] position 5.78 and velocity 27.52 for 'right_wheel_joint/position'! + [ros2_control_node-1] position 5.23 and velocity 24.90 for 'left_wheel_joint/position'! + [ros2_control_node-1] [INFO] [1739860498.634800954] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 35.55 for 'right_wheel_joint/velocity'! + [ros2_control_node-1] command 32.16 for 'left_wheel_joint/velocity'! + [ros2_control_node-1] [INFO] [1739860499.234393780] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Reading states: + [ros2_control_node-1] position 36.99 and velocity 60.61 for 'right_wheel_joint/position'! + [ros2_control_node-1] position 33.19 and velocity 53.17 for 'left_wheel_joint/position'! + [ros2_control_node-1] [INFO] [1739860499.234812092] [controller_manager.resource_manager.hardware_component.system.DiffBot]: Writing commands: + [ros2_control_node-1] command 60.53 for 'right_wheel_joint/velocity'! + [ros2_control_node-1] command 52.27 for 'left_wheel_joint/velocity'! + + +8. Let's go back to the terminal where we subscribed to the controller_state topic and see the changing states. + + .. code-block:: shell + + --- + header: + stamp: + sec: 1739860821 + nanosec: 314185285 + frame_id: '' + dof_states: + - name: left_wheel_joint + reference: 18.967273133333336 + feedback: 13.294059091678035 + feedback_dot: 0.0 + error: 5.673214041655301 + error_dot: 0.0 + time_step: 0.100077098 + output: 24.857442270936623 + --- + header: + stamp: + sec: 1739860821 + nanosec: 414126639 + frame_id: '' + dof_states: + - name: left_wheel_joint + reference: 25.296198783333335 + feedback: 19.8859538167493 + feedback_dot: 0.0 + error: 5.410244966584035 + error_dot: 0.0 + time_step: 0.099941354 + output: 32.090205119481226 + + +Visualize the convergence of DiffBot's wheel velocities and commands +--------------------------------------------------------------------- + +In the section below, we will use *plotjuggler* to observe the convergence of DiffBot's wheel velocities and commands from PID controllers. + +*plotjuggler* is an open-source data visualization tool and widely embraced by ROS2 community. If you don't have it installed, you can find the instructions from `plotjuggler website `__. + + +Before we proceed, we stop all previous steps from terminal and start from the beginning. + +1. To start *DiffBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_16 diffbot.launch.py + +Like before, if you can see an orange box in *RViz*, everything has started properly. + +2. To start the plotjuggler with a provided layout file(plotjuggler.xml), open another terminal and run following command. + + .. code-block:: shell + + ros2 run plotjuggler plotjuggler --layout $(ros2 pkg prefix ros2_control_demo_example_16 --share)/config/plotjuggler.xml + +After this, you will see a few dialogs popping up. For example: + + .. code-block:: shell + + Start the previously used streaming plugin? + + ROS2 Topic Subscriber + +Click 'Yes' for the first dialog and 'OK" to the following two dialogs, then you will see the plotjuggler window. + +3. To enable feedforward mode and published a command to move the robot, instead of doing these manually, we will use the demo_test.launch.py. Open another terminal and execute + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_16 demo_test.launch.py + +4. From the plotjuggler, you can see the controllers' states and commands being plotted, similar to following figure. From the figure, the DiffBot's wheel velocities and commands from PID controllers are converged to the target velocity fairly quickly. + + .. image:: diffbot_velocities.png + :width: 400 + :alt: Plotjuggler visualization of DiffBot velocities and commands + +5. Change the ``gains`` in the ``diffbot_chained_controllers.yaml`` file with some different values, repeat above steps and observe its effect to the pid_controller commands. For example, to change the ``feedforward_gain`` of the right wheel to 0.50, you can use the following command: + + .. code-block:: shell + + ros2 param set /pid_controller_right_wheel_joint gains.right_wheel_joint.feedforward_gain 0.50 + + +Files used for this demo +-------------------------- + +* Launch file: `diffbot.launch.py `__ +* Controllers yaml: `diffbot_chained_controllers.yaml `__ +* URDF file: `diffbot.urdf.xacro `__ + + * Description: `diffbot_description.urdf.xacro `__ + * ``ros2_control`` tag: `diffbot.ros2_control.xacro `__ + +* RViz configuration: `diffbot.rviz `__ + +* Hardware interface plugin: `diffbot_system.cpp `__ + +* Demo helper utility: + + + demo test helper node: `demo_test_helper.py `__ + + demo test launch file: `demo_test.launch.py `__ + +Controllers from this demo +-------------------------- + +* ``Joint State Broadcaster`` (`ros2_controllers repository `__): :ref:`doc ` +* ``Diff Drive Controller`` (`ros2_controllers repository `__): :ref:`doc ` +* ``pid_controller`` (`ros2_controllers repository `__): :ref:`doc ` diff --git a/example_16/hardware/diffbot_system.cpp b/example_16/hardware/diffbot_system.cpp new file mode 100644 index 000000000..6415a7d5a --- /dev/null +++ b/example_16/hardware/diffbot_system.cpp @@ -0,0 +1,220 @@ +// 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. + +#include "ros2_control_demo_example_16/diffbot_system.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_16 +{ +hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = + hardware_interface::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = + hardware_interface::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // DiffBotSystem has exactly two states and one command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' have '%s' as first state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) + { + RCLCPP_FATAL( + get_logger(), "Joint '%s' have '%s' as second state interface. '%s' expected.", + joint.name.c_str(), joint.state_interfaces[1].name.c_str(), + hardware_interface::HW_IF_VELOCITY); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) + { + set_state(name, 0.0); + } + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } + RCLCPP_INFO(get_logger(), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Activating ...please wait..."); + + for (auto i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // command and state should be equal when starting + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, get_state(name)); + } + + RCLCPP_INFO(get_logger(), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(get_logger(), "Deactivating ...please wait..."); + + for (auto i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_stop_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type DiffBotSystemHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Reading states:"; + ss << std::fixed << std::setprecision(2); + for (const auto & [name, descr] : joint_state_interfaces_) + { + if (descr.get_interface_name() == hardware_interface::HW_IF_POSITION) + { + // Update the joint status: this is a revolute joint without any limit. + // Simply integrates + auto velo = get_command(descr.get_prefix_name() + "/" + hardware_interface::HW_IF_VELOCITY); + set_state(name, get_state(name) + period.seconds() * velo); + + ss << std::endl + << "\t position " << get_state(name) << " and velocity " << velo << " for '" << name + << "'!"; + } + } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ros2_control_demo_example_16 ::DiffBotSystemHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + std::stringstream ss; + ss << "Writing commands:"; + for (const auto & [name, descr] : joint_command_interfaces_) + { + // Simulate sending commands to the hardware with a slow down factor + // to show-case the PID action + set_state(name, get_command(name) * 0.8); + + ss << std::fixed << std::setprecision(2) << std::endl + << "\t" << "command " << get_command(name) << " for '" << name << "'!"; + } + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_16 + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_16::DiffBotSystemHardware, hardware_interface::SystemInterface) diff --git a/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp b/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp new file mode 100644 index 000000000..495098a6e --- /dev/null +++ b/example_16/hardware/include/ros2_control_demo_example_16/diffbot_system.hpp @@ -0,0 +1,66 @@ +// 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. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_16__DIFFBOT_SYSTEM_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_16__DIFFBOT_SYSTEM_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace ros2_control_demo_example_16 +{ +class DiffBotSystemHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) 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; + +private: + // Parameters for the DiffBot simulation + double hw_start_sec_; + double hw_stop_sec_; +}; + +} // namespace ros2_control_demo_example_16 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_16__DIFFBOT_SYSTEM_HPP_ diff --git a/example_16/package.xml b/example_16/package.xml new file mode 100644 index 000000000..b4901ad25 --- /dev/null +++ b/example_16/package.xml @@ -0,0 +1,47 @@ + + + + ros2_control_demo_example_16 + 0.0.0 + Demo package of `ros2_control` hardware for DiffBot. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + Sai Kishor Kothakota + + Apache-2.0 + + Julia Jia + + ament_cmake + + backward_ros + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + diff_drive_controller + pid_controller + joint_state_broadcaster + joint_state_publisher_gui + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_pytest + launch_testing + launch + liburdfdom-tools + rclpy + + + ament_cmake + + diff --git a/example_16/ros2_control_demo_example_16.xml b/example_16/ros2_control_demo_example_16.xml new file mode 100644 index 000000000..4508456d1 --- /dev/null +++ b/example_16/ros2_control_demo_example_16.xml @@ -0,0 +1,9 @@ + + + + The ros2_control DiffBot example using a system hardware interface-type. It uses velocity command and position state interface. The example is the starting point to implement a hardware interface for differential-drive mobile robots. + + + diff --git a/example_16/test/test_diffbot_launch.py b/example_16/test/test_diffbot_launch.py new file mode 100644 index 000000000..b2fc0c2bb --- /dev/null +++ b/example_16/test/test_diffbot_launch.py @@ -0,0 +1,91 @@ +# 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 pytest +import unittest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + +import launch_testing.markers +import rclpy +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running, +) + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_16"), + "launch/diffbot.launch.py", + ) + ), + launch_arguments={"gui": "False"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) + + +# This is our test fixture. Each method is a test case. +# These run alongside the processes specified in generate_test_description() +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") + + def tearDown(self): + self.node.destroy_node() + + def test_node_start(self, proc_output): + check_node_running(self.node, "robot_state_publisher") + + def test_controller_running(self, proc_output): + cnames = [ + "pid_controller_left_wheel_joint", + "pid_controller_right_wheel_joint", + "diffbot_base_controller", + "joint_state_broadcaster", + ] + + check_controllers_running(self.node, cnames) + + def test_check_if_msgs_published(self): + check_if_js_published("/joint_states", ["left_wheel_joint", "right_wheel_joint"]) + + +@launch_testing.post_shutdown_test() +# These tests are run after the processes in generate_test_description() have shutdown. +class TestDescriptionCraneShutdown(unittest.TestCase): + + def test_exit_codes(self, proc_info): + """Check if the processes exited normally.""" + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/example_16/test/test_urdf_xacro.py b/example_16/test/test_urdf_xacro.py new file mode 100644 index 000000000..744e0e0d4 --- /dev/null +++ b/example_16/test/test_urdf_xacro.py @@ -0,0 +1,77 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# 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. +# +# Author: Lukas Sackewitz + +import os +import shutil +import subprocess +import tempfile + +from ament_index_python.packages import get_package_share_directory + + +def test_urdf_xacro(): + # General Arguments + description_package = "ros2_control_demo_example_16" + description_file = "diffbot.urdf.xacro" + + description_file_path = os.path.join( + get_package_share_directory(description_package), "urdf", description_file + ) + + (_, tmp_urdf_output_file) = tempfile.mkstemp(suffix=".urdf") + + # Compose `xacro` and `check_urdf` command + xacro_command = ( + f"{shutil.which('xacro')}" f" {description_file_path}" f" > {tmp_urdf_output_file}" + ) + check_urdf_command = f"{shutil.which('check_urdf')} {tmp_urdf_output_file}" + + # Try to call processes but finally remove the temp file + try: + xacro_process = subprocess.run( + xacro_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert xacro_process.returncode == 0, " --- XACRO command failed ---" + + check_urdf_process = subprocess.run( + check_urdf_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True + ) + + assert ( + check_urdf_process.returncode == 0 + ), "\n --- URDF check failed! --- \nYour xacro does not unfold into a proper urdf robot description. Please check your xacro file." + + finally: + os.remove(tmp_urdf_output_file) + + +if __name__ == "__main__": + test_urdf_xacro() diff --git a/example_16/test/test_view_robot_launch.py b/example_16/test/test_view_robot_launch.py new file mode 100644 index 000000000..bd1e87ff6 --- /dev/null +++ b/example_16/test/test_view_robot_launch.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022 FZI Forschungszentrum Informatik +# +# 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. +# +# Author: Lukas Sackewitz + +import os +import pytest + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest + + +# Executes the given launch file and checks if all nodes can be started +@pytest.mark.rostest +def generate_test_description(): + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros2_control_demo_example_16"), + "launch/view_robot.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()])