diff --git a/README.md b/README.md index 390feae4d..839a99a88 100644 --- a/README.md +++ b/README.md @@ -85,6 +85,10 @@ The following examples are part of this demo repository: This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot. +* Example 17: ["DiffBot with Chained Controllers using effort interface"](example_17) + + This example shows how to create chained controllers using diff_drive_controller and two pid_controllers to control a differential drive robot using effort interface. + ## Structure The repository is structured into `example_XY` folders that fully contained packages with names `ros2_control_demos_example_XY`. diff --git a/example_17/CMakeLists.txt b/example_17/CMakeLists.txt new file mode 100644 index 000000000..1efc53296 --- /dev/null +++ b/example_17/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_17) + +# INSTALL +install( + DIRECTORY description/gazebo description/launch description/ros2_control description/urdf + DESTINATION share/ros2_control_demo_example_17 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_17 +) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(example_17_urdf_xacro test/test_urdf_xacro.py) + ament_add_pytest_test(view_example_17_launch test/test_view_robot_launch.py) + ament_add_pytest_test(run_example_17_launch test/test_diffbot_launch.py) +endif() + +ament_package() diff --git a/example_17/README.md b/example_17/README.md new file mode 100644 index 000000000..b83766bf6 --- /dev/null +++ b/example_17/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_17 + + *DiffBot*, or ''Differential Mobile Robot'', is a simple mobile base with differential drive. This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot using effort interface. + +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_17/doc/userdoc.html). diff --git a/example_17/bringup/config/diffbot_chained_controllers.yaml b/example_17/bringup/config/diffbot_chained_controllers.yaml new file mode 100644 index 000000000..53c971871 --- /dev/null +++ b/example_17/bringup/config/diffbot_chained_controllers.yaml @@ -0,0 +1,85 @@ +controller_manager: + ros__parameters: + use_sim_time: True # remove on real robots + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + wheel_pids: + type: pid_controller/PidController + + diffbot_base_controller: + type: diff_drive_controller/DiffDriveController + +joint_state_broadcaster: + ros__parameters: + use_sim_time: True # remove on real robots + update_rate: 100 # Hz + +wheel_pids: + ros__parameters: + use_sim_time: True # remove on real robots + update_rate: 100 # Hz + + dof_names: + - left_wheel_joint + - right_wheel_joint + + command_interface: effort + + reference_and_state_interfaces: + - velocity + - effort + + gains: + # control the velocity through effort + left_wheel_joint: {"p": 0.1, "i": 1.0, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95} + right_wheel_joint: {"p": 0.1, "i": 1.0, "d": 0.0, "i_clamp_min": -20.0, "i_clamp_max": 20.0, "antiwindup": true, "feedforward_gain": 0.95} + +diffbot_base_controller: + ros__parameters: + use_sim_time: True # remove on real robots + update_rate: 100 # Hz + + left_wheel_names: ["wheel_pids/left_wheel_joint"] + right_wheel_names: ["wheel_pids/right_wheel_joint"] + + wheel_separation: 0.10 + wheel_radius: 0.015 + + # we have velocity feedback + position_feedback: false + + 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: false + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + publish_limited_velocity: true + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + 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.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + 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_17/bringup/config/plotjuggler.xml b/example_17/bringup/config/plotjuggler.xml new file mode 100644 index 000000000..b60278517 --- /dev/null +++ b/example_17/bringup/config/plotjuggler.xml @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_17/bringup/launch/demo_test.launch.py b/example_17/bringup/launch/demo_test.launch.py new file mode 100644 index 000000000..bab5f8f23 --- /dev/null +++ b/example_17/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_17"), + "launch", + "demo_test_helper.py", + ] + ), + ], + output="screen", + ) + ] + ) diff --git a/example_17/bringup/launch/demo_test_helper.py b/example_17/bringup/launch/demo_test_helper.py new file mode 100644 index 000000000..933d9de75 --- /dev/null +++ b/example_17/bringup/launch/demo_test_helper.py @@ -0,0 +1,64 @@ +# 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_ = self.create_client(SetBool, "/wheel_pids/set_feedforward_control") + self.publisher_ = self.create_publisher(TwistStamped, "/cmd_vel", 10) + + def set_feedforward_control(self): + while not self.client_.wait_for_service(timeout_sec=1.0): + self.get_logger().info("Waiting for feedforward control service to be available...") + + request = SetBool.Request() + request.data = True + future = self.client_.call_async(request) + + rclpy.spin_until_future_complete(self, future) + + 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_17/bringup/launch/diffbot.launch.py b/example_17/bringup/launch/diffbot.launch.py new file mode 100644 index 000000000..1fd0e9626 --- /dev/null +++ b/example_17/bringup/launch/diffbot.launch.py @@ -0,0 +1,207 @@ +# 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, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +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( + "gazebo_gui", + default_value="true", + description="Whether to start Gazebo with GUI.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fixed_frame_id", + default_value="odom", + description="Fixed frame id of the robot.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + gazebo_gui = LaunchConfiguration("gazebo_gui") + fixed_frame_id = LaunchConfiguration("fixed_frame_id") + + # gazebo + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"] + ), + launch_arguments=[("gz_args", " -r -v 3 empty.sdf")], + condition=IfCondition(gazebo_gui), + ) + gazebo_headless = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"] + ), + launch_arguments=[("gz_args", ["--headless-rendering -s -r -v 3 empty.sdf"])], + condition=UnlessCondition(gazebo_gui), + ) + + # Gazebo bridge + gazebo_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ) + + gz_spawn_entity = Node( + package="ros_gz_sim", + executable="create", + output="screen", + arguments=[ + "-topic", + "/robot_description", + "-name", + "diffdrive_robot", + "-z", + "0.02", + ], + ) + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_17"), "urdf", "diffbot.urdf.xacro"] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_17"), + "config", + "diffbot_chained_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"] + ) + + 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", + "--param-file", + robot_controllers, + ], + ) + + pid_controllers_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "wheel_pids", + "--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 = [ + gazebo, + gazebo_headless, + gazebo_bridge, + gz_spawn_entity, + 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_17/description/gazebo/diffbot.gazebo.xacro b/example_17/description/gazebo/diffbot.gazebo.xacro new file mode 100644 index 000000000..06e786170 --- /dev/null +++ b/example_17/description/gazebo/diffbot.gazebo.xacro @@ -0,0 +1,77 @@ + + + + + + + + + $(find ros2_control_demo_example_17)/config/diffbot_chained_controllers.yaml + + + + + + + + + + + + + 1.0 + 1.0 + + + + + + + +:q! + + + + 1.0 + 1.0 + + + + + + + + 0.0 + 0.0 + + + + + + + + 0.0 + 0.0 + + + + + + + + diff --git a/example_17/description/launch/view_robot.launch.py b/example_17/description/launch/view_robot.launch.py new file mode 100644 index 000000000..66793b01c --- /dev/null +++ b/example_17/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_17"), "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_17/description/ros2_control/diffbot.ros2_control.xacro b/example_17/description/ros2_control/diffbot.ros2_control.xacro new file mode 100644 index 000000000..11906e488 --- /dev/null +++ b/example_17/description/ros2_control/diffbot.ros2_control.xacro @@ -0,0 +1,26 @@ + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + + + diff --git a/example_17/description/urdf/diffbot.urdf.xacro b/example_17/description/urdf/diffbot.urdf.xacro new file mode 100644 index 000000000..0a9661d63 --- /dev/null +++ b/example_17/description/urdf/diffbot.urdf.xacro @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/example_17/doc/diffbot_velocities.png b/example_17/doc/diffbot_velocities.png new file mode 100644 index 000000000..33bae96ef Binary files /dev/null and b/example_17/doc/diffbot_velocities.png differ diff --git a/example_17/doc/userdoc.rst b/example_17/doc/userdoc.rst new file mode 100644 index 000000000..6a9e8509a --- /dev/null +++ b/example_17/doc/userdoc.rst @@ -0,0 +1,225 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_17/doc/userdoc.rst + +.. _ros2_control_demos_example_17_userdoc: + +******************************************************** +DiffBot with Chained Controllers using effort interface. +******************************************************** + +This example shows how to create chained controllers using diff_drive_controller and pid_controllers to control a differential drive robot using effort interfaces. In contrast to *example_16* which uses velocity interface, control via effort interface is very suitable for simulation because it does not break physics (velocity-controlled objects are not affected by inertia etc.). If you haven't already, you can find the instructions for *example_16* in :ref:`ros2_control_demos_example_16_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 control the wheel motors' effort. 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_17 diffbot.launch.py + + The launch file loads and starts the robot hardware, controllers and opens *RViz* and *Gazebo GUI*. + In the starting terminal you will see output from the hardware implementation and simulator. + + If you can see an orange box in *RViz* and *Gazebo*, everything has started properly (the robot is pretty small, so make sure to zoom in on the robot in *Gazebo*). 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 + wheel_pids 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 [available] [unclaimed] + diffbot_base_controller/linear/velocity [available] [unclaimed] + left_wheel_joint/effort [available] [claimed] + right_wheel_joint/effort [available] [claimed] + wheel_pids/left_wheel_joint/effort [available] [unclaimed] + wheel_pids/left_wheel_joint/velocity [available] [claimed] + wheel_pids/right_wheel_joint/effort [available] [unclaimed] + wheel_pids/right_wheel_joint/velocity [available] [claimed] + state interfaces + left_wheel_joint/effort + left_wheel_joint/position + left_wheel_joint/velocity + right_wheel_joint/effort + right_wheel_joint/position + right_wheel_joint/velocity + wheel_pids/left_wheel_joint/effort + wheel_pids/left_wheel_joint/velocity + wheel_pids/right_wheel_joint/effort + wheel_pids/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 four ``[unclaimed]`` interfaces from diff_drive_controller, wheel_pids. 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 /wheel_pids/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. wheel_pids/controller_state topic. + + .. code-block:: shell + + ros2 topic echo /wheel_pids/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* and *Gazebo*. + +7. 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: 307 + nanosec: 909000000 + frame_id: '' + dof_states: + - name: left_wheel_joint + reference: 43.33333333333333 + feedback: 43.309162040088395 + feedback_dot: -0.00912764403084563 + error: 0.024171293244933167 + error_dot: .nan + time_step: 0.01 + output: 8.598921377572319 + - name: right_wheel_joint + reference: 50.0 + feedback: 50.029347324692814 + feedback_dot: 0.008966862676684506 + error: -0.029347324692814425 + error_dot: .nan + time_step: 0.01 + output: 10.067540181319677 + + +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_17 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_17 --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_17 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: 600 + :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 /wheel_pids 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 `__ + * ``gazebo`` tags: `diffbot.gazebo.xacro `__ + +* RViz configuration: `diffbot.rviz `__ + +* 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_17/package.xml b/example_17/package.xml new file mode 100644 index 000000000..2c64ee5d4 --- /dev/null +++ b/example_17/package.xml @@ -0,0 +1,44 @@ + + + + ros2_control_demo_example_17 + 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 + + Martin Pecka + + ament_cmake + + controller_manager + diff_drive_controller + gz_ros2_control + joint_state_broadcaster + joint_state_publisher_gui + pid_controller + robot_state_publisher + ros2_control_demo_description + ros2_controllers_test_nodes + ros2controlcli + ros2launch + ros_gz_bridge + ros_gz_sim + rviz2 + xacro + + ament_cmake_pytest + launch_testing + launch + liburdfdom-tools + rclpy + + + ament_cmake + + diff --git a/example_17/test/test_diffbot_launch.py b/example_17/test/test_diffbot_launch.py new file mode 100644 index 000000000..4c7a25d74 --- /dev/null +++ b/example_17/test/test_diffbot_launch.py @@ -0,0 +1,98 @@ +# 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 psutil +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 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_17"), + "launch/diffbot.launch.py", + ) + ), + launch_arguments={"gui": "False", "gazebo_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): + for proc in psutil.process_iter(): + # check whether the process name matches + if proc.name() == "ruby": + proc.kill() + if "gz sim" in proc.name(): + proc.kill() + 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 = [ + "wheel_pids", + "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"]) + + +# deactivating because gazebo returns -15 and does not stop properly +# @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_17/test/test_urdf_xacro.py b/example_17/test/test_urdf_xacro.py new file mode 100644 index 000000000..b3abe0ee8 --- /dev/null +++ b/example_17/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_17" + 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_17/test/test_view_robot_launch.py b/example_17/test/test_view_robot_launch.py new file mode 100644 index 000000000..7d37de610 --- /dev/null +++ b/example_17/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_17"), + "launch/view_robot.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) diff --git a/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro b/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro index a5be5c6ac..8af6ff52c 100644 --- a/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro +++ b/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro @@ -49,7 +49,7 @@ - + @@ -85,7 +85,7 @@ - +