From a97760f4451cb6e3f50ae1e395c5ab8cab136041 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 28 May 2024 21:01:16 +0200 Subject: [PATCH] [Example 15] Using multiple controller managers under different namespaces (#423) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add example15 to workflows+packages * Initial commit from old PR Co-authored-by: Denis Štogl * Convert md to rst syntax * Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --------- Co-authored-by: Denis Štogl Co-authored-by: Sai Kishor Kothakota --- README.md | 6 + doc/index.rst | 5 + example_15/CMakeLists.txt | 39 +++ example_15/README.md | 5 + ...er_manager_forward_position_publisher.yaml | 25 ++ ...er_manager_joint_trajectory_publisher.yaml | 50 ++++ ...ontroller_manager_rrbot_1_controllers.yaml | 43 +++ ...ontroller_manager_rrbot_2_controllers.yaml | 43 +++ .../config/rrbot_namespace_controllers.yaml | 43 +++ ..._namespace_forward_position_publisher.yaml | 12 + ..._namespace_joint_trajectory_publisher.yaml | 24 ++ ...oller_manager_example_two_rrbots.launch.py | 142 ++++++++++ .../bringup/launch/rrbot_base.launch.py | 231 +++++++++++++++ .../bringup/launch/rrbot_namespace.launch.py | 144 ++++++++++ ...test_forward_position_controller.launch.py | 56 ++++ ...test_joint_trajectory_controller.launch.py | 56 ++++ ...ager_forward_position_controller.launch.py | 61 ++++ ...ager_joint_trajectory_controller.launch.py | 61 ++++ .../rviz/multi_controller_manager.rviz | 262 ++++++++++++++++++ .../description/rviz/rrbot_namespace.rviz | 202 ++++++++++++++ example_15/doc/two_rrbot.png | Bin 0 -> 29610 bytes example_15/doc/userdoc.rst | 176 ++++++++++++ example_15/package.xml | 42 +++ .../test_multi_controller_manager_launch.py | 54 ++++ .../test/test_rrbot_namespace_launch.py | 54 ++++ ...bot_system_with_external_sensor.urdf.xacro | 2 +- ros2_control_demos/package.xml | 1 + 27 files changed, 1838 insertions(+), 1 deletion(-) create mode 100644 example_15/CMakeLists.txt create mode 100644 example_15/README.md create mode 100644 example_15/bringup/config/multi_controller_manager_forward_position_publisher.yaml create mode 100644 example_15/bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml create mode 100644 example_15/bringup/config/multi_controller_manager_rrbot_1_controllers.yaml create mode 100644 example_15/bringup/config/multi_controller_manager_rrbot_2_controllers.yaml create mode 100644 example_15/bringup/config/rrbot_namespace_controllers.yaml create mode 100644 example_15/bringup/config/rrbot_namespace_forward_position_publisher.yaml create mode 100644 example_15/bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml create mode 100644 example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py create mode 100644 example_15/bringup/launch/rrbot_base.launch.py create mode 100644 example_15/bringup/launch/rrbot_namespace.launch.py create mode 100644 example_15/bringup/launch/test_forward_position_controller.launch.py create mode 100644 example_15/bringup/launch/test_joint_trajectory_controller.launch.py create mode 100644 example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py create mode 100644 example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py create mode 100644 example_15/description/rviz/multi_controller_manager.rviz create mode 100644 example_15/description/rviz/rrbot_namespace.rviz create mode 100644 example_15/doc/two_rrbot.png create mode 100644 example_15/doc/userdoc.rst create mode 100644 example_15/package.xml create mode 100644 example_15/test/test_multi_controller_manager_launch.py create mode 100644 example_15/test/test_rrbot_namespace_launch.py diff --git a/README.md b/README.md index 513b2fc5..1047a7c9 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 bfce9e6b..9dc42afa 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 00000000..84e220a5 --- /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 00000000..010dc0b7 --- /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 00000000..0a3afff5 --- /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 00000000..3792fa61 --- /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 00000000..784c8d5b --- /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 00000000..a987e786 --- /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 00000000..dd97d34e --- /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 00000000..c022d087 --- /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 00000000..8b357dee --- /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 00000000..b96e5426 --- /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 00000000..1de7e690 --- /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 00000000..0cd59332 --- /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 00000000..d539e6a8 --- /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 00000000..5aac0004 --- /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 00000000..df17e30d --- /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 00000000..00e73c52 --- /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 00000000..ce41246c --- /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 00000000..eeec6c3a --- /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 0000000000000000000000000000000000000000..c77ddd53a70dd57758558d7e71a65e595032f04a GIT binary patch literal 29610 zcmeFZcR1Dm|37|2C?OHDqwEpoSQ#ZFLa9Uc$jmwR2-z7KAuEnmNRE!}*cl-q%#2Jj-+z0)bH8RlTDP zfsnk0K#1Rwp8~(h)_!USzMOGX)pLPBsG3hci4p{;m?01j$lW`)9z9E4nb3&7s++>T z6Src|o&P#q&|mmzc4y!?#~E^oGi70R++z2(zr-QsFPhp4=+UW=5&b<18z~A! zrZW25ipoJteHKi&b=yCiUF?0BqhmdpcZDYxDJT+n$;WfbgmdO5&&;mD#bxnl4&#*z z>^(0Rqz+ac{E&dIuqE_38Ti^DZ4M|>u=&5aPMK`k`EBy}_2ZntlabhrLu>~6;OY<* z0`c!{{-5jrNZ@}Y@c*C$K4lQV0XDBatij114F<7iBVGFs8X=LPqofe{HL3kglV3k0 z#OtSZN2WTD7LKZA6AY?Q+u+42+9K@fJo5y5Mj>P`~_#xzb9})*v zTWnc&aArT%&>M#Mhnjt(NxAVOiHtNBH;*e2g%eW#*w$V&(gGOUTmMHZX&M9s;Q|)= z02oq08jpW!_aTQUasiGYF97T3zSvfGf!`<){-N3a7}+!Fw)p!%om_8$r-!SqRZwzB{FkVDe~T*g!oQ;WBe2_*)jlzV^AP{c-%pDe`^ABuNLBx( zg0~0T{$NcJ0yzL|p8NWn!Jn)W?Es02Kne}gadCwwvK<9K{L%EE>t(}9xy}<=-2X4x zrU00n+za`8uYzs?C4$%{2ypb##c9(LY{V#2OqqGpnn$IInh5eE`d?nJ@kURM&jSo( zT7m_8nShY0IWR;o9Nh@lb}vI9w@y@|Ykv*``Muh2H2uL8!;j|jw+M*PU#!2Yto3ThXJ!X3|7RUst6Su48(CuU3;a7CUx4*jc+|5V@u!-+o%yz}pZ3bEaquYmp_y|jQD zo5t~8Onm=uP_3Q^OwV!G0Q_dk{CoRoQl0FDJu9-^;(cGxxE^IKkH zmx1+ITrpyBgfLP}9Y7kK8Gr(sffWB7um8U*iL)DlWB0Ox%x^3K#9X_Py9nLAd`(%z z{!x$O3C#3EhCi^1(*UhJb-)^gdNyV;K#x8 zFOL2(M32{IW;=k9QBX5yZuogqt>Of?HgW&r_CD&5MH4V$PsWkyM#D zLT(6f^zwnwO%2=%y!WmhwwTPy(fNxOXAnUoN28qRAgq67^v71Ix$P9#NkSq63!nQx z1}=f+C<(yId(Y%le(keZ0y#uk`9x*zz-%hy1CfQztX7!h#>>g494#i*1GYc2KNrc_ z?xP+zC;sgq#JquA);|%JUDkoE#GUPPW{pK5k;{@M@&bU~Vy91KOMb6i>*!~`h;g|+F&2)Vw#gI_xH^zizeNdd)&p_?y!`)uvd;Bt zNCW3~E*W@NS%U8xF~l^ev=pt=KC*ZvXM4YoAjS+i7YR1REW0|#X%<9lW{W#lRf+bV z$}|8XYLxTa-rGXfXApQH{`Q_2N{*oXe0LP>2tOQh?G&&OF~Y)tX2v1B5@8@U)Dq z_|;^3Lqo~fKhXs%V+CceJb?D-6ux|38j=R0j6c}?bKR`0Eh|3<83iu-`~+5SjSB)P z`EMZhXYK4l)KqQ6b1N(WMj30`XWiBmA*SHKWST~^=^=<4e*QJco`y!AoaDdP`zJtx z&{hzccK(%+sey|kPFrncB)y;H2jJu%m-ocO$qd5rU)iDpG$~2wCY`GnO7qljApEBo z{(MjO?L@!|YPQ&M43gOe2a!IBN&Z7x4IebPtyCd*{{Gs+#Vsd^7*Ygq_YZLxmFSyq zNd9c#UHu8u$B;=4&_EQ4;|^vmYg!T# z{H+il;3mGhdo49yfiOy*NInrjQtz*#PD{#k_>e#p!? z-->dV?Jhp?mjjPC=$u0kw<-M#Kweyy%E4VoLj#b#XIbICzWiNWs5 zt25@uU$`9X$2}lF{+L=2wnE@7<4}LYzoh?fX|}&uY8v`i&cMJSb0slk1MDGXWOT_} zDX*X~tqvml7s#fy+_W-qqO&K?z(5p47z|*~{|tD4f6tNIp)NE?^Dv06<{tZOIvnMOGF3;S~qtAr=mBW9A2EV^o(A3rCHhF+rwzC#d z(E68=6SC*zPC+<;!_Rz{o2%^5Csm3e68j736MA#>{ZIW>M}wUGNeuSmB{)>rxtwwl zNYP1f{~f=bbP~|{cl}9X&P^Uotk#$u;_Hu+OpccQ_Uq#NL*>2Vv=4r$IB2V}AMuLI zlX9E_yE0l^TUqzL@~A&p_Ne!gB3yUFB8vBRrlThZ_g5=C_^#ff;-EfJOtx@I-5~)d zg(fnc;#&5Q9cfgza6`QX- zUpbKlu!D5#eO$D0*sm(luY5O;c6~RaKj|JX6c*HbdTm*4e2Lnfyt^*jTS~%lVgi(x zvQ!*=wtwK*CG>s6ni4(BuFff=)KceC7A z*ZPy7Kz4xPhin;-KUFw&KI{ZS)3F?6F(*#`PbL)k+pt4sPoN-EQ`M(NmMIVZ2C3Pf z6OpIG{kGfZ8W$JsI&8cgeGb<$j`u^WqEDH+!e5iLt!!$KzdM)Vccdc5tU!-CVV;4< z#C~7Kr1r>a&06mGSF~LHJ^{f_b7DpP6S{jc9>HL+T)v#eZ7b=t?js1}#binju42kJ zy0D@B{Wz3#nx;d#{IR)o-ABKYZU?*w5IiJP#GLsjnVYFPpSUerG);4%B$;C*vBJ-f zkb6AR7b>sDe)vOU?FkWt^8(JlWphmDrClyZl0}u}!2emxBg}dp2Ga2DCE}VgAz@^vNUklabgDh20n-}MkyU@#;~@LXVKSTD&FTJcphtGVO>3$T zB95;2r*I^N9K9L$+Z{GJ8uHsm8XXS?t>^BtvBeN(_0LNPNw)ZZ#C%XC9;e`KjT4Usi>jSG=~G{JzRm zzMqwF+bYc{o2-|Q%a%Ri%UK8s2d2>!y*O|3&PnvDDA zQaw-89Ft>#xPx1J;~yL2qR)^(8dSp&*I0-J3Rlf+V`Sw0Hup9N9v&Wi$qEn%-CcOl zOjK?yCZGF@?+#WZCCPG;w<=!yZH)ktzvy{frs*nG2ixndEuMu1t;G4!5@ucFp6F>( zNUH)oXp^@PT{88kdNOc)ZuN77k=JbqWUv{^dw0hSYbIXUxwT-z!e&V(9{_>yMvRi4 z4!bERQ-e)=r>o2QNz9DryrD@wVSm97=u*s%3hS68Rn$xGId6f(>`p~ZYxCF#lV+jxI)xbKNV4S*Gg zK?0ku+<57}F2NUa?$ z7HU6CrH@rdU#gHz(;N_fLNZKJ)w0EN@!8qF)4(Xdqrqj;rU5l8OKC>58Dh>kFXF-& zcj->~ZvX&jR8{XgoO6CVmU^yG$zpLhRX*>7HxcC8BXj1mC(!UaA6Q%9E@GRnxRNTU z{Zf462)r21HX;RHb7q$8crz?^7xwCA5oN;+U;NkcG#RTuNk7E<0?t$`XSe?cy6Yv7 zdE%ivC^x?tC9TQR&B1wWi6|{EulGMeJO-)zB#C{?r{$ONYs?uCh+P<>2(7)Nh_PIU z^61|Y>)@?2tHfHakU-G1IMY(intcX~C&;*(nwsywKY*lhHc7?ai(8sp$xpJc};ZMsWOM821;Qq;piMdtXyH1IuRRH$U?Dc*g9ujFf zY;nY^V#=dZYkRHZ#{E9paxLVK07P6rN_en2d($uLGt&FaqX@2 zv18|;C{)TlxdLB8hBLb=QA&(_Mb9u$=gCd+u6?5|mxVZY?On7BxL*tw4jpegtr4Gv zk4*b2w-a4sGiT1i+&2biA`R>4?KaeHS6holf zkB`QUwyGxcW8x;+c9>gJ3`}wDhPO)a^;3W*QU&-E!w_k~8W}8YTZ*$`1C&R$)-ZPw z6HyOM0(=7OJ$Fx5D9X1}#6g^Caf}qJ%I*oWMM4)bEDT$7;UDb>`e#CLig4Ol?ILfI zlUh>i&&B!r{3sz#MLk1DiHMadC=aVp`N9FVY||q>jir2Zw|Nj>(54ev>ogaElQ?nW z>*sNdf*gNxh^93NXTHJOloHrWD*a1Mm2W)6@-&0!%`yp(Y zOYh?u=eYJS04v8!L+Z5TkT+K`53=!u%M9xl>Q3b)`gchCETVhl2ufS;usfJO3+LEz zrcr&?SrkfbvE4f3aAto?M4$btgw(gUUr!=vpa9-ZXpcGC%h|S!&;FVar#I%k;NpIc zcuD{~YvN*6hS%n^tusTF7yR|inJp!=C)!%+Mg%=VD!#I~N3^Z26i2&uK!JgZ`!tFA z;pFaI=ITD2c>l z(|A^L|6pUhse>-c9ay|yflQEz5h_$Pu1E_U-)~RiyZBs#m{F=YD${FuzP@rTZ$Pt( zPxDLcp{r5y$DXqMUG{Sh_PC#9{~5Z zgDTVOW;{Ro713idF0`YfVdf5ucKWC?soAa~TS4gW2S|4WhfyKYi*jw{HQCUZ)$=Wg z$W%8U*VmD+iQClRS-MXmSXSRx1}MEskD47aD1W)zaMf)Q-@d{Lk6d{doX0`d%Um2K zcdoJ)69?n7e`1!sb|F24O-lL#e-WrQ|FS*k3h-;Ut**n`AK+48_#O47cO7@I36J01 zv>4t=IYV+chFH>nRJp>cX5X8{u6z8zO0)$abSV0`&I>PgdzM%8LjxusRFf2Sdh$O? zFDGK8?oPR*Qxk{exn2!_DY=MBz>z_?crblu=E`L|3*bvK+c21qjANd(ti0Cn4V+!K zg1|gp2c4S_OquYj=qcMK1Lw6>b1Q>9984XUg#N?U7YA2d*N9FQDZx^Op2D%s8#iDx zrkqvg+7^pFGY_D*U}?riM#I*oxd=_tyymD_Y}Z5u@N&)`9w1aUR}z*w^xvSt*{HDc zTXD-72(QC^>J#xY`~#hcMNtvU6r$c!INwAJ+v10=_WM{y&rIoUVIQKAl|>CrdD9=w zP%6urt?8X-B#>N_{4xPBcYQFafjVj#r% zC3S`b^I07B^$?0*9h|rLu-+F?Xy$0(E-c&2CaZp0%JgP4)cyTL+cf=7JmwdZ{XM6y zv78@Q-^3`RS_(pKTjTBf)kppAjdlFbkg-xsql4YAj(=dk-x9i+~eEc8)wVAcJsGhe4C zIp_nmUifNB~bc1yReP zP)kGkNnh>AF8E03C(O|2y5+=k$3=fRKMLx%#;MP-i}KU^`8hi_E@E-MJ`L0`YiO>e zQ1|x~1-@nsO|nr63p=~+iBCA}nkJZ9doizBSCAmbL0J2H7GSZu3WaK)o%s`EXyy(O z@Zq{{>>A>qw{)2ih7uzq?F&8i&+to@Fj#lHVjG-!m2UR6AlY{=4CPdR?nt_UP<$55R`Fc4t;@OBx>vnEz*&QS(J@|v}6j2+u!@c>AxGMu?2>T`*8hVWzjx&9C73Xh+h6XGP4l^Iq&rAE?8 zEZmsrKJ5IUAMJ|1)B&v#Er|dD!?Ks%8vkx`m+%1n;KJz8kmc502{o!#ML2WjVTxk6 zY|sq}v#O(ZCtspSjFN>4X3C^mt|iAQBQa_oUnWp;ahcdL0->pV+sbZaCe=Sx;CUuOVE*829O=ZoMMw2{162JR z{c&<1zps?5^u!=e*Fka2WnR8GlX9!)lz~A?`2Ay#@e#ODzE*c6-`dhnV?ftsDv`5& zA0$$Ck^_(r?I8%wL|`oE)9Ysx@GCD0juUPThnuN3W11}Z3K0&rm+cEEP0QAuSyifg zExvJxETY0yUeKusceS?FIvg~NrMAJ*$a})hJ`?M7v)X4Cc<40X0B}`Gl-YQ_k#UFJ)f}SFOyAXI+XbecA?Xz_7OMU> zSH9Y1yj*%Bgq7QU(WmW`X9RDByq#p!``Y$`6H@?#i6=k*sG?aDZqrhw5u;9|`G65p zEL1==5-3JL6L^zW@|qGECTBh|JzszQu^*i^<07X%GVTDzI=cJF-R)LoJd&am8|gaEjbd)m_`IjJ{=WYMMuuqlYP+K1a~gM9#v}?CRoVTV&NLWPb^KF8%TtqYa0< zXdmv#yKQTAz*+MoHsu{TD~w$dUYwZ&y-vTcKADAKz4Z*fvprK75ouFwHx^6CLfr-~6(X^^%LJ z!P5~s5767lU6hcDer6F;EV`*Mu-G+Oyn0}>U%ASeUd#WW40@%GZ$?zJO%>i@du>(O ztJLzcrs;AUTp)=?m=9CN-y*&+yFXH|*`7Z_JD2$x26MZ9=(WaUiFx(rk=*Y>g;a

zG3^UaggXVFz(YgkjKRVYuFJjhnDABhMY2SGu;uE@Gy{Xl~DON4AP zYobt5x!q!x4s%AB%k-r3KvxS}sbC~-GCC?Y^s(F5p@{FmWMNBl^ABLq%h=QI)c7dU zLfkZc(760zu3vvMG`jtPa5388t6kbyiPCI8&kWtnuw7G8xx+GQX|NQ*S}a=f(9ILU zjTC}IjEi-<#ND&JRDxz489?3W#x4^R_xbyr#b|+QbJvBK5qIQpJXgK39UV5!L2IJW z&u7nM=rjCaW^mWZ&_PBl2}^HFTq%K2tuB4~biaQ_cJVHa0mj~_wlVEU~e?wGK3OwhnnC)z(?Uf7)R6-0Ipe?pS5lGVPdD32y`)f$-`3gg3N)Jfqqy%>ghR4O0X=2j3zr~ zbEH6@Rzz4N#Ao>}mQiZuxQo~*^?B89hAi^eE_!wvd)<9#!0Cwr?TB@uy%wufZDd}^ zJzjGI3fHamM+He9SAr!2&~|wAHk(7Fp>{L#uB`o@xHtFY zhNVkn@T8&c$K7dxhOg&r5=JrdbL9mscjT zvelGE4r=A#e!Fig)z{tuA4{mf`QwM@Kt=4j+AH^$2*R7yc?Wqf1`M1gS$#3MlFdE! zm0_n`s>^~!A0G&6Vi|wZi}N^+h*32fu+OHjGk7h%jvd7igPjvbeO(I9k8Ee3F$l9` zdO{9o-B(^t!FfzX&(OR%)JUEQh-Yc4}!jr5$pXe3D2Rou~^$iGMt z0Tc1)NR}KVa0p9`G|kfOYm8S(F*#NuT`nHMrMlNsnBHqxvf75juYPLE4`EU=I6LJL z={gV){}t|1g1Raz6n=b`!g=Ku+`#&6t<~G!PekjIMqav->X!@7z?D>H+GcKsViR*L zXXC|N#Cy)79uz76g!16aZ!^EvbnAPWO%{=B&p!<%@wtFYWWKk-Q!NO3l@%)3Eh1xJIZ*MS|{PVcD@HE zKxd<52D2gU>^%-f#^SD`6}eH@>bM+YQVbt(28S_MhYiZfC&gQ^jM#U-8`N(ZOL-g( z5SnXRR(6ryRf4(bAj9g6ue@yu=C#G>Kgsdl-s8F4hbfM1HU+PWQRjJz@SjTqy+^sg zDtCEtaCL)$`E0q2FFL1|GT^nXDK3(p_X=2^L?XJHmiJCRBy(K-N~CY$ZhWJUy(C0G zgF+oF9RKo@lauqsFU^%&Yh7o>}nUa_0jc`FcGxv5+UMFo0O5xa|Cbg}oANQvvgBK9MS} zy=7x#1FT*e`IRGQ`}eE^Z%@iaM<1q84jWh@TWc7?Nq)+-}du?cK?lzcO={Yu6R+msN7z>}b;I8yK z{oMB`yMWnaaQ;Pz?K7iFd8PURvDJ<5yGZu@Haxto7>&K1h_RR8Y+BWbXq$vzm+?mO z%%t`(N11ULoX>}57EC=z&Xzod^ch5fFzUv=&&vbA4ljJ7P3x??WCgd&LDrL3P@aTe z8m0znm+f_pD%h&3eeQ3MW88INbTGeoz}m6ksT+^ zUx=`U(}2Q&9RB#Jh){D>_S=jAxNN?9iqX_=vVmD7v?VP=J`aW5F88VOk<9k5okTes zGC3h>b1=^6GtTXGFE6S^A$$@?FgI4n0XX^E@@3O+rv|`%zCP|B7i&rItxCV1Y;_g>`f-*l0-PSy`W1{tk>Pv+d*HaH*HiYXYw})9#au(;&c|3+3bl>m7o~Z;9dN zO1OJePDkLk;K~(ZEJ#C+ZX9xV$fqgYsxxL%%B{R$^0OZ-*AQnm95Y;#{&ZOn1~cmk zP19VVI~q6UvWMHz`|&-ji7hxC>8O8ar?8zm$g|P9FrA3PNk4j9WU#fZ?qdARQm4)V zg>hnSMF3SXiY4iGIxfoDD4vrLmR zirz%e>EV`}5z|Jn;Zz*os86^uNb#BgP^F_zSnHvxYeiYwF9%B&deU?CQ0r-rf7?=2 zz|UF2jYM_QChN+jE=}8|8^K^t+S)gQ4ChDZ%}}f57pFr8XL2}DDa&05DTAAz2aI=} z$P9n}o~debCZ1%hs$kPj?MzgvV;A3jV1X%SNzyl(A7e{af~CE3p&joZkjaU+TX0Mt zeP8RId3d5ZJ#x7BV`)><+}nnFZu3c-A zliGLg2)fi7)4=xTWknr*htnpp=&IN2T%U}KD}0zC75S|z3A4Q~TgWAnpa1&Z=<&;M z8s=$#D+8)QL$7HggxT%WI zGGwvWh%p$KuT={zMjK0AW#00t80B={xk4^B<{c9vAyY$#->E&RpSo~M=zhk&EY`#i z120!PAK@ofwYO4gaxYgRKV`29F&OOSmP2`Wgr1=ixQOpxq6-Uzxm>rH6AqxRYgvr% z#c$as>^wobb5+6hMmy!e>M0FYhhwEMVl2j!vm5Ih=JhKR<$OGk$?i`3jL(xxn(5a1 zbgvyZ7pD|&O}roOU} zvL%hZ?!2V6YunZ%kT__#y=%EU6?DU8jKH~$Ywg5*kju!A6B(ecxmiNf8*(KJ!KAu2 zGXDaBrG}iD>-rd1X&-&u zhM0aer7ttCOzOcaT$=#xSGg0_7N%bhB9~A%?f4MNU~<`>65PJD+TyoyfNb&eDqDDt z{dS_WJe-mXP=jEcgi?f%S`simOEjt7VJGJx1?I;ppl>uI@u zUu8Z)7HZ>i`^K_4t}~F&-josLoXa<;TYwF}^OB|@zTu{+ zp1abT^`a)SpTY^MY{gLPHjSdR3cBlrT~6g<*otj>P%d?5@1Rr|bGr7kEUDtyYh9Zc zcBg_wRs;iA!pCtdyIyaDC7HQO!Abjy+BzrLGQ>_~HyoK_EzHUM^V8RZM&5UN47jt> zbs=V|AiAWXDX=}5!AU0toDLBWu3XBO*Dzrp(Q`_ONsZ;-Wk~n4 z?)cU=q7kX@`T1SnBfFfm4SzzfamCgJ08%khc@pgSpVY+y#b}2)OxTKEE{&#XM5ZT0 zc5q}$U0#+xj}k3UX~`lQ7IohDn57cERIckF1?Um^}^F76DQ zzt|Ecp>gLhxB;qS`M!%FyKY|$4Snv(+(93(nzxKFiIB8FoA`;_D84p&o2Du~I$c3F18k1@AWC&zjEod8-^*d^DIG(ROIx3Jqjo+{DlGyFsdhwJ(YN)+x*==cd zHed7jNKR;&bLski1D)Uo0q&cmYOFV#`8w?-32!m9?JMbj-PYoAv`5U`)R+M#@!&tBaFMF0>87%)x0ijH1i zTU*j;`gCryg!Us@=~Q{w$d2LV0ydU~s5VWj<+en=g8cyKm0PM5LSsinefJ-djjF=! zlIdeT6<-K~Fmcb7_%Q4%Ux;dj?<79o63x+XS=*ooWHfB@zQ-Vh zmIoC1(3@Yt>1U28u|xzQbawhCv)YQ z3RdzI`D-?NlCG$ulKW3PY@jWm)8V|cmhO}AHVi?Q$DnxEl)`kR=@t11D>3%KDaBsm zDE*-Bjb^Uh&UeIfPGLUE@+-bpNF)ww3jD+j!|pgMC0~I=tUO&-{|deFP>J~J(EJNb z7=rLYWDd0NoSv0Gd|?;y)rA2}GVd7NTkj)PzFfei;VW$C>E#n|HttT1c`l-pBiS)b zkG;;KR+r?vD1!F7_eiQhYuOx$ZjC4lJC_p?!_lzS+!eUhjn$ z_=)|Gdb>~ZE~ywrjc!V;56(A-z7tvo#xT^){i2MO{+*D1Ettv+-0NYkx2}&yAB5qw z(y*JpH-t4mb>g*D24>vY5j_9%-9vo|BTMByeLI>< zY6XaAoa6j>+H`j%(mvetTA-+^)A>V>Uvw&G9UWAH=@#K5XFhvs>DU+jYq^S21r*2( zgq==4C~mm5@I`$Szy?Xb?WDiCmF~@Hh{hh|9I@P~6D>Km?Cn3De|y}JSgR!8PUf(d zb@`zo)5gL(Gj9upwDhR4R2KWt#4DvDyWiA7xTXvO_U7@o(%t0Ny0g>3L}2zRg1x++ z?sS-!1y>R}>6g94tvn5yRGlK?$8NX3epfDq9ZFBTvY_Tzx;*%mEg|Ytha;o8l4_@ka1FoZiL-%UPQ!T8v~ONCo#(7`9-Et zdPmpQ0m#nG|h1z=Wmk30v}3~+q`g4`$aCX zM`u(`RL`eQcJ7XFvj#WAB^l$QlOxkc&~Q0cY00T9B7^F~V8PX?Z04f~P3dUhgoU}Q ztULiGddZ1#@9;Fj9Z4XmA}Q9mEWFN8)JcDTayZrp)Nzy^XSe!wJEo$RTt7H`ZPCHkZc*3jMzxN?(6+#N}mGV&>x*aAV2*0VIYnm-p?ua9=qEz z=j~l;Qpc(!iB{n?$_TFH6vIglt*ftPk@G(Z2eVHzK4;npVm!{sINoB!u%YK$y_sAz z!+UVPV7)6VZTiswAEGzY=w~c2MN7~tlSxAO#F?c+-*xqSlS%lQGO3r0I3{&<$K~}m zV_pn8w4)?J?yQW{&n*~Vg1TcF{_quH;eALV*0}bFd-pQSSCIv>+)K!pRe?}1_ttq^V>|T@#l#u@}pV8N+Q(4+xE?BIs@NH;#=4E7<1hvDB z*AAnfP8QjP$5Aq3CdX3gXmLy~S1VDdBdk0j1N39T7!YU>EmfZnKcc^cj&}U=`=j09 zA&C6EAC8SlXRcB>eQ$@iYz%LkuG8fX(kE8Dr&5WZ@C9Kuj(uuk0yqP3g$N49wwM1KyygC*x%9qAK4WMjwjATteHrl+0^gq{m@wX|RH_%Ts!O_4?5}3Z@b8 z#gHLlT(VSsD?-)$=_eE8IxzcraR7c{E4ID6 zH9!)iOzIbDaXVoo>eikk4CEub?q}eQLxj)~`Bv_xMr}7MgPX_J^_IE_>WH6tmv+!Z z2vyGfdaosw+~M^P_p}YEK(OP~TS9jpmm>GVTO|;jq;~T%*;59N1WF6q=}ZiNX_sF0 z6FTMMq2w7Q-_VhAdrPPqqWj)k1Fb5%O!~q2_l(+XPth(*7AaN(Bb}C zmT8WnRXO@;4NptC>ekSOmT*Kq7;8d&7j$6KU~`PV{(jPG?xmx5%@h{Tpf$PASy#Wm zd-B?w-~C^=B9ak(<6dP)W%RmX9rj{XMc3s^hS!<94`JGd0?z}(X|G}s1DqvffJM?X zgDIisC~c_h#Hv)K(y!A^V+L1h2s@ZTnJVhA)41(*+rIw0Q=lgC)`sz^MbyamzXAdFTdMUSU z5-BEDta5#!J=e;>_+D%{b+daR!D(jB($l^A@WMLO){bz z8eG{}cxyTOHTT)Z)8jroEnRsiwdLd6cqUvp72L&&A%$3=5tO1w9YFo>`%A~ub4#K% zg}P*2@CcBESdC=n_)!YM76PUZ{V4C!%-!cJKxu{^)I*=iwqOiF;Z5|-h13R}7Xft| z#IGFPLf?E7U{v$ddHUpXftNM;ccSvkd`eF1eJDDKZ%^G}u%LavOUUjg1~DL9d@S8j z6XuVq?!i-> z4JPdtlTCO&YG4|6U-{kVQD{+0gC>2=n-@Zjlr`t2uf!TeyS^PK*Ok92-n4A&6q1CU ze@?i3V7B&M^4`xcRJMa7bzruxWICs^n!yx+ODnAzqYWxEPi)T!w^f(&P3$S36CP2( zaGVWy#LEmFkS(pu+7w)93LwqEhP?e`1oI-YKBrt#Z_3Cgyjfgqj4Vc|g4Sr!I9sr&A7(dvbI3X;O?a=c+tAe%n{h5d-dJk=kPKKyNsjC4WzGSA7{*H zx9PEt71N{BM!Abt`JbEtoq@@`Z><=xgcGM>K`|VRzV-OoLzX({ua`%G7}j=QWaI$@t!O3()0cIA-51vr(Hvqp&4L19hkldc zDJFbf%i#BFmn+D%kVfUBo#>T9EsjT_D4>%!0*&IHtl4!ZBd)(^0VG*#XINak z-^|<)ZK(J*=@sFK!Ch=d#~i{_X_BNXdEQapy>%r(2$ah&3E<+*Ge)_IUC}KUQYG>^ zCO-dP0LKUO9#1ip^=^i2E5u@v5z3reDhde zdNk|2ZpGz|Y5u@!o80+NK~!2!z+A?`sws;RnD1c%wk-K-|9Gb^Lc%^TwyBM*wY9!o``g%Y5ovd$O{lVlC(@DQ#Fng7V^U3&!h~E!TaE z(*0F1xA74*P*YQnNSFS(AK6*Z_C`mk^3U`qxSmvuj#Vtjci2)i$f~FuDwRP%TmvVX}zh>R~x06I_-kQqR`Zl!%CUDRM1{|+E0b75UPe`b_N?-Yj zjGbcy-x^@9x~9h9VMG>r=qns|QceJ(yZ_dyyJv)HiLv1WwU#dIiFLT-vb6`*s|)P> zEvSmfwx!jX-Q~9Z+vC${KHD_X*OtkexgZwka&|(pCNy8cBv{uAxvM!|T3pow`NcaY zPL}r>Nj4gt#?p)47@{DU6@B_w;YV9$dgWQpYxFOBbQR1E?&|9uiU>9CpIv!1%yktd zp}eNgaB*4;bsf3-1yMD5qYQl!C*O|QekqyXDd@_{*L|?`1NHuvoC(oOgyoM9hSzli z?LuU)EO$U}tfx3s2A08!X{HaZl(MdYR}#IB7^pAhRg|5VsQKP;p_B`wLX}R(kUUCn zBu`k5|E6!v0X!pnhmHa5bo%kcx^Tar{l*m2+gFp~(nB-ZR>75WeB}v0VmuH&;p+Ni zcy)M$zzFe!d@D~Le8B4HMZI^)D#|sicGnrE`w0ezd8HmxY9a*T5W4A@@Gi!C@2Fe~ z^RtY8b)9vZc<@vJjO-6AY8dz->5`I0=^xYue%uwXs=d>#EPk~T!Putr!4NI@eqeDs z4Y{qEaXg>d1`h@!9tTydRqxLyeH8J?9n^V;l3ukg)OkHq&4URBdCp*f(4hXEGxl7> z*t3=qMsF=Jw=Gk9i;B-DJaZpz9Nt z-z-vdl6Mjjf0pBs9nvY#UP~Cvu8e91gXeBL>)87sW&k6(*O<8$J|vDk!;l#vmYAfdEx)yy)wX(IHQ0o|) zg2{Vwd%MDg;O%kg10-2(i~Yv;L{i-_3W6jjGv&v7(!0kcOP2;q=o4!$;Jn1T8!$En zBqNcz>c#aq(yA=Co22(YNXbZzeb=JoYHEioG54f~+@hZ$#jIJY&ZpmEy&F!8ljbQB zg55u3|3PM1xe2E$aHGkSK;;T@pjN-SqXTAZv=w7;W#1>C&(}~D4p;az;mh|AqhPvm zGbU-*&&qV*6fpfm%v^OoE^03w6`$@&Y2xwMcOT4x6Or@@S0eE3sdgUP1_o=fJ)Ek4 z`(6?ZODJ`Azp{d&=7but7xk*SoekVSGC-T6tX(GV11~S&)dxD7C^(ijhtQpc zj51=QHMzzuoXxC_gb`hc{9N&}Os}tBBt7e3&|lp$-swZ=~*s zmsN5w?`wU?CkqCw^4a=>TbOxE-Tb3WOvP{zP&`zR2(FAZ)cn%W^qyTdSF#u(ams)O zAkiu*x=Zo?(siX$QIMC4PqmWk&d|OT+`Mnja?(-7%jBqChb-k>0zRHh?mhn#2f+2L zJY6!uFbQRTaOcSjKPUgvLWvaSc2Jsehy<@(eJOQxUFAl|RA{_>f8QmzQcG@sBVV^r z=i&@6@QwP%3>LQG28wWlyyUdi_ey=k@~Pc!No?A4fWpQ(7B$!4L#^lG6}T|8Uu> zr5B0@3$9l}rv4t39_Bx)C(77~t3PM& zF$;SresNmg>6_yHI|H_WEijNmH)EPr+>EjEO}aRJZRJLc5X05V)fT;(f$#%L%E#dL zo%!PAfS0sQNmPvY!*H-viTJg1@|ff_*=xbw!${ynu3WPNL>hqqP%=y*RLOg)mX){7R3}YU$c2NiK*&WK0D!u{|`8haoI>K0XaO!TUiRu&KMlhLm zGdRSN#-0+P8j>6TwezGXH8o)Eq616wVr?~jQ;hBnJZE>#nbznbf_Xu3-SAMJ!fj{G zU~w+wwW|AWGJ55_a8p-*h{dy}#IfOrVFps?&AMnTi+c>_4og;>{-?|uB>|r zAu89UM1*T+l~pn`?scyj5#nBZlaiHpMiH)cZF24GWL|qFTiN4&UwwZ6{_el8bMEW; zI?w%hKF>Lo;~;__wLe&mE<4s`pZIb|q~^2?8T#5l(}^3%5W65#g0sVkU|m-pT#dP@ zw_@f=2FonQHvJn60g>#wV+}MPv^XFc1cm`c%FYLNF+{1w+PqdK&^-wfuGAsH7k=?F;qfi6l>)ke*We*jdWu@&Bd0$+1sGAhy1?oK zFqFyOn3x?wn$RA<4b1biTm^!P1O~|#z|YV&+j6IOFWD|t4L{JRIbwf2u6E}Abp2mk zLHgKd06Gyuareveg9}kM+gk(u{c%1>zW*ZLAbB_~LcsQ#JW0I-K@)5`(CIcSa1Yp_ z=C`54;J-8dA-T{m86n>nw(D3|wh@JLoh3mE7T199HzsUM8n zK|(8vxg?02_KJ;u7xFIwjY*Bg|0K-FFu3o(f*%?VCie{x`e2Bztl1AnG2dV^l-$6l zThW(@KP;d)GzXO9t`*H7(dPF;(;-_wsp7bJ zO@C&3&GIHI6=A;YM!7K{>{+z*fV-*t-*FrXNxh>m);Ruge!q|ZFN!gLHeXnnKb(F+ ze(Is%M4|Fp;I*6K_`+C>!+EeX)ol7n$^3>?tG*7V??3?zsrLHcu`@^=$MqCCO-&Aa zi#zp2BGUymR=Ao%MQ?NNx1be*Tr{!v%%>}sR%(v+tNG|DXwBLfweFN!9h*p*A2L}0 zRQFGvIp@c-j4TU)+=UEGEo`X$^kb!Fv-WIsYG`&Xg>9UsHyzxGG;uAWdSR~o{19Ej z;6f9fJz4^p+BYn{=JrefylP$4S^4B<8?cqh>v#D*VN?e}BYAcuHFW(?y4*RQrwcSX zF9MV+!}T1aKauG)hrGfEKCi20DE_ANW&;q&o)*(%z2{)qfLN;Sn&fuuq1C;7IPy^7 zS(;QvEUWbE#?=}WklVv=RH@^EaK?yX{6kV#T`VfL{cJSjQ(uhuArnov3V}@u5YN=N zbI2Gz!b+IM7Nt#S9_v=nQT=RQGwF9DP^9UQ@!b*s3fL7NskRY$-FjuS%WL8UImbUF zz*7dDto^}^vc&h%SA2MqoHu>G(i~B)p1H+^> z)kgQ})oMB*;Q0!Czv){ek5(__NgS%m@d0__Mf3Zj=Gr!<;HQb9cPvBx!Jx`O)7sWn z$%Ri@6{J)X?S`J*5)!J29bG+DR|+{Mlj^grd*VI?FDeP%X@i`UJ=Z|^L2S~HTj_c! zeak4x3c8K*^6|ZbZ{%+vp~P#F!ev5xA!3pcb&wE0JuBEe3o+-O()(7lo1NaqFtGSQ zVKvJ?@NfL@lI<6qv9}l3{a&AXd5?TqXfM!^(rU}pFZFdD%Ku@LPKDgv*znv>mh9Bn z%{M+|2~V%Dw{AL|sh&P>d@kng6W}R-Jhu;)kC$!^`@5`pew$;v0TLlDlC2 z^i-fBaB=5^OUI3>t+cfHV4c>WM1hVFuMd41#qK_za8JK&cX`t+5|2WC>cUtF2D(x= zsz@blFZRa!?tu8jYgW1{y^NNpeJ@U8n=eY`Gd#ENANMM}^u5DHJnvOHX^Gzk&Ni_7 ztgr4!&x;yhc}tKcy3|0kKwiZwM%# zQyvS9jTw+PUMeeQK8sbW5xD0&EX!5Gm^)dwdOk9jUb7PmZ)z^?;UwhgHFrs*)MIz( zY|c$J*I5~t?ldb%&MNnSsduyssrRm(kp#};JhL_3g%}UZ{NWd5c4Ae8Njf!`=3Oh@ zHU<6`l(qn2x#(STJ@!$E`q^VH$_YsXJQWo{1*I;aYCV*EpV7cqj@fF#&Fa&QF&Lph zzCqrKQr8-9r$r>Q!8)JR$ceWSY;VFdqi1Ur#6k-ZjH<3te0CFvmNj>Q3fX{CPx|YB z*oa@$3;RMcl^xjJ^>o+?-Sh^NS5nSw!jDiGzk!e*Y#{>T1JaO|SLjJcyUVsAHTYQM z(@Ko#=l{Of{e1a00%#F_Fb?v#$(en=IFHUaOIqCIf)u9o>iNjJI~rjItb?5W-W`

=lEHjv&|E(X;* zyUK1B+17IX%tJXoL9}=)Bj%>BXE&ZMUFRaBXHT=OgSLmsVDibp6a?towi0u)-mf3z zV^;pP1NXsVCY+8Qe4?D{Bi^o)OQB*rVD;|m#ZU**k^?W^*BJ8${aWDkR*u9E z7Tmj-t* zB7Qul`JG@zgRzj~ZB_o!$r&$UY|oqPa@tX9a4wMrUr0$Xr;TNqMq=94W+|?~(MkSe zgtTjC7vKBkMq1hQYeo~#PK|qousZ5C0dcw<7hkp7-d2hsU6r1LBNpXr`2_=`WfS^X zaJa;xgb>R(c@sTn>2p=OYM$jpA73DRe^RB~&d26n`;?BP$T&_z)Ym){{Ty{ieZdoT z=tD{fNxv>@S--3B*tzj8wa}CV!|=_nw$ma4h%6YzI30;-L&3NR6Ww*X9q9Zl=Dx=A z2&GEz7~P_F^O=A8_w;3L@)E2KnJ?z7mc$@5bAbk^F?3v{gaWSv4K9+y5w$1 zbX=!ps8HQREvEQMt1ahU<*l01$lC{bptV&3kI=hIIr9%{#@FXu)l75cu)k~v+aE^# zXKB!K-sG=zp>7Lv1sEGDcdOq8r2ktKQI(^t!{*)?Sb9?!X=#788Rz-z)Ai>T@n&jy zQ{y$!TI}c+sfWFedY$N^5?cxEVkau9VfxXjv5CyFMn*uXWwXb0Fdmh8wVC>ImpwRP5dRRatam;GI zr-X^(^C#O2sfM3zhf^F3kZ5s0ZcqK--D$*KrRTP((;e#{L2BK0r{neMrTzBmuM9&~OkaL@ASrYfzInvwdxBwtyb zo}=ci)B-50CP6P6Px`JV?{^V93V|7Ne`&KX3cV#>bK~&jt$)h)KsdjLrrP(shTrD! zz%7^`U>w~wjfwWcZi8dFTbI(2mJaS0sDYNaT`R|KE84&<7b)GA&0fMeDO8@G6a(i+ z*M2h0(*?zur0bncy^XMTdYQ)VZcq9f^#^Se(_84RVw!8tp#ih;NKh$-I3$3VjTkt) zh+10IUtk|ok8lT3k#HhRD!1G)mAH2lTZpI-i$V4S(sgfVtX6#jX4WAW8+fexk2cnl z8yw1gPg5eIo7d7x9Oy}2iFq<{vmK{$fXz=PA_#jvvBsF}vjc(q=lelp7gmGOFle6^ zCvo?ae2>5NDP*oZ>EFxpRL@{e$g6eNWD@%FWhURGkKM;^Ui@8&d^wC^k@F}fu@iA? zrFhXh{F*mJ+1`soOujgD4Z4%kVFuGZ8CS{?(3;H^t~)4D z;Vn{y&xGQ8{RbACoOj<%t4n={T?a(;gS%p5T~px6-g3dsX92)gTluc$9JCh$?r855 zt#aUb`q7j7J_h##cXCHpA6x=<7>u`H1y$b+{dRLSUaubS5I^g}{nLQ0 z**lVmfMhja{P;6w{pt`n(2blrdy?FIwztOB+$s1bP`12VQsKu_YvtG5jp!#lo6OAP z< zzG@8%p56?`P8dph7(F$8xmDoC0Hyf1MW%x9@XpJwKbrbE2F_E|z5(%=d(3d`BJWfa zHU34Ve9Aa`^(Q-f(r+D0&e}8U-{+=(w?zVrB(4ABEEXFM5!P>U4;v1HLC4cv035c2 z(<9zs+z~VZYjhDYTY^bjwd3yk`t0n9#bE1|5uEVScw>xJ>kT2xOE2UW_xi>}8ONe9 z1vE?vs3KA5M2gkdu(NdgnekiSKgzJ_KX}R~4rd0eJK^$TA(GmDzEXkD4IXB$LC||A zVVcN=JF~0_z8=`4#{j9dlGkr#e|3vmlPfvt{Sei=+as`0$qd+xwRr7dILw};?drro z_M9o@;>uLQ)HSC;24-23LhfADmn=6N&1C8&^BM`?7EUILM-N^kp(3v~m$_bVF@w?d z$&x548vYFVm}yFV=)A#B7*Qa~O zx&LEsn($##DLWQ7tD84ttz*qG85`h}fBVr}9{f(F- zDGeLwWmmLpVn}++Jkx{2=r>p06#7ukkC<#Mg+PVE@1hsw)8)4~G=%Y>igs?O%yU24 z=PI{$eDr@~F}Le_>uo75l2Qf-RS36;v#o&Qwa%xV-uocO+KeM2^c^uP@oUKy4Vfbx z8Az6lUcn`o(t`rA>od%%bX=3X3jSbKNh{=~)dZAIFqFhr{y;OxaJmCM-Q0{dtt$_9SX`2xkI;OCxnT||Xf9P|7WgqRbc{1H4=h6`*FB9Bh6cq^yI+VNy8n)%GwGnAZ4 zi|X2|O5U-dF^8{a96oW3zJ8pH6`vPnee)Hzga_3!#22Rxavf|g|!vZ@yd*- z59;@oQnIatYQ+?GRVqfzip8{1$ddF!Q91h@d@pV>4c^8SH>tsizEoJ0xVy?g!O+oW zlS~pNPX=WB!{l6*+1OlnhefrDRU6Oif(uo+#AntWH=tkgPRFo(GDehfyG~3wPd1gA zQZjOh=vr1XQ9ZYm4=hEnFn0Iu{-HELXaW1X=9*w6N!Ubvlx+{quqfYAZlhugGuoxT zbEhZW!<;F>rKey>l27NdY!vim&VB^V=i;2ARn zeFSC>r848>{-h8~GpP!%Wpp$Z{3z0`Xrc}iI`dlcO{ovkazeB!Fv;q#GU)a1`A{2U z%AXE~QQ2kOC>$Q`hfYaf;z~rFUM>_)7u_YCo^`(4I|l(YM!qi=5;4YjM7R08;a*Ps zKJuM7R7FQ=PXYnklGc|pp#EEcr4fmm-HKn4WD0o0M~s-TkWCBe8EVJcGV3x(Redxr zmIPMmo+iLe*eZ8C=}Sw>470Koh^Bu&u<}Z`F~;6RCK5MP^3@!!4U^FzY)Tdi-v#CW z{q6?d>IH2P?5*8dWG&@?smtkS&W~tttf|i&;#quRd zbfs!Pf9|BrN@LL3QTP1L#&VB=PTYY8`vyo$+nL=o5w7{nD`WdU8^eF_SaUVym%Z>O zig(E%zKxGmEVqe!U5Z>RQsvwP*#zc4FznwpbpF)}o0)bPGZ6t)*18k-dDfCC4^K2D^dL!6}9U$hsW_jYw^`ulj%LS5XP~ztbg2YRnL*R z^>Xd&RCs&qtUcxO@idh!%nDizNhX*5n=>3FnYiSCO&Q-sG#~UK#J)k+eBKhKCyDK* zJ37z$J}hORJz!#UV*LWMGI^eufOEA7r=;NS3KRL;t1TL6ygv0;O7QU^{QkHK{LL5d zdoxKctqsI;O_sJkO8 z!tWNXDp^q|ut;a``pFo5;qXN7Ysb`TJMb55vyT-=UHfBAy0QOgqM%=*+EiT_AzVo+ zAuMy$zrOf|dZ6~X91>Kdkm6Vrll)d@v68p`PZ0 zR*U=2cZr4)rrtjn+Q-A;k{OP#Dp1e~d(&AMO zi#5WnPi=b}aCDw=cjH8WHO{cQc5u!uO#O~Qk6fl5WG!8e5pnJ&485Z2Nlao^y@+km zTdkb2=D?ZjXUX*VOZ5~{yP8`{+0Vn$y9py#NXUFWd|DN;3rF;e20Su0-t=o`ks}j< zqu(+a2zQK7W?g#+Kt3P!10VY=(F|AF&DMr~gOg85cw~S=unAMYj9WH_!vwpyuoy1q zfNCIv`1+&83~zmKOrX7)u?sqj9qB24 zPU1hIC_c5sfO;8H7v(`rqe+6z2L+E%Q|(YMbq^RU^-?Igdt9;-sUCCHt@wv}Qp0Dg^g65_R`=>hB&#< zDqmQhEtK)eZIZ@1>sNIfd)axk8NYg;<+NR^m(Pwuw)OztH9!Fta#`f?1<{}hpi zLt~V4b(VMXQZ2c|;xDh%*LV&-OJ*35u}!#?|CVHYqWDj}35i66bn^`$Tg%bQ*xQVM z(pdIpN%zjpjsCxhArPf#l!f_xY{Gyvc6dQK^>b8n4Sl{qfTh50I4n83BlR9^p zPjVpTHpG5l|4L*`=%WW`Z@jGPZvs{p<+PTc-Os@{bf2KWkB&U3imbrwV0Q{}sm2-Qao0H)2lmH0k#)HDP57=XQTNtuqVeMHH2cNpzcuZ!24s|)k zxg52mg*O+Au_xaW>2KDueuKI$!5=st!ywPN*SE(~t^bY2%IL7b4diA>h=qa==Hw7m zY{A8Ktg8gDKVN(`@;+?Spr!2i`4yOeF`MJOkHbHEsAPYR_k4h*93-BBw>|CR0cRfm z&ivFEv*cXHo*sYAp&a8mK|lvAf@4OAj(|^RON#N%o@IV*V-0a^<54y4PdzFoYX?jQ#CJg=P)e(tUi-+Zm0hZ5}-^24u8r8V;tt z_D14n#5!t&V=GvC5ME~jgy$S(NZY~z2-^hsUS3BPP@8Sb(j^pKM5=_gaY=4^v} zp65vPB`zMtrf=WJsH#dhSdf|X-{V>L!}aJ-0lWS%0BNgNt=>^?7>PYEyiyr?-M|Cs zGUnR^!s{J>f{E8W8#)mBPTdY@&?JH)0$pY%B<2w zSrzK?e@&_qLtHbns<8Ra;z=0~No-ZQGD&5~aRXE@2;AU9^vMOifaLyH@iXE2uzGXF=cGSdR_+t;k;{0?Qnt`=KNDAlb?2!-*p)SZed@|()DLR=Tys5FI z@HgiCp%AtOK`aD*w@x;=wTGU()0pv5&+-XH?p%u9OI3-&3KS(D*ymJ2!j6X zq@Xv#6z*`vO;x@i+2$C?ujXR8d%~7ZYC4`FO77MC?*INXO7+s~QZX9(l(EAoRU2kt zoJSG#aLU6(czhf{i=Z!xDLsyE#ztS}8`$=$`$|xM`nnm)gMN@Z7o1g@f&xFEBk6m^ zJoFf~C1@?pvi(qa%&1q9#w|qsZe-WD2Zl3I>?SbF?4T+D!J>v4~XhV^$?f`2@e5o@F8ZlIACH6)!fC9M9Os+DI<&yz9b123@x5j$Lm0!(m zuW)JJ-Emux|HAkYP#s1!>$0mA4O|PQm)pIvP~e2zHE$T=w&TM-QNUPyzE@c zNIsPdCC_(7m6J>mEzE!|Pq`%|RX;(vq`-{T`P6iUmIE)uZc7O0IF>i2g)mGe(u+?4 zBV#AdN;NnM?S>IdmoFILd$c{n~M;N z6>reGgR1u{6-qvDAH{M=?*R_hufG2MgMz#!V+BIC_TaSsh7G4DmzNDQfiFTt(pxh=r1!+iVE$1FK6|+m&O(by2rt{Ai2;McN-EQvw z)eNxlwQZA+)NT_8tSHL}GjsYsSn`r(7eUX}bq?009vB%U<$s>h~6NBadP|uk0n3QhtGDDsn@=y=A{8ge2*_AhG=r z?239{Y#3VN=CTNQR{QqLuIit};m?q2qF9tOAQ>YDsvhxOiM(m}Z;CPzvnH9?F09}U zkXbaZcFWKi=y)&*8tcne(x9^KlNJ5IubZ12y9Rb#wQQxLjl~wkYYe6z2mq#;Dw+^tyr)=6!clI<&t?K zsz+qx+y9<#^9psP1SR{i0e{IkW{}DWhN`Rw*|$>!yZEpK08X14NJottUe`|3eyo(E z`v&bFz8ZRaY0AgB~~Kr1f8WJ3%BV z;3Y8Xw!*?rKlCyE>Dwt4E$WtleO46Wkw%Rkh3LO(=(;gx#=0yg2UL!dW1dV+I9K6m zLn4Gzg6^)0F3PhBimCDEf4%dy$|r&1@R3+BGTlh%c%_kiGDbcFn)A*Nn+LRb&4y_LNzarqliHq{BF3wMjOWbX< z^Gzg!; zls}MNm8M%L66@20i7y82e4sckXwE|5T93vubktTxOxvPrCSn%P{sqPXQ8&6BE^mZE;P865Wn3bc zp+Y2(***HP@{@KK3=lv-z5F~9`r?*PiGRLn_f_fFvtFUpf__5pf@UlR(MbaxY42q~H#J+*0EU%T@&; z0`2#m3ztoh{Oul6S+Na!Im++tJry0*c<{Bw)W28?w(lPf0dLW83hm+RDGt&HyZXoWc51#hY;?($Cs)>KU zE<;=QoP;mWWLv8J^s1N7GI@*t~J)#hY$0VY3APtDoHgh5UCb``=?kq0{#0>ma!F%XN z+j=$5&c))ap3A@v-ETFU4PHR^!2?oKKIazArHIF%X93E~;|r4ozyC5pauc*)DD})m zMR8ck!@~aE@sAs6um^hx{K5T=R(l literal 0 HcmV?d00001 diff --git a/example_15/doc/userdoc.rst b/example_15/doc/userdoc.rst new file mode 100644 index 00000000..8b2b65ca --- /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 00000000..8aea9905 --- /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 00000000..f86fcb44 --- /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 00000000..806fa94a --- /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 b332cf46..6cdf8c9b 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