diff --git a/README.md b/README.md index 513b2fc5d..1047a7c90 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,12 @@ The following examples are part of this demo repository: * Example 14: ["Modular robots with actuators not providing states and with additional sensors"](example_14) + The example shows how to implement robot hardware with actuators not providing states and with additional sensors. + +* Example 15: ["Using multiple controller managers"](example_15) + + This example shows how to integrate multiple robots under different controller manager instances. + ## 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 bfce9e6bc..9dc42afac 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -79,6 +79,10 @@ Example 12: "Controller chaining" Example 13: "Multi-robot example (tba.)" Example 14: "Modular robots with actuators not providing states and with additional sensors" + The example shows how to implement robot hardware with actuators not providing states and with additional sensors. + +Example 15: "Using multiple controller managers" + This example shows how to integrate multiple robots under different controller manager instances. .. _ros2_control_demos_install: @@ -273,3 +277,4 @@ Examples Example 11: CarlikeBot <../example_11/doc/userdoc.rst> Example 12: Controller chaining <../example_12/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> diff --git a/example_15/CMakeLists.txt b/example_15/CMakeLists.txt new file mode 100644 index 000000000..84e220a57 --- /dev/null +++ b/example_15/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_15 LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# INSTALL +install( + DIRECTORY description/rviz + DESTINATION share/ros2_control_demo_example_15 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_15 +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + ament_add_pytest_test(test_rrbot_namespace_launch test/test_rrbot_namespace_launch.py) + ament_add_pytest_test(test_multi_controller_manager_launch test/test_multi_controller_manager_launch.py) + +endif() + +## EXPORTS +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_15/README.md b/example_15/README.md new file mode 100644 index 000000000..010dc0b7e --- /dev/null +++ b/example_15/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_15 + + This example shows how to integrate multiple robots under different controller manager instances. + +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_15/doc/userdoc.html). diff --git a/example_15/bringup/config/multi_controller_manager_forward_position_publisher.yaml b/example_15/bringup/config/multi_controller_manager_forward_position_publisher.yaml new file mode 100644 index 000000000..0a3afff56 --- /dev/null +++ b/example_15/bringup/config/multi_controller_manager_forward_position_publisher.yaml @@ -0,0 +1,25 @@ +/rrbot_1/publisher_forward_position_controller: + ros__parameters: + + + publish_topic: "forward_position_controller/commands" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0.0, 0.0] + pos3: [-0.785, -0.785] + pos4: [0.0, 0.0] + + +/rrbot_2/publisher_forward_position_controller: + ros__parameters: + + publish_topic: "forward_position_controller/commands" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [-0.785, 0.0] + pos2: [0.0, -0.785] + pos3: [+0.785, -1.57] + pos4: [+1.57, -0.785] diff --git a/example_15/bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml b/example_15/bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml new file mode 100644 index 000000000..3792fa61e --- /dev/null +++ b/example_15/bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml @@ -0,0 +1,50 @@ +/rrbot_1/publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "rrbot_1/position_trajectory_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - rrbot_1_joint1 + - rrbot_1_joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] + + +/rrbot_2/publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "rrbot_2/position_trajectory_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [-0.785, 0.0] + pos2: + positions: [0.0, -0.785] + pos3: + positions: [+0.785, -1.57] + pos4: + positions: [+1.57, -0.785] + + joints: + - rrbot_2_joint1 + - rrbot_2_joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/example_15/bringup/config/multi_controller_manager_rrbot_1_controllers.yaml b/example_15/bringup/config/multi_controller_manager_rrbot_1_controllers.yaml new file mode 100644 index 000000000..784c8d5ba --- /dev/null +++ b/example_15/bringup/config/multi_controller_manager_rrbot_1_controllers.yaml @@ -0,0 +1,43 @@ +/rrbot_1/controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +/rrbot_1/forward_position_controller: + ros__parameters: + joints: + - rrbot_1_joint1 + - rrbot_1_joint2 + interface_name: position + + +/rrbot_1/position_trajectory_controller: + ros__parameters: + joints: + - rrbot_1_joint1 + - rrbot_1_joint2 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 200.0 # Defaults to 50 + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/example_15/bringup/config/multi_controller_manager_rrbot_2_controllers.yaml b/example_15/bringup/config/multi_controller_manager_rrbot_2_controllers.yaml new file mode 100644 index 000000000..a987e786a --- /dev/null +++ b/example_15/bringup/config/multi_controller_manager_rrbot_2_controllers.yaml @@ -0,0 +1,43 @@ +/rrbot_2/controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +/rrbot_2/forward_position_controller: + ros__parameters: + joints: + - rrbot_2_joint1 + - rrbot_2_joint2 + interface_name: position + + +/rrbot_2/position_trajectory_controller: + ros__parameters: + joints: + - rrbot_2_joint1 + - rrbot_2_joint2 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 200.0 # Defaults to 50 + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/example_15/bringup/config/rrbot_namespace_controllers.yaml b/example_15/bringup/config/rrbot_namespace_controllers.yaml new file mode 100644 index 000000000..dd97d34ec --- /dev/null +++ b/example_15/bringup/config/rrbot_namespace_controllers.yaml @@ -0,0 +1,43 @@ +/rrbot/controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + position_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + +/rrbot/forward_position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position + + +/rrbot/position_trajectory_controller: + ros__parameters: + joints: + - joint1 + - joint2 + + command_interfaces: + - position + + state_interfaces: + - position + + state_publish_rate: 200.0 # Defaults to 50 + action_monitor_rate: 20.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml b/example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml new file mode 100644 index 000000000..c022d0873 --- /dev/null +++ b/example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml @@ -0,0 +1,12 @@ +publisher_forward_position_controller: + ros__parameters: + + wait_sec_between_publish: 5 + + publish_topic: /rrbot/forward_position_controller/commands + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] diff --git a/example_15/bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml b/example_15/bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml new file mode 100644 index 000000000..8b357deec --- /dev/null +++ b/example_15/bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml @@ -0,0 +1,24 @@ +publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "rrbot/position_trajectory_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] + + joints: + - joint1 + - joint2 + + check_starting_point: false + starting_point_limits: + joint1: [-0.1,0.1] + joint2: [-0.1,0.1] diff --git a/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py b/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py new file mode 100644 index 000000000..b96e5426a --- /dev/null +++ b/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py @@ -0,0 +1,142 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, ThisLaunchFileDir + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ) + ) + + # Initialize Arguments + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + slowdown = LaunchConfiguration("slowdown") + robot_controller = LaunchConfiguration("robot_controller") + + rrbot_1_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "namespace": "rrbot_1", + "description_package": "ros2_control_demo_example_1", + "description_file": "rrbot.urdf.xacro", + "runtime_config_package": "ros2_control_demo_example_15", + "controllers_file": "multi_controller_manager_rrbot_1_controllers.yaml", + "prefix": "rrbot_1_", + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "slowdown": slowdown, + "controller_manager_name": "/rrbot_1/controller_manager", + "robot_controller": robot_controller, + "start_rviz": "false", + }.items(), + ) + + rrbot_1_position_trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "position_trajectory_controller", + "-c", + "/rrbot_1/controller_manager", + "--inactive", + ], + ) + + rrbot_2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "namespace": "rrbot_2", + "description_package": "ros2_control_demo_example_5", + "description_file": "rrbot_system_with_external_sensor.urdf.xacro", + "runtime_config_package": "ros2_control_demo_example_15", + "controllers_file": "multi_controller_manager_rrbot_2_controllers.yaml", + "prefix": "rrbot_2_", + "use_mock_hardware": use_mock_hardware, + "mock_sensor_commands": mock_sensor_commands, + "slowdown": slowdown, + "controller_manager_name": "/rrbot_2/controller_manager", + "robot_controller": robot_controller, + "start_rviz": "false", + }.items(), + ) + + rrbot_2_position_trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "position_trajectory_controller", + "-c", + "/rrbot_2/controller_manager", + "--inactive", + ], + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_15"), "rviz", "multi_controller_manager.rviz"] + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + included_launch_files = [ + rrbot_1_launch, + rrbot_2_launch, + ] + + nodes_to_start = [ + rrbot_1_position_trajectory_controller_spawner, + rrbot_2_position_trajectory_controller_spawner, + rviz_node, + ] + + return LaunchDescription(declared_arguments + included_launch_files + nodes_to_start) diff --git a/example_15/bringup/launch/rrbot_base.launch.py b/example_15/bringup/launch/rrbot_base.launch.py new file mode 100644 index 000000000..1de7e6900 --- /dev/null +++ b/example_15/bringup/launch/rrbot_base.launch.py @@ -0,0 +1,231 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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, 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( + "namespace", + default_value="/", + description="Namespace of controller manager and controllers. This is useful for \ + multi-robot scenarios.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="ros2_control_demo_bringup", + description='Package with the controller\'s configuration in "config" folder. \ + Usually the argument is not set, it enables use of a custom setup.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="rrbot_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + 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", + description="URDF/XACRO description file with the robot.", + ) + ) + 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.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_gazebo", + default_value="false", + description="Start robot in Gazebo simulation.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "slowdown", default_value="3.0", description="Slowdown factor of the RRbot." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controller_manager_name", + default_value="/controller_manager", + description="Full name of the controller manager. This values should be set if \ + controller manager is used under a namespace.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + namespace = LaunchConfiguration("namespace") + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_gazebo = LaunchConfiguration("use_gazebo") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + slowdown = LaunchConfiguration("slowdown") + controller_manager_name = LaunchConfiguration("controller_manager_name") + robot_controller = LaunchConfiguration("robot_controller") + start_rviz = LaunchConfiguration("start_rviz") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + " ", + "use_gazebo:=", + use_gazebo, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + "slowdown:=", + slowdown, + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare(runtime_config_package), + "config", + controllers_file, + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "config", "rrbot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + namespace=namespace, + parameters=[robot_description, robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + namespace=namespace, + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + namespace=namespace, + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(start_rviz), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + namespace=namespace, + arguments=["joint_state_broadcaster", "-c", controller_manager_name], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + namespace=namespace, + arguments=[robot_controller, "-c", controller_manager_name], + ) + + # 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], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + robot_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_15/bringup/launch/rrbot_namespace.launch.py b/example_15/bringup/launch/rrbot_namespace.launch.py new file mode 100644 index 000000000..0cd59332b --- /dev/null +++ b/example_15/bringup/launch/rrbot_namespace.launch.py @@ -0,0 +1,144 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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 RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_1"), + "urdf", + "rrbot.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_15"), + "config", + "rrbot_namespace_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_15"), "rviz", "rrbot_namespace.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + namespace="/rrbot", + parameters=[robot_description, robot_controllers], + remappings=[ + ( + "/forward_position_controller/commands", + "/position_commands", + ), + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + namespace="/rrbot", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/rrbot/controller_manager"], + ) + + robot_forward_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "-c", "/rrbot/controller_manager"], + ) + + robot_position_trajectory_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "position_trajectory_controller", + "-c", + "/rrbot/controller_manager", + "--inactive", + ], + ) + + # 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 start of robot_controller after `joint_state_broadcaster` + delay_robot_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_forward_position_controller_spawner], + ) + ) + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + delay_robot_position_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_position_trajectory_controller_spawner], + ) + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_forward_position_controller_spawner_after_joint_state_broadcaster_spawner, + delay_robot_position_trajectory_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(nodes) diff --git a/example_15/bringup/launch/test_forward_position_controller.launch.py b/example_15/bringup/launch/test_forward_position_controller.launch.py new file mode 100644 index 000000000..d539e6a87 --- /dev/null +++ b/example_15/bringup/launch/test_forward_position_controller.launch.py @@ -0,0 +1,56 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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.substitutions import 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( + "publisher_config", + default_value="rrbot_forward_position_publisher.yaml", + description="Name of the publisher config file stored inside \ + ros2_control_demo_bringup/config/", + ) + ) + + # Initialize Arguments + publisher_config = LaunchConfiguration("publisher_config") + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_15"), + "config", + publisher_config, + ] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="publisher_forward_position_controller", + parameters=[position_goals], + output="both", + ) + ] + ) diff --git a/example_15/bringup/launch/test_joint_trajectory_controller.launch.py b/example_15/bringup/launch/test_joint_trajectory_controller.launch.py new file mode 100644 index 000000000..5aac0004f --- /dev/null +++ b/example_15/bringup/launch/test_joint_trajectory_controller.launch.py @@ -0,0 +1,56 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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.substitutions import 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( + "publisher_config", + default_value="rrbot_joint_trajectory_publisher.yaml", + description="Name of the publisher config file stored inside \ + ros2_control_demo_bringup/config/", + ) + ) + + # Initialize Arguments + publisher_config = LaunchConfiguration("publisher_config") + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_15"), + "config", + publisher_config, + ] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output="both", + ) + ] + ) diff --git a/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py b/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py new file mode 100644 index 000000000..df17e30d5 --- /dev/null +++ b/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py @@ -0,0 +1,61 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # For example: the parameters for different node may be placed into the same yaml file + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_15"), + "config", + "multi_controller_manager_forward_position_publisher.yaml", + ] + ) + + rrbot_1_publisher_forward_position_controller_node = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + namespace="rrbot_1", + name=["publisher_forward_position_controller"], + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + rrbot_2_publisher_forward_position_controller_node = Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + namespace="rrbot_2", + name=["publisher_forward_position_controller"], + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + return LaunchDescription( + [ + rrbot_1_publisher_forward_position_controller_node, + rrbot_2_publisher_forward_position_controller_node, + ] + ) diff --git a/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py b/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py new file mode 100644 index 000000000..00e73c529 --- /dev/null +++ b/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py @@ -0,0 +1,61 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + # For example: the parameters for different node may be placed into the same yaml file + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_15"), + "config", + "multi_controller_manager_joint_trajectory_publisher.yaml", + ] + ) + + rrbot_1_publisher_joint_trajectory_controller_node = Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + namespace="rrbot_1", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + rrbot_2_publisher_joint_trajectory_controller_node = Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + namespace="rrbot_2", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + return LaunchDescription( + [ + rrbot_1_publisher_joint_trajectory_controller_node, + rrbot_2_publisher_joint_trajectory_controller_node, + ] + ) diff --git a/example_15/description/rviz/multi_controller_manager.rviz b/example_15/description/rviz/multi_controller_manager.rviz new file mode 100644 index 000000000..ce41246c2 --- /dev/null +++ b/example_15/description/rviz/multi_controller_manager.rviz @@ -0,0 +1,262 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1102 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot_1/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + rrbot_1_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_1_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_1_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_1_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_1_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_1_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_1_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RRBot 1 + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot_2/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + rrbot_2_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_2_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_2_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_2_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_2_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_2_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_2_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RRBot 2 + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 0.20000000298023224 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fts_broadcaster/wrench + Torque Arrow Scale: 0.20000000298023224 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 11.863113403320312 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0044944556429982185 + Y: 1.0785865783691406 + Z: 2.4839563369750977 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5203989148139954 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.120418548583984 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1379 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000004f4000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000046000004f4000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1470 diff --git a/example_15/description/rviz/rrbot_namespace.rviz b/example_15/description/rviz/rrbot_namespace.rviz new file mode 100644 index 000000000..eeec6c3af --- /dev/null +++ b/example_15/description/rviz/rrbot_namespace.rviz @@ -0,0 +1,202 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1102 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 0.20000000298023224 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fts_broadcaster/wrench + Torque Arrow Scale: 0.20000000298023224 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.443930625915527 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0044944556429982185 + Y: 1.0785865783691406 + Z: 2.4839563369750977 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.23039916157722473 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.150422096252441 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1379 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000004f4000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000046000004f4000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1470 diff --git a/example_15/doc/two_rrbot.png b/example_15/doc/two_rrbot.png new file mode 100644 index 000000000..c77ddd53a Binary files /dev/null and b/example_15/doc/two_rrbot.png differ diff --git a/example_15/doc/userdoc.rst b/example_15/doc/userdoc.rst new file mode 100644 index 000000000..8b2b65cab --- /dev/null +++ b/example_15/doc/userdoc.rst @@ -0,0 +1,176 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_15/doc/userdoc.rst + +.. _ros2_control_demos_example_15_userdoc: + +Example 15: Using multiple controller managers +============================================== + +This example shows how to integrate multiple robots under different controller manager instances. + +.. include:: ../../doc/run_from_docker.rst + +Scenario: Using ros2_control within a local namespace +----------------------------------------------------- + +* Launch file: `rrbot_namespace.launch.py `__ +* Controllers yaml: `rrbot_namespace_controllers.yaml `__ +* URDF file: `rrbot.urdf.xacro `__ + + * Description: `rrbot_description.urdf.xacro `__ + * ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +* RViz configuration: `rrbot.rviz `__ +* Test nodes goals configuration: + + + `rrbot_forward_position_publisher `__ + + `rrbot_joint_trajectory_publisher `__ + +* Hardware interface plugin: `rrbot.cpp `__ + +.. note:: + + When running ``ros2 control`` CLI commands you have to use additional parameter with exact controller manager node name, i.e., ``-c /rrbot/controller_manager``. + +Launch the example with + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_15 rrbot_namespace.launch.py + +- Command interfaces: + + - joint1/position + - joint2/position + +- State interfaces: + + - joint1/position + - joint2/position + +Available controllers: (nodes under namespace "/rrbot") + +.. code-block:: shell + + $ ros2 control list_controllers -c /rrbot/controller_manager + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive + + +Commanding the robot using a ``ForwardCommandController`` (name: ``/rrbot/forward_position_controller``) + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_15 test_forward_position_controller.launch.py publisher_config:=rrbot_namespace_forward_position_publisher.yaml + +Abort the command and switch controller to use ``JointTrajectoryController`` (name: ``/rrbot/position_trajectory_controller``): + +.. code-block:: shell + + ros2 control switch_controllers -c /rrbot/controller_manager --deactivate forward_position_controller --activate position_trajectory_controller + +Commanding the robot using ``JointTrajectoryController`` (name: ``/rrbot/position_trajectory_controller``) + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_15 test_joint_trajectory_controller.launch.py publisher_config:=rrbot_namespace_joint_trajectory_publisher.yaml + +Scenario: Using multiple controller managers on the same machine +---------------------------------------------------------------- + +* Launch file: `multi_controller_manager_example_two_rrbots.launch.py `__ +* Controllers yaml: + - `multi_controller_manager_rrbot_1_controllers.yaml `__ + - `multi_controller_manager_rrbot_2_controllers.yaml `__ +* URDF file: `rrbot.urdf.xacro `__ + + * Description: `rrbot_description.urdf.xacro `__ + * ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +* RViz configuration: `rrbot.rviz `__ +* Test nodes goals configuration: + + + `rrbot_forward_position_publisher `__ + + `rrbot_joint_trajectory_publisher `__ + +* Hardware interface plugin: `rrbot.cpp `__ + + +.. note:: + + When running ``ros2 control`` CLI commands you have to use additional parameter with exact controller manager node name, e.g., ``-c /rrbot_1/controller_manager`` or ``-c /rrbot_2/controller_manager``. + +Launch the example with + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_15 multi_controller_manager_example_two_rrbots.launch.py + +You should see two robots in RViz: + + .. image:: two_rrbot.png + :width: 400 + :alt: Two Revolute-Revolute Manipulator Robot + +``rrbot_1`` namespace: + + - Command interfaces: + + - rrbot_1_joint1/position + - rrbot_1_joint2/position + + - State interfaces: + + - rrbot_1_joint1/position + - rrbot_1_joint2/position + +``rrbot_2`` namespace: + + - Command interfaces: + + - rrbot_2_joint1/position + - rrbot_2_joint2/position + + - State interfaces: + + - rrbot_2_joint1/position + - rrbot_2_joint2/position + +Available controllers (nodes under namespace ``/rrbot_1`` and ``/rrbot_2``): + +.. code-block:: shell + + $ ros2 control list_controllers -c /rrbot_1/controller_manager + position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + + $ ros2 control list_controllers -c /rrbot_2/controller_manager + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] inactive + forward_position_controller[forward_command_controller/ForwardCommandController] active + +Commanding the robots using the ``forward_position_controller`` (of type ``ForwardCommandController``) + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_15 test_multi_controller_manager_forward_position_controller.launch.py + +Switch controller to use the ``position_trajectory_controller`` (of type ``JointTrajectoryController``) - alternatively start main launch file with argument ``robot_controller:=position_trajectory_controller``: + +.. code-block:: shell + + ros2 control switch_controllers -c /rrbot_1/controller_manager --deactivate forward_position_controller --activate position_trajectory_controller + ros2 control switch_controllers -c /rrbot_2/controller_manager --deactivate forward_position_controller --activate position_trajectory_controller + +Commanding the robots using the now activated ``position_trajectory_controller``: + +.. code-block:: shell + + ros2 launch ros2_control_demo_example_15 test_multi_controller_manager_joint_trajectory_controller.launch.py + +Controllers from this demo +-------------------------- + * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ + * ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ + * ``Joint Trajectory Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_15/package.xml b/example_15/package.xml new file mode 100644 index 000000000..8aea9905c --- /dev/null +++ b/example_15/package.xml @@ -0,0 +1,42 @@ + + + + ros2_control_demo_example_15 + 0.0.0 + Demo package of `ros2_control` namespaced controller managers. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Dr.-Ing. Denis Štogl + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + controller_manager + forward_command_controller + joint_state_broadcaster + joint_trajectory_controller + robot_state_publisher + ros2_control_demo_example_1 + ros2_control_demo_example_5 + ros2_controllers_test_nodes + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_gtest + ament_cmake_pytest + launch_testing_ament_cmake + launch_testing_ros + liburdfdom-tools + xacro + + + ament_cmake + + diff --git a/example_15/test/test_multi_controller_manager_launch.py b/example_15/test/test_multi_controller_manager_launch.py new file mode 100644 index 000000000..f86fcb44f --- /dev/null +++ b/example_15/test/test_multi_controller_manager_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_15"), + "launch/multi_controller_manager_example_two_rrbots.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) diff --git a/example_15/test/test_rrbot_namespace_launch.py b/example_15/test/test_rrbot_namespace_launch.py new file mode 100644 index 000000000..806fa94a6 --- /dev/null +++ b/example_15/test/test_rrbot_namespace_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_15"), + "launch/rrbot_namespace.launch.py", + ) + ), + launch_arguments={"gui": "true"}.items(), + ) + + return LaunchDescription([launch_include, ReadyToTest()]) diff --git a/example_5/description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/example_5/description/urdf/rrbot_system_with_external_sensor.urdf.xacro index b332cf468..6cdf8c9bb 100644 --- a/example_5/description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/example_5/description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -28,7 +28,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + ros2_control_demo_example_11 ros2_control_demo_example_12 ros2_control_demo_example_14 + ros2_control_demo_example_15 ament_cmake