From 3de10aaf4b7c44efd02528bb9d646ddce9ac84ee Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 Feb 2023 11:12:17 +0000 Subject: [PATCH 01/13] First compilable version --- example_1/CMakeLists.txt | 20 +- example_1/README.rst | 154 +++++++------ .../rrbot_forward_position_publisher.yaml | 11 + example_1/bringup/launch/rrbot.launch.py | 6 +- ...test_forward_position_controller.launch.py | 41 ++++ .../description/launch/view_robot.launch.py | 99 +++++++++ .../rrbot.ros2_control.xacro | 2 +- example_1/description/rviz/rrbot.rviz | 205 ++++++++++++++++++ .../description/urdf/rrbot.materials.xacro | 44 ++++ .../description/{ => urdf}/rrbot.urdf.xacro | 6 +- .../{ => urdf}/rrbot_description.urdf.xacro | 0 .../ros2_control_demo_example_1}/rrbot.hpp | 28 +-- .../visibility_control.h | 56 +++++ example_1/hardware/rrbot.cpp | 8 +- example_1/package.xml | 4 +- example_1/ros2_control_demo_example_1.xml | 9 + 16 files changed, 579 insertions(+), 114 deletions(-) create mode 100644 example_1/bringup/config/rrbot_forward_position_publisher.yaml create mode 100644 example_1/bringup/launch/test_forward_position_controller.launch.py create mode 100644 example_1/description/launch/view_robot.launch.py rename example_1/description/{ => ros2_control}/rrbot.ros2_control.xacro (91%) create mode 100644 example_1/description/rviz/rrbot.rviz create mode 100644 example_1/description/urdf/rrbot.materials.xacro rename example_1/description/{ => urdf}/rrbot.urdf.xacro (71%) rename example_1/description/{ => urdf}/rrbot_description.urdf.xacro (100%) rename example_1/hardware/{ => include/ros2_control_demo_example_1}/rrbot.hpp (78%) create mode 100644 example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h create mode 100644 example_1/ros2_control_demo_example_1.xml diff --git a/example_1/CMakeLists.txt b/example_1/CMakeLists.txt index 24d91c90a..362db3428 100644 --- a/example_1/CMakeLists.txt +++ b/example_1/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(ros2_control_demo_hardware) +project(ros2_control_demo_example_1) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -27,7 +27,7 @@ add_library( target_include_directories( ${PROJECT_NAME} PRIVATE - include + hardware/include ) ament_target_dependencies( ${PROJECT_NAME} @@ -39,10 +39,10 @@ ament_target_dependencies( # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_HARDWARE_BUILDING_DLL") +target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL") -# Export hardware pligins -pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml) +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_1.xml) # INSTALL install( @@ -50,9 +50,17 @@ install( DESTINATION lib ) install( - DIRECTORY include/ + DIRECTORY hardware/include/ DESTINATION include ) +install( + DIRECTORY description/launch description/ros2_control description/urdf description/rviz + DESTINATION share/${PROJECT_NAME} +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/example_1/README.rst b/example_1/README.rst index 5e4db7b46..577421b11 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -1,98 +1,90 @@ - - -TODO(destogl): This is not adjusted yet!! +**************** +Example 1: RRBot +**************** *RRBot*, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm that we will use to demonstrate various features. It is essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials. -The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` package. - -1. To check that *RRBot* descriptions are working properly use following launch commands: - - *RRBot* - ``` - ros2 launch rrbot_description view_robot.launch.py - ``` - **NOTE**: Getting the following output in terminal is OK: `Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist`. - This happens because `joint_state_publisher_gui` node need some time to start. - The `joint_state_publisher_gui` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in `Rviz`. +The *RRBot* URDF files can be found in the ``description/urdf`` folder. +1. To check that *RRBot* descriptions are working properly use following launch commands:: + ros2 launch ros2_control_demo_example_1 view_robot.launch.py -1. To check that *RRBot* descriptions are working properly use following launch commands: + **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. + This happens because ``joint_state_publisher_gui`` node need some time to start. + The ``joint_state_publisher_gui`` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in *RViz*. - *RRBot* - ``` - ros2 launch rrbot_description view_robot.launch.py - ``` - **NOTE**: Getting the following output in terminal is OK: `Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist`. - This happens because `joint_state_publisher_gui` node need some time to start. - The `joint_state_publisher_gui` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in `Rviz`. +2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with:: + ros2 launch ros2_control_demo_example_1 rrbot.launch.py -1. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with: - ``` - ros2 launch ros2_control_demo_bringup rrbot.launch.py - ``` - The launch file loads and starts the robot hardware, controllers and opens `RViz`. + The launch file loads and starts the robot hardware, controllers and opens *RViz*. In starting terminal you will see a lot of output from the hardware implementation showing its internal states. This is only of exemplary purposes and should be avoided as much as possible in a hardware interface implementation. - If you can see two orange and one yellow rectangle in in `RViz` everything has started properly. + If you can see two orange and one yellow rectangle in in *RViz* everything has started properly. Still, to be sure, let's introspect the control system before moving *RRBot*. -1. Check if the hardware interface loaded properly, by opening another terminal and executing: - ``` +3. Check if the hardware interface loaded properly, by opening another terminal and executing:: ros2 control list_hardware_interfaces - ``` - You should get: - ``` - command interfaces - joint1/position [claimed] - joint2/position [claimed] - state interfaces - joint1/position - joint2/position - - ``` - Marker `[claimed]` by command interfaces means that a controller has access to command *RRBot*. - -1. Check is controllers are running: - ``` - ros2 control list_controllers - ``` - You should get: - ``` - joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active - forward_position_controller[forward_command_controller/ForwardCommandController] active - ``` - -1. If you get output from above you can send commands to *Forward Command Controller*, either: + + .. code-block:: shell + + command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] + state interfaces + joint1/position + joint2/position + + Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + +4. Check is controllers are running:: + ros2 control list_controllers + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + +5. If you get output from above you can send commands to *Forward Command Controller*, either: a. Manually using ros2 cli interface: - ``` - ros2 topic pub /position_commands std_msgs/msg/Float64MultiArray "data: - - 0.5 - - 0.5" - ``` - B. Or you can start a demo node which sends two goals every 5 seconds in a loop: - ``` - ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py - ``` - You should now see orange and yellow blocks moving in `RViz`. - Also, you should see changing states in the terminal where launch file is started. - - -Files used for this demos: - - Launch file: [rrbot.launch.py](ros2_control_demo_bringup/launch/rrbot.launch.py) - - Controllers yaml: [rrbot_controllers.yaml](ros2_control_demo_bringup/config/rrbot_controllers.yaml) - - URDF file: [rrbot.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot.urdf.xacro) - - Description: [rrbot_description.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_description.urdf.xacro) - - `ros2_control` tag: [rrbot.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot.ros2_control.xacro) - - RViz configuration: [rrbot.rviz](ros2_control_demo_description/rrbot_description/config/rrbot.rviz) - - - Hardware interface plugin: [rrbot_system_position_only.cpp](ros2_control_demo_hardware/src/rrbot_system_position_only.cpp) - - -Controllers from this demo: - - `Joint State Broadcaster` ([`ros2_controllers` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/joint_state_broadcaster/doc/userdoc.html) - - `Forward Command Controller` ([`ros2_controllers` repository](https://github.com/ros-controls/ros2_controllers)): [doc](https://ros-controls.github.io/control.ros.org/ros2_controllers/forward_command_controller/doc/userdoc.html) + + .. code-block:: shell + + ros2 topic pub /position_commands std_msgs/msg/Float64MultiArray "data: + - 0.5 + - 0.5" + + B. Or you can start a demo node which sends two goals every 5 seconds in a loop:: + ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py + + You should now see orange and yellow blocks moving in *RViz*. + Also, you should see changing states in the terminal where launch file is started, e.g. + + .. code-block:: shell + + [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 0! + [RRBotSystemPositionOnlyHardware]: Got command 0.50000 for joint 1! + + +Files used for this demos +######################### + +- Launch file: `rrbot.launch.py `__ +- Controllers yaml: `rrbot_controllers.yaml `__ +- URDF file: `rrbot.urdf.xacro `__ + + + Description: `rrbot_description.urdf.xacro `__ + + ``ros2_control`` tag: `rrbot.ros2_control.xacro `__ + +- RViz configuration: `rrbot.rviz `__ + +- Hardware interface plugin: `rrbot_system_position_only.cpp `__ + + +Controllers from this demo +########################## +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_1/bringup/config/rrbot_forward_position_publisher.yaml b/example_1/bringup/config/rrbot_forward_position_publisher.yaml new file mode 100644 index 000000000..7ea4fe301 --- /dev/null +++ b/example_1/bringup/config/rrbot_forward_position_publisher.yaml @@ -0,0 +1,11 @@ +publisher_forward_position_controller: + ros__parameters: + + controller_name: "forward_position_controller" + wait_sec_between_publish: 5 + + 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_1/bringup/launch/rrbot.launch.py b/example_1/bringup/launch/rrbot.launch.py index 3ce2b90d8..022ca8f6a 100644 --- a/example_1/bringup/launch/rrbot.launch.py +++ b/example_1/bringup/launch/rrbot.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [ - FindPackageShare("rrbot_description"), + FindPackageShare("ros2_control_demo_example_1"), "urdf", "rrbot.urdf.xacro", ] @@ -41,13 +41,13 @@ def generate_launch_description(): robot_controllers = PathJoinSubstitution( [ - FindPackageShare("ros2_control_demo_bringup"), + FindPackageShare("ros2_control_demo_example_1"), "config", "rrbot_controllers.yaml", ] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("rrbot_description"), "config", "rrbot.rviz"] + [FindPackageShare("ros2_control_demo_example_1"), "config", "rrbot.rviz"] ) control_node = Node( diff --git a/example_1/bringup/launch/test_forward_position_controller.launch.py b/example_1/bringup/launch/test_forward_position_controller.launch.py new file mode 100644 index 000000000..c5e62782a --- /dev/null +++ b/example_1/bringup/launch/test_forward_position_controller.launch.py @@ -0,0 +1,41 @@ +# 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(): + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_1"), + "config", + "rrbot_forward_position_publisher.yaml", + ] + ) + + return LaunchDescription( + [ + Node( + package="ros2_control_test_nodes", + executable="publisher_forward_position_controller", + name="publisher_forward_position_controller", + parameters=[position_goals], + output="both", + ) + ] + ) diff --git a/example_1/description/launch/view_robot.launch.py b/example_1/description/launch/view_robot.launch.py new file mode 100644 index 000000000..5295d4531 --- /dev/null +++ b/example_1/description/launch/view_robot.launch.py @@ -0,0 +1,99 @@ +# 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 Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_example_1", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="rrbot.urdf.xacro", + 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.", + ) + ) + + # Initialize Arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "rrbot.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + nodes = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/example_1/description/rrbot.ros2_control.xacro b/example_1/description/ros2_control/rrbot.ros2_control.xacro similarity index 91% rename from example_1/description/rrbot.ros2_control.xacro rename to example_1/description/ros2_control/rrbot.ros2_control.xacro index 5bfe39152..6c3b3f7ec 100644 --- a/example_1/description/rrbot.ros2_control.xacro +++ b/example_1/description/ros2_control/rrbot.ros2_control.xacro @@ -5,7 +5,7 @@ - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + ros2_control_demo_example_1/RRBotSystemPositionOnlyHardware 0 3.0 100 diff --git a/example_1/description/rviz/rrbot.rviz b/example_1/description/rviz/rrbot.rviz new file mode 100644 index 000000000..0efbecc38 --- /dev/null +++ b/example_1/description/rviz/rrbot.rviz @@ -0,0 +1,205 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 1096 + - 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: /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 + Mass Properties: + Inertia: false + Mass: 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: 000000ff00000000fd00000004000000000000016a000004f0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000048000004f00000010101000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004f0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000048000004f0000000db01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 1470 diff --git a/example_1/description/urdf/rrbot.materials.xacro b/example_1/description/urdf/rrbot.materials.xacro new file mode 100644 index 000000000..eb8e0212c --- /dev/null +++ b/example_1/description/urdf/rrbot.materials.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_1/description/rrbot.urdf.xacro b/example_1/description/urdf/rrbot.urdf.xacro similarity index 71% rename from example_1/description/rrbot.urdf.xacro rename to example_1/description/urdf/rrbot.urdf.xacro index daa838f56..b519fc2e3 100644 --- a/example_1/description/rrbot.urdf.xacro +++ b/example_1/description/urdf/rrbot.urdf.xacro @@ -8,13 +8,13 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + - + - + diff --git a/example_1/description/rrbot_description.urdf.xacro b/example_1/description/urdf/rrbot_description.urdf.xacro similarity index 100% rename from example_1/description/rrbot_description.urdf.xacro rename to example_1/description/urdf/rrbot_description.urdf.xacro diff --git a/example_1/hardware/rrbot.hpp b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp similarity index 78% rename from example_1/hardware/rrbot.hpp rename to example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp index cdf22b62e..46881e85a 100644 --- a/example_1/hardware/rrbot.hpp +++ b/example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_ -#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_ +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_ #include #include @@ -26,42 +26,42 @@ #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "ros2_control_demo_hardware/visibility_control.h" +#include "ros2_control_demo_example_1/visibility_control.h" -namespace ros2_control_demo_hardware +namespace ros2_control_demo_example_1 { class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC std::vector export_state_interfaces() override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC std::vector export_command_interfaces() override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::return_type read( const rclcpp::Time & time, const rclcpp::Duration & period) override; - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -76,6 +76,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa std::vector hw_states_; }; -} // namespace ros2_control_demo_hardware +} // namespace ros2_control_demo_example_1 -#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_POSITION_ONLY_HPP_ +#endif // ROS2_CONTROL_DEMO_EXAMPLE_1__RRBOT_HPP_ diff --git a/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h b/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h new file mode 100644 index 000000000..8d6bb5e3a --- /dev/null +++ b/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_1_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_1_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_1__VISIBILITY_CONTROL_H_ diff --git a/example_1/hardware/rrbot.cpp b/example_1/hardware/rrbot.cpp index 06576229d..40b110be2 100644 --- a/example_1/hardware/rrbot.cpp +++ b/example_1/hardware/rrbot.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ros2_control_demos_example_1/hardware/rrbot.hpp" +#include "ros2_control_demo_example_1/rrbot.hpp" #include #include @@ -23,7 +23,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -namespace ros2_control_demo_hardware +namespace ros2_control_demo_example_1 { hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) @@ -228,9 +228,9 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( return hardware_interface::return_type::OK; } -} // namespace ros2_control_demo_hardware +} // namespace ros2_control_demo_example_1 #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface) + ros2_control_demo_example_1::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface) diff --git a/example_1/package.xml b/example_1/package.xml index 866c80bab..787f446be 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -1,9 +1,9 @@ - ros2_control_demo_hardware + ros2_control_demo_example_1 0.0.0 - Demo package of `ros2_control` Hardware. This package provides example on how to define hardware of your own robot for ROS2 control. The package is used with `ros2_control_demo_bringup` from package `rrbot_description` and `diffbot_description`. + Demo package of `ros2_control` hardware for RRbot. Denis Štogl diff --git a/example_1/ros2_control_demo_example_1.xml b/example_1/ros2_control_demo_example_1.xml new file mode 100644 index 000000000..7a9a82409 --- /dev/null +++ b/example_1/ros2_control_demo_example_1.xml @@ -0,0 +1,9 @@ + + + + The ros2_control RRbot example using a system hardware interface-type. + + + From aa037522ada45d12ff9a38defdfa3f4995afada3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 Feb 2023 11:17:13 +0000 Subject: [PATCH 02/13] Fix wrong paths --- example_1/README.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/example_1/README.rst b/example_1/README.rst index 577421b11..5df8ed055 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -72,8 +72,8 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. Files used for this demos ######################### -- Launch file: `rrbot.launch.py `__ -- Controllers yaml: `rrbot_controllers.yaml `__ +- Launch file: `rrbot.launch.py `__ +- Controllers yaml: `rrbot_controllers.yaml `__ - URDF file: `rrbot.urdf.xacro `__ + Description: `rrbot_description.urdf.xacro `__ @@ -81,7 +81,7 @@ Files used for this demos - RViz configuration: `rrbot.rviz `__ -- Hardware interface plugin: `rrbot_system_position_only.cpp `__ +- Hardware interface plugin: `rrbot.cpp `__ Controllers from this demo From b9af91abd98eddf76aaabcfff7a0b0573cc6e878 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 7 Feb 2023 11:19:30 +0000 Subject: [PATCH 03/13] Fix rst --- example_1/README.rst | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/example_1/README.rst b/example_1/README.rst index 5df8ed055..ce282d27a 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -8,7 +8,8 @@ It is essentially a double inverted pendulum and demonstrates some fun control c The *RRBot* URDF files can be found in the ``description/urdf`` folder. 1. To check that *RRBot* descriptions are working properly use following launch commands:: - ros2 launch ros2_control_demo_example_1 view_robot.launch.py + + ros2 launch ros2_control_demo_example_1 view_robot.launch.py **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. This happens because ``joint_state_publisher_gui`` node need some time to start. @@ -16,7 +17,8 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. 2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with:: - ros2 launch ros2_control_demo_example_1 rrbot.launch.py + + ros2 launch ros2_control_demo_example_1 rrbot.launch.py The launch file loads and starts the robot hardware, controllers and opens *RViz*. In starting terminal you will see a lot of output from the hardware implementation showing its internal states. @@ -26,7 +28,8 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. Still, to be sure, let's introspect the control system before moving *RRBot*. 3. Check if the hardware interface loaded properly, by opening another terminal and executing:: - ros2 control list_hardware_interfaces + + ros2 control list_hardware_interfaces .. code-block:: shell @@ -40,6 +43,7 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. 4. Check is controllers are running:: + ros2 control list_controllers .. code-block:: shell @@ -58,7 +62,8 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. - 0.5" B. Or you can start a demo node which sends two goals every 5 seconds in a loop:: - ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py + + ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py You should now see orange and yellow blocks moving in *RViz*. Also, you should see changing states in the terminal where launch file is started, e.g. From afd375001df30ff5dd3d027f023a98917557dff0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 8 Feb 2023 06:45:41 +0100 Subject: [PATCH 04/13] Update copyright Co-authored-by: Dr. Denis --- .../include/ros2_control_demo_example_1/visibility_control.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h b/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h index 8d6bb5e3a..91529ae90 100644 --- a/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h +++ b/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h @@ -1,4 +1,4 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. +// Copyright 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 9095328f82fb1c4a46e6f50e3ff60ba521c7d305 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 9 Feb 2023 07:07:35 +0000 Subject: [PATCH 05/13] Restructure cmakelists --- example_1/CMakeLists.txt | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/example_1/CMakeLists.txt b/example_1/CMakeLists.txt index 362db3428..49ae6f970 100644 --- a/example_1/CMakeLists.txt +++ b/example_1/CMakeLists.txt @@ -10,12 +10,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + # find dependencies find_package(ament_cmake REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() ## COMPILE @@ -31,10 +38,7 @@ target_include_directories( ) ament_target_dependencies( ${PROJECT_NAME} - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -74,9 +78,6 @@ ament_export_libraries( ${PROJECT_NAME} ) ament_export_dependencies( - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() From 0fc83a1c3a6960b785f75a75b03fe81013586cc4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 9 Feb 2023 07:09:28 +0000 Subject: [PATCH 06/13] Add maintainer --- example_1/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/example_1/package.xml b/example_1/package.xml index 787f446be..49d80d253 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -6,6 +6,7 @@ Demo package of `ros2_control` hardware for RRbot. Denis Štogl + Dr.techn. Christoph Froehlich Apache-2.0 From 52b6842c888eca50dead0d86aec0bc74a59af791 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 9 Feb 2023 07:13:24 +0000 Subject: [PATCH 07/13] Cleanup old paths --- ...test_forward_position_controller.launch.py | 41 -------- .../launch/view_robot.launch.py | 99 ------------------- .../ros2_control_demo_hardware.xml | 7 -- 3 files changed, 147 deletions(-) delete mode 100644 ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py delete mode 100644 ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py diff --git a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py b/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py deleted file mode 100644 index 2dbb17b9d..000000000 --- a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py +++ /dev/null @@ -1,41 +0,0 @@ -# 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(): - - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_bringup"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - - return LaunchDescription( - [ - Node( - package="ros2_control_test_nodes", - executable="publisher_forward_position_controller", - name="publisher_forward_position_controller", - parameters=[position_goals], - output="both", - ) - ] - ) diff --git a/ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py b/ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py deleted file mode 100644 index 7d19ca4c5..000000000 --- a/ros2_control_demo_description/rrbot_description/launch/view_robot.launch.py +++ /dev/null @@ -1,99 +0,0 @@ -# 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 Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="rrbot_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot.urdf.xacro", - 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.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] - ), - " ", - "prefix:=", - prefix, - ] - ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "config", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 519d54456..4116a15a7 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -1,11 +1,4 @@ - - - The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. - - From 35e314319ee0c2618d80e6b760db42fe968837e9 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Feb 2023 16:57:52 +0100 Subject: [PATCH 08/13] Update Dr. Denis' Email and add him as author. --- example_1/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/example_1/package.xml b/example_1/package.xml index 49d80d253..4cefc9509 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -5,8 +5,10 @@ 0.0.0 Demo package of `ros2_control` hardware for RRbot. - Denis Štogl + Dr.-Ing. Denis Štogl Dr.techn. Christoph Froehlich + + Dr.-Ing. Denis Štogl Apache-2.0 From e7d1f6895cf94fded59fed6aa65ebcd3f5304fc7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 12 Feb 2023 13:39:41 +0000 Subject: [PATCH 09/13] Update maintainer --- example_1/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_1/package.xml b/example_1/package.xml index 4cefc9509..eeac22b4e 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -6,7 +6,7 @@ Demo package of `ros2_control` hardware for RRbot. Dr.-Ing. Denis Štogl - Dr.techn. Christoph Froehlich + Christoph Froehlich Dr.-Ing. Denis Štogl From e0a83785f4fc3802e61b79f91cd9cc73e5d2204d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 12 Feb 2023 15:45:01 +0000 Subject: [PATCH 10/13] Use test_nodes from ros2_controllers --- example_1/README.rst | 22 ++++-- .../rrbot_forward_position_publisher.yaml | 2 +- example_1/bringup/launch/rrbot.launch.py | 6 -- ...test_forward_position_controller.launch.py | 2 +- .../publisher_forward_position_controller.py | 78 ------------------- 5 files changed, 18 insertions(+), 92 deletions(-) delete mode 100644 ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py diff --git a/example_1/README.rst b/example_1/README.rst index ce282d27a..05baa7c15 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -7,7 +7,9 @@ Example 1: RRBot It is essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials. The *RRBot* URDF files can be found in the ``description/urdf`` folder. -1. To check that *RRBot* descriptions are working properly use following launch commands:: +1. To check that *RRBot* descriptions are working properly use following launch commands + + .. code-block:: shell ros2 launch ros2_control_demo_example_1 view_robot.launch.py @@ -16,7 +18,9 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. The ``joint_state_publisher_gui`` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in *RViz*. -2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with:: +2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell ros2 launch ros2_control_demo_example_1 rrbot.launch.py @@ -27,7 +31,9 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. If you can see two orange and one yellow rectangle in in *RViz* everything has started properly. Still, to be sure, let's introspect the control system before moving *RRBot*. -3. Check if the hardware interface loaded properly, by opening another terminal and executing:: +3. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell ros2 control list_hardware_interfaces @@ -42,7 +48,9 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. -4. Check is controllers are running:: +4. Check is controllers are running by + + .. code-block:: shell ros2 control list_controllers @@ -61,9 +69,11 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. - 0.5 - 0.5" - B. Or you can start a demo node which sends two goals every 5 seconds in a loop:: + B. Or you can start a demo node which sends two goals every 5 seconds in a loop + + .. code-block:: shell - ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py + ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py You should now see orange and yellow blocks moving in *RViz*. Also, you should see changing states in the terminal where launch file is started, e.g. diff --git a/example_1/bringup/config/rrbot_forward_position_publisher.yaml b/example_1/bringup/config/rrbot_forward_position_publisher.yaml index 7ea4fe301..d0fd6330a 100644 --- a/example_1/bringup/config/rrbot_forward_position_publisher.yaml +++ b/example_1/bringup/config/rrbot_forward_position_publisher.yaml @@ -1,8 +1,8 @@ publisher_forward_position_controller: ros__parameters: - controller_name: "forward_position_controller" wait_sec_between_publish: 5 + publish_topic: "/forward_position_controller/commands" goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] diff --git a/example_1/bringup/launch/rrbot.launch.py b/example_1/bringup/launch/rrbot.launch.py index 022ca8f6a..2c978f9b6 100644 --- a/example_1/bringup/launch/rrbot.launch.py +++ b/example_1/bringup/launch/rrbot.launch.py @@ -54,12 +54,6 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - remappings=[ - ( - "/forward_position_controller/commands", - "/position_commands", - ), - ], output="both", ) robot_state_pub_node = Node( diff --git a/example_1/bringup/launch/test_forward_position_controller.launch.py b/example_1/bringup/launch/test_forward_position_controller.launch.py index c5e62782a..3605c3446 100644 --- a/example_1/bringup/launch/test_forward_position_controller.launch.py +++ b/example_1/bringup/launch/test_forward_position_controller.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", parameters=[position_goals], diff --git a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py deleted file mode 100644 index 92aedbbd1..000000000 --- a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py +++ /dev/null @@ -1,78 +0,0 @@ -# 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. - -import rclpy -from rclpy.node import Node - -from std_msgs.msg import Float64MultiArray - - -class PublisherForwardPosition(Node): - def __init__(self): - super().__init__("publisher_forward_position_controller") - # Declare all parameters - self.declare_parameter("controller_name", "forward_position_controller") - self.declare_parameter("wait_sec_between_publish", 5) - self.declare_parameter("goal_names", ["pos1", "pos2"]) - - # Read parameters - # controller_name = self.get_parameter("controller_name").value - wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value - goal_names = self.get_parameter("goal_names").value - - # Read all positions from parameters - self.goals = [] - for name in goal_names: - self.declare_parameter(name) - goal = self.get_parameter(name).value - if goal is None or len(goal) == 0: - raise Exception(f'Values for goal "{name}" not set!') - - float_goal = [] - for value in goal: - float_goal.append(float(value)) - self.goals.append(float_goal) - - publish_topic = "/position_commands" - - self.get_logger().info( - f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ - every {wait_sec_between_publish} s' - ) - - self.publisher_ = self.create_publisher(Float64MultiArray, publish_topic, 1) - self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback) - self.i = 0 - - def timer_callback(self): - msg = Float64MultiArray() - msg.data = self.goals[self.i] - self.get_logger().info(f'Publishing: "{msg.data}"') - self.publisher_.publish(msg) - self.i += 1 - self.i %= len(self.goals) - - -def main(args=None): - rclpy.init(args=args) - - publisher_forward_position = PublisherForwardPosition() - - rclpy.spin(publisher_forward_position) - publisher_forward_position.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() From 70d2691d297b74f843a2944b72ccc5482d9b19eb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Feb 2023 09:46:02 +0000 Subject: [PATCH 11/13] Fix trailing whitespaces --- example_1/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example_1/package.xml b/example_1/package.xml index eeac22b4e..b42c85c92 100644 --- a/example_1/package.xml +++ b/example_1/package.xml @@ -7,7 +7,7 @@ Dr.-Ing. Denis Štogl Christoph Froehlich - + Dr.-Ing. Denis Štogl Apache-2.0 From 0521eb943950523ab7bc5e189d5c5a4ec0edffec Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Feb 2023 17:53:26 +0000 Subject: [PATCH 12/13] Fix rviz display --- example_1/CMakeLists.txt | 2 +- example_1/bringup/launch/rrbot.launch.py | 2 +- .../description}/meshes/hokuyo.dae | 0 example_1/description/urdf/rrbot_description.urdf.xacro | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) rename {ros2_control_demo_description/rrbot_description => example_1/description}/meshes/hokuyo.dae (100%) diff --git a/example_1/CMakeLists.txt b/example_1/CMakeLists.txt index 49ae6f970..f7bea298c 100644 --- a/example_1/CMakeLists.txt +++ b/example_1/CMakeLists.txt @@ -58,7 +58,7 @@ install( DESTINATION include ) install( - DIRECTORY description/launch description/ros2_control description/urdf description/rviz + DIRECTORY description/launch description/ros2_control description/urdf description/rviz description/meshes DESTINATION share/${PROJECT_NAME} ) install( diff --git a/example_1/bringup/launch/rrbot.launch.py b/example_1/bringup/launch/rrbot.launch.py index 2c978f9b6..504fd93b2 100644 --- a/example_1/bringup/launch/rrbot.launch.py +++ b/example_1/bringup/launch/rrbot.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): ] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_1"), "config", "rrbot.rviz"] + [FindPackageShare("ros2_control_demo_example_1"), "rviz", "rrbot.rviz"] ) control_node = Node( diff --git a/ros2_control_demo_description/rrbot_description/meshes/hokuyo.dae b/example_1/description/meshes/hokuyo.dae similarity index 100% rename from ros2_control_demo_description/rrbot_description/meshes/hokuyo.dae rename to example_1/description/meshes/hokuyo.dae diff --git a/example_1/description/urdf/rrbot_description.urdf.xacro b/example_1/description/urdf/rrbot_description.urdf.xacro index 19a0c59c3..8c0673208 100644 --- a/example_1/description/urdf/rrbot_description.urdf.xacro +++ b/example_1/description/urdf/rrbot_description.urdf.xacro @@ -144,7 +144,7 @@ - + From 5ba106f7d44fedf08edb342a5e6d76581f77f6cd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 13 Feb 2023 17:53:47 +0000 Subject: [PATCH 13/13] Add image to readme --- example_1/README.rst | 6 +++++- example_1/doc/rrbot.png | Bin 0 -> 29549 bytes 2 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 example_1/doc/rrbot.png diff --git a/example_1/README.rst b/example_1/README.rst index 05baa7c15..1ba724026 100644 --- a/example_1/README.rst +++ b/example_1/README.rst @@ -15,7 +15,11 @@ The *RRBot* URDF files can be found in the ``description/urdf`` folder. **NOTE**: Getting the following output in terminal is OK: ``Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist``. This happens because ``joint_state_publisher_gui`` node need some time to start. - The ``joint_state_publisher_gui`` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in *RViz*. + The ``joint_state_publisher_gui`` provides a GUI to change the configuration for rrbot. It is immediately displayed in *RViz*. + + .. image:: doc/rrbot.png + :width: 400 + :alt: Revolute-Revolute Manipulator Robot 2. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with diff --git a/example_1/doc/rrbot.png b/example_1/doc/rrbot.png new file mode 100644 index 0000000000000000000000000000000000000000..9f5980b2c9ae3f77956c2ce3fbbb5648b06794f0 GIT binary patch literal 29549 zcmeFY2UL^a*CvV}q9`KL6lqHDO7BPskrKKnU6B@$9zbgBG$~3CO%ezlluoEhQ+fzR zN}MAujSRqav~08#6clti z5421uD9%p+uUi)>fp@Sr##ewJm%JWW_)<_@ZU6f^mn3ra76k=2g^rfG*^7*I{7fwG zqcE<+Htb^^ItAl~ZKkmrOexmuceK6R`98|%3lc29JDOjA2z?bwsrG8R>yZ*GMsoHt zT^*In3O?uJ4gQ?3n6HTzEcKapy7VUDZeLY0I~k z1HzV*2Gq#P=O`#z8=DzJ0nsQ#Kgt8L->2|qqM&$5F((MTb*|kQ_)Zyt0=}QW1_qwK z$Q=u8=i2WEDz)I{%m62)tH4LAJH&=f&f|~c|2I$HXQu|X`SO2$`u`a4KXVBGp{4)O z(tq5zzo7I7zd}XvQjyd*bJ{0T-Z&IS+FtA#vkl&ek(-WC;idQy1z->EWV=VA<#f9z zjGZ(WEdfB=ifPGZ;BDZKWAgZ-3B_lgSjyJjX5f)Ccyq{+&p|rH7q5Q-VF?HIOCi$F z?9-oIBrI3xRtKy5D!>P) zs*)6`DS+^mL8m{*+*u$(pZi(AfE zXGoLq7e}>rAqghOe}8KUAt%K(2hQ(}*~O`fQ>3Q<)!E7N416&gP7W~9C1j%$hro8^ z1POckixhVRNB_g`{EvP4zjY&b0JEuH(WH2Jq+_n+-WRucJiDP6)u zmwXUC6GGa9-#RokY$hwI9d@Ot+@%mq1TJY8sRw=(0N;0p%bhB<9FNr6oo=TPTF%Bi zTf!tr2h-)JsLSkZ6p6Y3WHnAky|a}AwsETGDSku) z$Mr9q=HLG*fCW=f@cs9v|NkbJk3ISQX6!FaxB~7Oh&}#~(doYvKmC7k53#}ZN869! zNnv}BuwCaUn2$Vm&r#e*+j=zX3x$&A)IxVgSjj|BT+4~vLDuZq7I`XktKwuSA-h?h zqE#O7HB&o2JBwoyXQsM@^z!B#ht(eF-AC{}U0niAdGjZe<2~|GH@$5rAIX!vF$cax zvB(HM_q%pFh$II;OIY3*YdJoF4~K1in`vo0>6f#zJN-TK=&XZ$I0;zv2LXWEg#nn- z;(O3hJ99LH+uViEI=7r1CrCV8rfiL@k_2q)e{8Y-LK=Tr$r)SBq7?J4F}*|xaAotta}?5HTb zBIIektWz|`*7kOm2TUYFYSbV?G7#L^G720Q@NCuGftmui@A57p?>K1_0$x>~5Gsjr z`3hbe6_-!;L#bv-`bEToQDz4njX&SebBhmdK3n%57Ll#!^HjWMnrTl_kSb5R;PF1KseAVpQ=YY3syl*~ zA_8?kR{Dnugc@t3PyKT78u--*n9wd3)m1#Qy1$Q;eF9U^`t_^$HL%1R`yz@y`N~v|zU>`P0dt zwieE>&{YZht4tIu+U(R$J_L8mtIY0KB}7H@8`aBqz>=3~ zJ>!F6PiLn&OVdPkk~9oi&W2QicI6X_FH-pM$5P@h8|Jj_i|gQp#;#a>V&lw<8ijjU4o;7xfTL+O zo)h#cL(%y}`xE#I*_bj3_tTM$$}YHJUNq$wfcASK!>&9ow-a?84Dr#1G$|N@>Yamj zSG;d6^@Q$mGMUzvWrOhE6vF^r<6Ih|^XZX6y93eJ@p1l!+}IjxZEY=}r34X9hfDX) zR;m-*%_xe1ydg^THkGwTEpIinY(l6Uu&09=vLmL4Ibde&&r;pThMM8kv+boSIkbn3 z0im^5nZiMXo%(8bqq+yf1vVz?8i;T-_;%X`j8|SmYLM@aInLT$*3Mn_);;EzArYun zS)tvKd>KK6gN`eNMU^86O=Nbvbu}O~OPdHy6mBZ`Ckqf#OuSHEnHD1K+52f&yxcz$ zxg!5m5q3TN`{@sr7~nFtE=Hohz)V(noeYe?j2-tgejJ_pOUwf|+d4`>$=y52V zr0#qgp4twn=Tr%rDr3mpaAYz)C!Z}(yN5%#XjoH{1dIJE4P=P+RJFaK$kDN`--yoIuq+D?&ldWIIL#oF^hQp-3xSL>;8MVSQubBe( zH(tt{DN6e3o2glavQo)0)5EU{Mvaut>s)MS@_ECit zuiA|Te?AB01`8PDnzD*zcxgcOxYdE*9HA=tmcSVG-H-;{o_oiO31W5?UQ1Dopi6a; zH&%lvIgVa58W?%C5Wn%nfHrf~{Nqkq`SKfool+Tvi%8tWNf3aA+%$874#EjciqGX0 z7FcfO4*mq`kL9Co$|N=NKD?G`-5}wn^bEe6a$pP9(WkDv;mx$|Q=;y1j&nkcFqEeL z2C4VswhTviL18anp+aMB62W$8r?yQY=qn#^_uz!mka7jxc@)8`Xbk=(HLgceL4 z%nbONxpNd=_Oq0eGen>j;M%(m&;CI;%Q?aGk9}x4yq1!sOjh`YNkyC`t*5gc7 zf}TAjc_^%rRZBp6RuX#@Gb8Qi+4tbO>8nqXC@%|Om8$ZmN=6M$O{By5IKWG~R=dJ2 z2a~0476mK9Wbr0{h!bm`-H7EWn;cyU94|41TrzQ8ITV7MmCKv-GEIG~kpi zlWw`tvc|@Q7lZubBR86EL3|HRrVn0*M56BU3GMnAq*nL1_oOMiyT7fhf}g#EeXWF+ z0gIk&l1GA?R3}_VqSb71*7&K$YTNRe;LT-h*Qd*X`uVPdpHf;f+Voddql~Tk7j$fh zi|eJ_9CiCcuQI;$f_dAbZ28&jL-YY_FCHmAkeG0tp?IYqf$~xWR$bE91x(Jg9P1eR zu9;b)wnxiKJ!rRD<=4c){p=CfvQ}K3M}D>6QFYP)SJ>egz!YBatBLK=o)=eVe@7#g z4^tq0$F{yN3%+H;DYn~3*7YLvFJ(A^cuq>h#ZcnDX)Dh?JhE~VG2P3v`Md~^y!=e?QU zKgdZ*hSPHRYt-f{kYn;%8rl5-{N`(?iZd4906Zha*UzAzE4A0qUidCRB7$AwgQ3h_ zxmyVZgikw<@G|U=ck~k#lXIu1l%a^u4=e9>mog+OE)#zsb{ueKkCnZd!t`jHs-jF+ zzKqvoSgecm8iVK68^j=2Z&q9J2j%2y{nnWJ=pF{ZLiFv}@1Ed4HcHYKi!0#Hz5m>SL^^mk6X zakAT!^yF7((yIHM?GE@i8DAEAohmH=P%006m)3o%XClU!seW>&cHlfJ&ani0&o6jj zdSQo+_I7NAD328*U7|T{!LtWBrZ<22prK`D<>d_u%YaB|=)~C6<$k^o7N0OmfinE4 ziFU^e_!^{NqnffHx(ooQn~Y$Kw>!5M{uy2gl^_7_YB5!aRQ22Klb#ZW*RRgqgjK77 zL!$m|sXIu-e>X?xy6J7HIYF5Q7q>g-s&VklyuqHE1}Bw%dPTNMK8yO`6hO0$exW37 zT>`*RU@4YsVZ~SO`B-5wc(}_az_B(x{GJzyG7~IPCk|1%$}ML!lWJMX65XTFp4C`? zJ&fC^-?*4F?NIRAA?QzzSy2=Jrup5lNnba>oAKEtFU!M$xnd#^s0R(M^7SG$$R{eP2ncF&Et1*EOi@fA+SxW0Y$G-cYn%7cn; z1uB3B65D{9c+uYr+l`}Ap3Q=f^FMxbkozIlJK88CLzi;4kJfzooz0wF=Eoq1^ zwm9g;do?j99oNXosN~{o5dCnk7yAq69-ZXi zKP_FNu(>858=*SMa>c`knF8(p$V6M=1gV z>mzMld(zH)tl#Y2Onec@W%CJBoAaofhvJtx%qgV@NfoQA4RBMw;KiK*HMQN!fayTz ze$A(|w65`11t*%tPSuuqiRQ(Rd~ga&c&Zd`jNOoQu&&oZoc(q z^9eDBD~*FMl}Ryu4}W17;yV(?gr5$ZkOrcnw0fPPp+tE*CVRjlPLrib+=!#ruWUBg zLN}YT!CXC#jupHkHOL*2JJDzw4+Ns!Op-hH`jgqxNR7XA-c}sbM3T;vHcZIPyT_pz ziPzYL9clnTJT~T8VqP4lb|y$Re#TK@!>^csPtT;GG3cy_z}Ks>j7}6){QWj{B6|nG zGgj2&l%oeZxBIVvtoWLB?sMea`s@Z#P6x+vwj3?R$))!Jln8)!XWQZ^zplF*aKu@< z>ngKJtmwhR&q^({6)Tr&R)=t}++-pPKLB$*uw;Cf>gwvTXMax9S! z;Prcm<;F#Qg8pFU^B3RMIGU{|xqxscIXT*1Oez|Ubq_Orm09pJM!q`_kjF{z8lFSX zPXKcZ2nw&Nm);6+QmmhWz_;-|&ZlXqIyn+PABp)<}V9ogEe%$gae6>(EO z{3NI6Ux~=a{EC%CpQidipWxuhe)6WM4xV#2xJZ)OVpqN0%z!^ul1@mvJ?8@GtG`;p z>g58oCd&E_c~dCs^d~ReTSeJFvZA4MW%|7|PXksyVE!l4G|)&N{~YkPouCHkIYH-p zf^jCJB5K!+f^Fd4k6*f4bLMey@Ry&ii!#Req(sCHfas+mDj41od{qy>qSUWjrrD~Y z4RCcLURJ2yqnIal`w82^1!0kQR|U&7O=QYLA}8d{Mnzio;`5A(d{O$L(K0XMuIiTc3z?t-NTOSkOj{Q&o^FCk7g*Tkf+E*?F1 zyz-FZmADE=DB%At-&pHG8c!T#XwnT`SR&(IN$ypB(W`Zt69l`&-x91*H?I5j67v>J zF}~Vz`0fI>VRn5mFWo2e1ExN3NE(t;9zy73T`^cQ(uU|+eiDwp;L7z)dDjr2u_q}W zmcPCTTa|~L9W954?e{Bf_67<%c;n4W6WddjzJkl1 z>AcaMY=XGV(G}U2KKy2i_qmk*bwMGHNwi}C1mT|zF)TdpXf4Jzm~ z*7kz3*cALheqyw8gJDifH98e8b>rQze7{`#v6V0mn+?1Xy)YHYT;ugjC@;Zq*IO6)MPWwLpx@ncS}!- zA^i%@IR}M-o(z7NhE(cdO51>y ztD34$MvXcE2j3?LZybMqeDHcLFbu72c=vAlL`$k^*&BEVCaGAI%!JuBdxO+>@KX{E zJm;FDUghV+PFi1dYdKsBXXL`^@MolCJ?%-0N~$h(>A(bHd&G1W1ubBfR;<3=GUzB2 zvm)Tpg|3RJKmr?6fH*L6f7`_n-0%GK(z)st?ujCv*nc?b5|Y8?g}z-MUnn}NmRYO^ z%ZA@c$(8KH<$Fpr>IZPvyCZ~SN1rJO!CGzjLjMd36kF8siIB9+7>)GQ7((;9u)mxX zs8V3U8%3(7dy`Qt3k!S5XOgZ_SpmWkg;=i*M<5F71d>2a$t@{iC>C&ZisqZC=Y7^w zR=Cs#ShegAA@R@tav+&DqR+fbmRtu`XV`4s#rBnlD*vX9v=4K(Y0F|+QVJ<%P9^ks zf3yLY=WOaXDwf2g*lE5zUE1_JpRG$T=cwS!dR-PZjVUsfJy*@86?4~q@urql`0?lc zvelxZI}N@$cka=ote}(z8LJQOZs}&PifvFKR#3kuodvB@=CJQa2xr{6WVwvMr|BGh z#yNC8JJNTt(o1cB`SpVG?<|YME3WuuWZgJcv8JK~)$oB5AL207R~LKvB%~X*<9$~D z3$WR>UpSK6fdo!B#F&IvcHBMRpKgyHGQFwY6l{<~mYlz0v#y_v&VTchs*Wf)jvw{f!bt){D7Km4#NG6IAeYc{Y7FwE z`$7VUUcvIlIP=&1E8?3wLbyI%oHQ#Y>cZ+TnzKZCHci?-@X5zP)$h<2-{2EygV1o&O=-1I|L6IY8tfs5YrwMbfg^;(=Ls_wD4qujvS+?wLZoleMO}EmxiM7;Q;ghy=q^xz=k~Saa?#H;0h~xNKBYv3 zC%}d_19A6qVSnb}B_QCs%^H~WGv6gzkta`7=OH@%FH>0_b-T~r#9bQcBLy5Jk?P6j zkSSp|XL!*muc>kJsMi0r;s)%+z#l^JUazsCV>|EaFEMAPl_e)*i`{r!a;~j7N*LD% zd5}jmo?F??9lT;AzQc~NUd__=@H0R@dz+aPc5TD4Kln{LK7W&P6@>pA=V`CkX8`=0 zF#U%#&FeZz^r#$PkZt?oP_go|NzbHKd^>SyVX?Uzc2kEv^Mg*-l|HzMf4EG*=Jq4Q z>>3XH6xGmGgP((MlGll(e;cnrq!?C$3_Eq$~kfYqP2l z4>I(sayL;7)+v(2UvYd~nBI<+g2t!kmrxGi^HF*EXieW?yy(UQV%3i1)5~_2$&pS4 zR_dhbV6{yH-S<}gqA{Y1r3opy9yY7pFsWg{roNHXP!Xv7c#R$DW_9Dw)Z!|As|1S7 zuxpti+mQPzY5B5&idCg6m++r5MlV%c44bCw1-obi3=@KDB%eu4+Dt90J=w=h`Jp_npk!D`Bgiy;CMU?LUoFG1{E=Wx${ZFC<7T5KmVx91shZSK~4R)H@@UW*l2bJ=oLNB6^~w8H%cS&?yIKg90W9Z zAc`%^Pj?or3)7Qtf=?>l$HvC~DNyVkgK92^q;$TWe2WRlF5FM@_Lt0eRdi%G5@YPM z4nAQD@-Ny<=8bj|=l!#8yRS2uODQH8dsX^}yTnvkAi)My5bbB~(|;wF<>0FIX*9x1 z#8}DP_hj8~rQ1*`&S|K&Z8wtUp+|Nv$Ao#63lG0>y4i!}8y3>O7Gf%2c>Tr}`O5^1 z;p`kZ#0v3y)1#R>8SS47oW*fWY%aY+xW1)gfan1ErHzeJ#>9N_2)5#wDy7HcoQ0>v z8MG(=WWcZ_wpUDeRT*| TQ)$g?~3@eE*1acG;_=}4y4C^DY*N}1c530R)GcMGV zN@Xy$1b;$iB(4ysP~_lYf3xDcKtu+lM<6)mpxqZuXZujh4s(BvBI5*GVF|p-pxp4! zSW$o93@_mP++PSoH$OEhs8Kp#PaT`Zy;}jkJyHefK6P=6MkN?l`+4FvGHU|ns_LR8 z<1c`Y+-BY#^w%s_miK=5^MnBj=HkMdFZ@NmJCC%+kb-~Hvg=m;KafYo6}zYkzQUsc zo3U%)xxjNJ4jCsX*Con^`OFqp<4`>*$w15Jn}`ncdITREUN|W zkFKFCZ<5x>?BGC)VX0>r&@1(~b7X(*#O)7JtjV9J*YK4`#Mzah>07t)T!>o32Tk*i zWhYOfRO2OgrCiBtdMPIWmHD|~9&~^YpeGfij$=(sT%0{EJjqW3lc<2(rF+lt`?zJ0 zJAt5i3CVoJ#euEi(kdl$Q>hD9TOWDm-;-A3n`L>>D^{c*a2-6!pVuR*X*eDO3zTNm3R@!%xJXyXt4&FHyO;+2!zCsibuM*;AZB< zsnjNbuA{|u#1^F;*}!WLa>9p)>2-`fge0v4T?ho**DI8l3RoRy#!RFkmE+H+;pB1? z-J2c?nq72$Pyy#*Gbj6QmyMMSI-knw>QV8@NK>IICQx=^DJWnhPVGE4M8D1-COrB% z3yn7)R!M4UFj+M=)T8kTUx=-{>xi+qY5NJV+=VuJwI2(yKY}(2m?q?mM$_AVpjMt& zN|QL$X;m!RI+_%w7)+sRc)admVi-8r4zFPwQR9WrD~ndGrSP8g`32Z`Gq%I3>$p-7J`yQ};f{ll1u4 z%F@w98KjlTXxo;FAWp0!h&1;%MF-b{5o&Y<(iqRrHWPmaLSDs1VZy6uUW2J@Yn zN9ZQsz6d9gHhVndIHC~S)0TI%)jUd9bOpoX|kk;?@&0LAUKkX8Xw zW9+Ju6-fPBZXokw+X?4z%A2t| zfX4a(>NsHjN+!e&W;bA5QmpP#bJg892q-Ef4*i2jn=RxX2clvyC%eAOXS!bV+AqEr zZgR3SEl)PA2iWC$YIk6dnZ|DatU-1N-HQgIO+6E|RkCbipZ^Bk3|nD90J{%y-K8Zb z_O=hhFL8h1|BqW3v-vIZ^Zuzlvuk1okEYh!)56r3{ID=(=h+(ny)lHKbOa3wvQG~(!;De;`)r?^VExjt0w*5CNRNYor zr3z?adS;<45c)JN2p>$()l=FL;|N6*KQAw6;$#7z2muw6k2@qy=aAfStI?BP!X$y% zG;}k&Z`4&&BDfB}6;LaDLcn?6YJDziXJgm$(PT)nmK#V8PAhixo&$1Xd_eL-#o_k_ z9U9`t#IEbYju<{y1iIvwyV=*W3-x>mfBOv=HC}$~!xjNwHxRTo<$=)OtQ)^ctLbYT5oPlNsb zs(a{N*W>Wf;cCaG1eQ9aGj#OO?#nEwPWLw^tk;J8#_B7 zP}o=UNPhhTS$R|Ny&-*zMMj;*V#wUmuYklf$6HSO$$R^9KGHAma#$9eK?2U0AY=ob zs9=d+$fGhe6mTd&6$*3CeCg!#PbWO|>$a;<9vNh%OCIS%&Qi+R#|;*yN|hv#!FGrNj0oxBaN*_ABe#eQO^}oyRtbs${-(L@u(ml&%m{t?tta zL_4jm@UEi3?p*P^E5--lDM?2cMDhNthIc!5%H6NKbcy5XB~rY8IE$EY1kulB48R@d z-)3hvQc3vOE~bTEcThN$D}3vLPmlFWQOo2r(vb8T8afTsw+ zbF2QlCOB*6ptVR;poJx^%7TkpZlYt%oH=CA-q5bT8zPyURb#fy;@-;?V% zbHF0$Qo3k76xtp*ffb^e6rZ@wRYivwW}nrh`5jtiNj=8+yjiiGPsw^eqht!ikC8U) z4`ysL9=%%RrCdz|hG&jK0<>y*6AGk+Fz1rw*$QJXfTS9NH+%B9OdRhyy-N1FK+`$S zT!O09uYxs6-DrFe9Q{%;OVko`sjV05p)+5b@w5?LFMJ@w1b#L=@!7@c)n65wKe?p7 z8{Y(?DXhxN*He8nB3RsPc^|LGB8LAVJ}PkQUL*$IlXjWIFbRSFxe+=2Vs53xAvGG2 z&-IDHi+ZA>b%A&KX8o@8VwF=v$&EJKU?9y9{ZcsEoZF$YJKO+?*4_N9!({U}Y6%3= zyJ-Baq(|g9p^Z_Q;-sO%+KY3p(?A$P$kt3ECHe!pV z@fjp;B*6407$jR%ZhOOmNEbkq`dQk^dqD`JbHS~)-@Uvy`7Vq`Efix}R#;Sg6z!%(=Jp)W}xj5ueU(_A=~fEtW^xbOn5nQvJVJekg=SmGnY^DZ`UfEb z`bab-$|lEjDs*mzkUQvlC8Ha9a@!lKtGEOEK4h9-!8Yz8VwsHpnSDf6bhLpmi+^fh zG?Of~!K+JX$I>KdxNU#Fy{P~!C{4)Y21f@^dWId&(r7p3nYx53P@StDd*xYyb zmq`Ld-sE-FEsW5ZsktV4EZEN%8=I$Y4B-z%->u;Kw6-vG9y_!bP6-5FVLN?XuO)q<3WW_tgsc{0_;JWY+%p)3-$Omj>oWJwHo=s<90e-z}I zz>^<$?e(tZ4I+9&WDg87{7qG&oAUX2BSsfi#@8==+h8?r$dViy`b!8jsa$n+SV|)$ z@z4$r1=GH9_9)awWrBa=U>w@-w1@tqh6v6|gVEf+lxyhid2DyamzqB-L#Boyl;glb zQq2?6(`zTh=+d1?E5;{19>uTmGIgV7>#sWvk!TGJU*GS;32fMTcknAxr`X7coV>yA zs(`Q)1;ogu5qg4yyT{Lsw>}j$lwM7?**9%s(Wbck9it(OnEfo*bjQYnX&@?qFVz%j zTGd<&WzvJ?h&dM(HiZ2*MXTS2{xMc2Ke4c4^^XA~v`i$29FaBmxS8-!y=$ety{1#7 zk1#g54`(}ED#2@eGart6;QMRkYDe3PqjGA&M-vjwN3#lD)*1ez9UPW1D0b2@b4oeZ5l*Dzo&s;Fk*~zP&RvCP^!?Ds68FlZ33Q;jN@Gk78W9wUsL4`;Cx z1mR{opF#5{LxlgBiW+vqimMv%EAIefn1AhEwP_^XG+5FPwDi1wuYN_u+ePUOK%7{r z_)s{@;`DR@2(I8-@dtNMv9u-&zPf?${Yym!JgiuHsVerD&(fsMFjLcFuRbsj5z zk(4n?Bjl29s$OM||Ma~>^;5Z1z_wTobE%1H+>P_Y&}vUDTcmKNza^JqB*Lk_N4yTG ztRK%WgaJH!8 zC+Y$oAzbOmmFQPoRg1k3?zw@vR^M6xp@})k$IaPaLm_c*l;P2@#a{o>wY_liOs!_C zCNr&LO!{VLbq6ntwy-!5S`aduE-%m?=R@?;JW3Hut3!|WrW-`@i$gyJjHRMbp$*t7 zXjzsj$$u19ZCPqwUef}dGQAmhW2x#d4lLDr-ou5(M~g65&UW1n52}T6EvFrD0y^)| z?L`J(DriU~n3q5iM>Fx89@*i3dTfEzPHf;4;t0b-SklLb_H_Cg z=t^*}fky|1+f`-u1{eo5zuc(eSnycJ0#^Qx#>^4J+*5S#8<2~d1TwYE*T6W^&C0($ zb|;3eH;P5gS&BI3>CXo zljg(gx=Adlpo1cr&%(IUNx;QK7lSZrkr;5S_wHMQB({nht*IwBMGvh!T0K6F8FBRs z4tA?E<)hBPGfa+4EwpffQ-(sXUEaNSZVb?4 z9?~JjUj*uQ7B#K}u(--dbwQB9)}q68qa=cPIkc>(WEgVWTRObHLK8c@-p7kk`08-c zsyrN>h&#T`&?AW*;t(SO{Zv&p>wjC_8$Qx>^NsI6a|$OmRP8(wgdBVWLdqZrZdg$p zArZS2uW@i4B9ORY;iQqEpF($_+3n2HZ(rD7Iy5SH1T_5sK=ZfQR6aI)((QE{sC=Ec zgCwpSsmNKsM%NeZG1)1{ddLXs29|$gh8Uueo|nV}lJh5)5_jrTSdkE+&8CB$GmER^ zkKXru1no$>17^{1%?Zp^GV@t5Nugvs2JP;A5nD9?ThZvfX}q7TKnRznAF0>7`=xZn z%ex$A$zoB({nr(vW`9q3ZvGl6J`pwK0vGoRY%mX&3o}6=IigyJk;DT{U7GG7k+8Td z2&Vx8>Z~;oz+0D&wA6B?-Wi5UrC)XznSd)N zG94_bv)IJSB9-B1qHPePG$VWxhOi)S2>@i^FdKyRton-cf~+4J>)PB{T$G^-8-kRmQd(s7DNs9Hqw4TBry&A#sGxN{p?G^|{X|D_jT zJgBKgzbdFncl~9EJ%P~cfL**X19^16^!{FELRY7;se^!sIKn?hKSAK?1?Z!edOPlj}rALga z_FfoP$J^?#@+QY7cf$(HliL>j80kvpl`PcBuv^QQ-Cdw0fYd)r)w$niy(e*K#2yMb zv#$#p$G$QOTBe#KAaBgc1%3llF~_F*zE9r9RFhFo{X;n14`j4tv;F})rK@UD`7or- zTY4wIXhfa~;{4d`nIa5&9*YuQ3(r*yfO1y-ZLN_|vDtQc%wmHm;4h|hjqoQ}@4cTZ z5P^nOf=s_UKr^-&neg;KZ`ys9s%7q*YIUizgJw%MiwFori$l!)#KTqZ;QFPes&X5A zEH|QxR#0_yz6TtOKH>}zNVs_XW67k~$G88fe4s}ke=h@06;WSmWEF^6Y^(NeOH|zKtB*-$rW%;V{V3hDX2_= zt=-$B#F#YkaeAF=HvDu6Tt#-+_$UBzfNlqK1rXQ8ou7@sX({r4Y{w$1YGJsNbaWul z{QWnv428oET7RloSBggj9mgl>9cX^iNiK|V2T93UPo*zk1WpY-*9{xE{n_6&^ukv? z_EK<*c&(Z?)N}r9*$p4CImE8Gxd8nh_Q3rBu@}@7A17kD=m8wS)=9S(HI)S{P1(!Z z>Auad)J5;A3vGEp4SsKsP!0_DM3|969G{|lx@kkCcybx2a-8E_t(*-;#630eP$XL1 z^u%K`Z9duY!f;e1idrcLVyuQB8uZjX)5Fpu$?)I+oV|t5a)9!Lw790*9XidLE&0pz zMf*z&d-k1AuGZfj78!j0;Soaa?jfXC?`COZ<^A7|Iepw0oPwU6@3i60hm?E2e+IKhPhFHcnt&SWm=HY{?y+qLoQCAH1A(PRqdXHhA*wJrb zhJ)%v*oW(oyxS2q>+73m-vb};kICf!XeAS+_)7b~w+E0$O)Fe_3J6ZzvY#=J;seiY zu3rO#b_D1_c2{j8T`XDDtGsOjDn@OsStbKGufIioF790s8|o;{cfckad3G2t>pgrF$59(Q4aE z^3@j;Vu^M4kf7WI~m5viyj`k~C zPL>R7&-C!sNagdEHDN6Uj$BduK z%GKaOLECUEp2r+w_t$_aN)U((w%Si(EIFxXYthhWv(n3}g48yLbZrbRbol(eIg=|3 zMvbEHWv240*9_?C4}{ffN@An#Q+l)6$?UZ~>B=cILM3yrdU2ZEu@<3CcUu8=c%*;lg}mB3+-6;(~P#JlVRzXSGO5<*Zr8fkwWTB23zh(8t*tmD+WuND0LIO`eNLF<;Yxr#FThDbwsoDC{t+Og(F6AD*pP6spdI_8PEoQf z2=!Oq9-i6frE(egD3YuGThr*@5#31Vn5#>9Q|VV_Y~)8L1hdg>RKniUS7U{`KPS6P z_?)&r*qGGfnN#KsC>69|+OAv~?hbZ@!QYzxmbQ5@YRkuVX&V@@D_8+$kVFZ+IcLkW zF@;Ac(ie6fzS4qSdB106c&PxmWZ$E!6)~NNm#f*vH&uZ=QRd^TNzd3IcpYH{f$HgM4Ge5~s&i@m?xamU#V-46JODikI>H{HxM@(_t7x#VBpI zvM9~)8I}GZZIjXcH&5pI4`0omkXShf^=s3rqp_`Z{}$I%GRZ0 zLEnqVXQJfYd~zU>%fSzpOfUA{0xjO~2vTpr!gC&5(Vi79eWgtTE^@3QS8+Sqc6egc zRA!v&hK1^F=qwj+Y?iu2Er$pSTBjl22zm9qt{udLU%s7V;(!f>d8*{0qZv9h?b(b5 z?!<=CyK#tJMir{5EFK>Z#N3mxJxu_xxs?9$^F?%O+uggo?IymQ*H=L-F^-F)rlxJ5 zo^4T?-Hw?&eR!NN_(9(FN>5Jei)N{%)vc{90^z{W&~VgtkHq078Z$48)wNNk|2cg3 zBM4OYTOgAV6g{IoeL$R}$|=wA@UXQE26fT$Ahn?krCr9!n)hU+gge~;XM_GzVl@Ap ze$ddN?KkbZCQIM*-LK46L_ZYrH$BCx8_ei&`EDPulCF%k^g)EdNAGi{Ks_waJe)5H6Al+&@*5=iJ}mu1UQIctzeH*FxE}QxvbXYF+kQp zN~|ZRcsclSx@ROx>`t`#Q;)U6K-CDPj%}fL`WcQJ%-+LQ*TD56*L|-q(j7Ql8rb1Q znCGtVf*AN;EeofrS!fVeNJAS~A?cmQp+*^u^xko;mIs22{O!jQMtB)giHj~ziJ}IPdL4rH!!8Lg5w=yE2agJ2F$c@E z8~@ay>NuJ;29Z9wAh=v!VBSL>5Q@07bBgGOWO+hF7BC1QNuBC-; zOkKvK(qBH8aVvtPA$=3YWeptfU)iXn(soZ4zVC7|WUHRWd=a?Tb9~l60*iaD^3J>+ zYn0@jxM(C=tP&CXA?m&sZvI8WJ4fO}&=Sp$aYIF4^Ryqx$2Hevzel~*@h!iy;07!s z-C?X2>99%HZ^9+@{gQ~AkWvBHZTWt_m^q%emgSEfj72AlKKPwXrs9}Ar{i)Nt z_R_!BF7xAt$dR0qd)DN*pMaaY*i9->gxCtPE$~5PJm{7%K!vBW!Uo_L#jy`Y>-g+pW3v4=A-`tQ~J=?>)D6 z-SgPoVJ%Zmp~r#NBdYJ8|X{ZC;3h+YIy)%T#gYJbZxe;i#FSsgG;*Y~Ff zy0{Q0BVGc&mITH4+?nl(^cNqUukP@m%L)fp3aTE41w8&0RCIDF{=qzb@*((Uy`53Y z&E61kz6kp!uPy7vXEt<~u!-DJkg00qhvfOCqEH0qM2z9huR3n{o_a>?gM7CtSxfEf zStY?W-D^r=@5|vO_p1sX-GEd+40^ouJ*c~kIZ4~Dw07(0jx5#;hYv5!Pp_I9mI)LO z@;3bQsJ!{h{whG-0i?DIJEH#p66J|-{d60Fp)wAC`ec^Ut)Ru+3^6gFc_-SQu|ozQ z`YQ<2+V`It`|^LNzxVx8WQ}30*|)~NG$cfrK{7IyvKJvs_PtPH7&D9|Qe%mkOm^9d zLCQ{1V=W;|_AHSQ>3h81pFiOH6OV^;&htL^xz9P*b=|k;Gu%F}`8$@Zb6s2K&Ap#9 z9Yk3rJ423Pg}ZRKw>RakfAZODY$>ZL)aj|QsD`OKzb%89F_?axb7C-U)u1+M1zXMe z?Cl&AZXl^0Ubq?B3_I#o!OiUfm&<(R-f9ZTHXodLF<1V=G0ZDc;E^Q3mncD)Co8aO znWo9TR3lcSp!NVnM-KEGEK|AsK2$NOO~8m_cgJ=+(Ol;V!rJoZn|e}Z{j?m4rXIM( zQE2c;fsIW%(8r^&_dm~HD=R#!`g(h4=wE5y6BWq1(Dsf$VlQvw5jwEg4B?f=Ogw4oMXBwjVz6&`-~dKc3BMnK}rl z%xk}Y``gxjyQoC?6&SX`->J-2 z{oYmXr(ER-{0$k$Ob2RIcis91y;hhs(M=kzalg$T#tx z$X3zbb>0#C22_Rz4pPDG^CfL7wVU+$A+f@Lu>3_w%H<#|*^F?{Xt1DHPDBs7|3j;C z@yENftW6{SP{o4dict|dFw#X>+x+p=n!x!D3Cq`7qdLsj9 z;}OPIjaBR{4$M%Q9cuMiKeJ?_#&Upt(#ezPAM-Qq%B%q*pPb>`7BszJ2RU5%!6WD3 z@7(OC$`GjQg$?)IW+;|UuHj=d9v)V6b30lmVql@~)x)urKfn;ehR_5SKoQ7Lu>H%I z+rPTE3f9*(rry#7jhbmHXCv#LU*hQSr9Z_^vpQbE-2q|QM}8c+%kREnGvPHdjG`l+yB(y`}=hw8Yfg|PBsbIUyaCuc*zms z57K2Cg3rgm2&vl9SPGzIu+y4>gf%eJ0bw`egrFxXw|!SUcTYKnacb26Ee6%fYcszGTfcF**`k{3k0pOA=xt=s zgPsZjFx1{NRg`G*wvMDBzD@n}$J-1o<3-gdS}{{{Y6_2xy#9N{@&cB;(7oHb0Q9WG9&(ud;Vs9T&n< z_<>7L%N}aRZlyx=DfrnxhrZ4sQPWQRA-1qAd zJ?vOas4o<#bb7pLMPS_NEB*%M=h?id*uwR7T#F~S&6*TpUUcIXUkuE}SzbzYjP9nu z6BqS#kyO3xTZvn#7Qla4lAqv3U%RzF-8oT$yHr8%%C)L^XHo4b(HIYP1%xHAMP5-C zdIJQ~Rw@Z}uqm-|zw!AlacxOYP@(gqQq7!23`~5P`l#+P(~ams57~ppRdLV@;Yz!Z zlTd5Gy?4B>=+`g+ZQIzZ>@yw_{3S;YZ&ycO0Jg_-iHqYa5anS~4~I>Lu5|gERBdNy z$YS@Sk~M0Ay-)j=fp`~Ob?EU*qCkTo5ZmDM{RW!&LNwxeh(@2!MK$}wTZv@-2HUYU z1qvY3^ABK*q>wZbRKqoBKpIj)6Wn>LToMVLpX0(-u~n7X9FpXjI&P9&vD` zFc6PXoFvLlobF3%ypaDNa4PGk~-Q{NMBDf{f z#qm7Px3wj!VWeB;eE~*?@C{UQw7Am7$p(sumarMRzC_PQTQuQEIQIYvuPtytF>PVKB}v9`TA-n{*a4;~|m6B_x?2jZ@1H&ET# z&dI|$t=5GEPCd;)gY4AvhMW*HDL^L9QfR?^3_Gb}%A%UpMRyDP4Q7N-OSXCw)OcnV zUtG9GufIv^c1@;Y@ssc2NwgIzpSo#4iVUlPM) zXJ^4fQu52P?fDS+GHZe_UPb=t@&JH@3f?Tn&eKDr8jKTz*oXw)MK z#f_Y9j*vGk;&wltE=m;L@3ov8e&@$EcZQed*mMmp|I4g23}GoRp)ggp@y^BFtJ#~P zI23%m$4016FFGPHSXQZ_R&nzbUrFfqOJy6?r${pW0FNYWFNjrzW1eeRhDn)?C{U<5 z$|aks0niHS@%Uu~7&nO$s3?Z8bwwjoqPEeVc2NFywFhIhgQg&R1H_}xfKSWO*FpuK6pxHOKHy5_1%sM;W& zI^Q%Q#Qs5mTLLd$=bz*VFvZS~&+o?M+WNk#ywns(xVOw7q8EJHdh($5&5s8#vah}E zpnWlxqM3?Jdq;FD1@$*gUR~1gkn;s)*{P+qY;B+J82fXv1bSNB;X*Q8V4>OvI9Cg} zqFrAqosBzliq-F(|K-tKV(!8EB|%h-sHQ4aA^vPQR30ZDiyy_|^x@^aworZqzdI0) zrahareWn6B-b=UBo{Ts3UCcISyxnm7{G*OoMpCzdm0p+}$rIU4>XztOQeCOMfRyl9 z_C*RNx1FMyEconIbp*ez(6aS*8<8%!-0qFUOAt*Ad62>p`mN(?FPMd&+azjGxq%=Q zvlu{@Rcc8J`YespPIr>C6Qo%|t6`h|H}aqEKf3+VKvg@amR#J(9T2R0bnogNQ?K2u}&SVqJ~>bc>Y3F{DV7x4l8!wkH6$~FWQ#vxaD&S+*hu#Lq(f+S=GOP z2Z+f3B?Pr~Yass4$ceL!|mvk6u9)i2I#x ziZLoouHV2Sma=xbomR(4>QD=@XVrsVsI&Yuf%72J*+MT|E?V;Kh%n9!bbi>kVGl7< zlE9jf8++e}8<>SS%GvSI;J6B{fGN6a^u}7OogWgK^jaKCzQV|`=?S89GL@fxVxl$6 z<-_Sw5C$(~y}+6Yr@P81zVdmeFZdU}@KG%JUL0H7C1sBO7v{X*!5jxk0nc-)iBkq$`Umu5gqAp8NXG9k0MOFCds9=VRVeg>iE!0 z(yWRt6zNW_9rY;-uT)R4N`*y%xqu`cuCxQe|(-MCwkl z#8--q08aSbMK&Rd&Ol-zS{?K8xs=U<}0WkyY~eTXiGPQw)HaD?yYV#%75rm>!CZR z|0+>j*GwUuTx@L#Vt{-u0y3Io*j1!OqE##EL6N@rf?T{fY!RpCGw4Z#q$ATr)Fy<6b_Lcrsq)uTW;H8)P+>X2QszY~gYv0J}?4;!q zVJm>=S(nKw=|KzY{fva~GF$PY{u|fVODBWi)g1Xbf;}*G^T^o!obpZI-ooe~RnF{B z_q>r2mZfbD1=ro-Qg{;7i&}i~YVwDok*_;plvRx7WbAXxon@FFlvZB0sruWTgJpA- zqu$Ge2Pq?em3#C@gt6}p)_`A(7&ph(V-{Q28Ho!@y8pP1`7Y_L*`bA_*aRG%-Aq2v z0b4M{3OE_>8a#S8f;%v$L}NM62*`A24AgxS^6Gy7{5es2{onHRbY8sV<`%u<8%UZi zG0=mjH?Yx@FWLpO5Wp8wH(8;Trghqa6Q~QY?i;uEcX!u7F2@XM5BMCNh=bU!i0lR+;SB=f1E_xYZlq)&|iFzGMw(|>N zdC|m{NE*>tj$LePi$aolhz{e@0xPfhrsUI)h2l9;{10ISVuk8(ZURCune;|OfB_x7 zk8UT-RvW(9R1FT`7{!S{j-mnbHDgRiwMMTL7N0?)0stePAw2u4P29U2y>RL&$Y&(x z`1Iupxx1pFU8@4+kZ@s6C4`GDmi%n~;KJj8Fc!&0fWdxGJyeN~U+}~K9R0F|XhV)m z#swb;?HnF=dvpI?^Zt!$n56&J)&X7m&4}nrQDK#Ndp1yNW`jQthA1O&BE;sL)rRB&EZZ>*THs70UWLmeL&$dR=5sNy%n8X!RVotE_vhG{?&PpOtY% zLU-3#t$p#Hz1ur2E?zf$=CTRlF#C8-_&jup9!FE)cmF~nb z>q(0LP1d;jC&(A~TXc^-^1xw>mc%P2>YU5N~x=$6jmRLi&-?*7?#Ldep`%8oxRDEd)NX& zRh>D0>;i`6g}cSdD%CRNoM8`hJNo>Wz;A{JP9)a3H0xu>IQNgf5ukzoib&zJj`05X z(en|J#z>!@SAc|fNC!)xQ`rj-)8_hz=CU;(mjw)+LQC7H=1(GNZs2x#)MN*E(D&|$ zymK@tFM1Fo2oh#LXE2THy@FcoOlA}f6Alc_E_RX8deYAP69z`V&Ld8A!4VY^{A}IY zqdCiuwF!SBbk8X;(vH+F@2;SIDa*iOILHN|zq58PNv7~S#TG5;6W_FCpGiHsv`ndr zm-&p&*gnuletvShUcrzPWMJTd)r+7~#C9lHtd~;v=jEDn(RTlpqivQNuW|vI`C9&{ zkxR{@F(2ybfCvdWUULZa8P_~aGfv|M55x~IL&S-ZW5<5Q{;5fiDIk2L4 zQ}0Q~*Bp>B(n)6>)C#R*#|-5mepL2t9+(Yx#&=p7;TBa9xn5iP0s z>Pcs79f`x(jYmj~ID#cAo?PB4t2ZH9xL&S6MGRn7*jf< zd9Zc7bJC@n!l2!;NLozMreYBN#N*ossfqHM`tVGHRywG%)Ia8N|LGws>lG;i35Yf5 zgGfS8&NPTcLmo#%xPtK%9dv4FoAkV5D-}yF;?%QJ1zW-Yim;a+&qSODk;@k9GOVM4 zc?OOgWYlMAJ3K>a(;<>+Fp3euGqL)^)$H#Hg&OL$XP+|0W*4!w-e!s9A|^a(+pD1W zJ)YCpkS7`AL5!(h(#b#{RD1_Vwe)&Ing&jj12O!`Zf?#F4Vs`Nb9$J*9jdz_gJqug5!v?|C%oQ<>K?i8<+jr8v(k`R8j)58t3h$nE6FHasx7Sb?VO{!GoIE z$X5F6>hTX=3&^dlbE}h)|3b3sUL%FQ%LpXx7!Qf@;D$~G3JA#QZW8jP7gbozk?}=+ zapPs`Rw`^oA2!zDQ&QUd9xNO|URKX0T7pPWMu_D!zDm~6BG*-Pa}5FJx((Om9) zP`PRHz7Pzx_hgYpMWMy^-tbNtQNRGjJF_>*W0-f}c3|YLtq}5fZIFsMAj}|WBVEyZ zlv2vTWMd(S+Rc!O#LE5hS9uBT%dqGE?@?{apRku;4mSQly!TCw4==Nh;z)pI$(mkA zwPEPDX&Gpotc`cbeNIb85nt)g33a!oGltQw`dYm9++SRJ2QHSpm2U}6E+hW_E0pk7 zPlIQJtaYz%>1y=m`Jnr{0ni=@BEbBWtkTi2rg<4O0Th2x5#0$jwH>)84kMWrafK10 z|M(YKzj24&I#Ln+xwBD}6b*We6VC=34>-&Z(JRX{ygUKW;*)1KV9TOJT5(e}ekqOr z3>cBe6}{aWBwtFP z+W6d#r1ZQvwS;fiY=m$tLYK~TMD~1?R!VJf&ME!ai&md5C>)#w6tbVN&7WtNW zNPF-eCj=Lp_t4_F)aCW$`uFmH0NFapsh`ivjaETyNUiErA@3i zyMadSJAJt42}i+;G@e7btA6+LwOQa!K5M%FFM6%AIr^{WLP`v-{@l%Y=lnIS><34= z~4f*-M`^s$mv<7k`;HdGsnEidZDnYtH*ke)wBhI80mQ?K3$3Dv_kgGKvEi z-E^K@-aQyr9)|!i6?OmiX(LN7Y7wFC`iEN>8yvZ1x8oaifdymA2fCQ8+0pQRSI2!+ z$d(a3g|HZToVK{(|JpysJ>w|trAALY=&`K{mwUJ@*m7;?_1L^G{y0ejPpU7+IGLmr zosLy+0g(4qGx?CIL%KBdB@8Ff;ElY+X7|seUs8whWU>TaAG))64o}MCo(t4e`d@Mi)(*h%ZslOq~6v ziC^n#_P=e9OSqzkx<%~PuA~04roLnC!IF`ZDdfC+>H#&8g&qqei=a{5{6CelI;)7P zywn1gAIjxR@fatPOgpLA9`QzF*?gw_v>tTZuKrqWJ~}h2OO!$oIb_WU42VkfV17ED zlqKi1`CiDVSvhNyl@lMTxI71v1qT9dR&#DzCj_fu%XQoEiI*4j;YeF}$Q2o$BO_HI z2y`WQZ>UwowDtMB4?m?12EU+XWCeE@ub8FZaF)BKL(t3!pUcn1mt<I}-@2ZHk?Mvc@I1EmrXUHeyXdD0GvG9MdwTuAv|h@WA%WkOlJ@ z)Wc9kuhAb9Nmdac|40g5@}g!(Gi(W(N65O!5vgv8ax47-QcgSh0SmAAJNu5GM8ib4 z>}cAC){_;DZ2w%RMNXHdAqy^1iEDR1>b~uoaGIZ0?d)PZjj+tOs0PNsehL@(WeEvL z{n$$-ex=F7B%SPB<%j zf&_8debstN`30tdT+57&Jj z5s`-_*7q}LCaBDCCXzY%i#PHdxi|Z`Ez}FH5qp;CH5_u*{e6cg@H>BIIQKL4%P$~a z%26NV0oGx4FDfXeG)~rjT1mm4(?09%UOrbP|FmS72FVx?Sb`mXbzrumhagZxH@wy`%gogp@JgAoqfQNv;R){}u|9YggSqc_`N-#SR8mPaA z>Bs_hwTb6g%3j0Lsq9!_jIU)$`+IftVp~}=eYuZH$Tx^-Kt5A$XHF}Ti3S%ktb?`F zE*{jK;^(>Ixu7ovq!JX1wNo|h7bpeepuuzBw9NX{3$g_K8qCo(a*Usj^)9c^|GX0> z<9_`lx~RPv=@XcOI+7s0mk?L`Ex=rRS|W_1r*91!4ZiU!yps-&?oPhw?UJ|4iwqT^ z9!ZI6J(OIQOxU9Mb4u?O9*hZ8z>Qcz9NFKHk9nxT<~H$!lo38LMpI_}a3`N>6zB5r zemIf2GU+T{UydN0nc_h`VH9-PKoBLaof!5v2yD|@Qa*mWm1{O>*TIAbbFEC&I>Cd= zxo-Phj6kWYKJuXp;I2_enE=u!oV=aZj+QYz*X0gNJKE5*0lF-FDrg}T9JqFXsXwi} zb?V>S;F<&g$I?rhfYZU@l0HW+xu`4IqIGd?e!j5oHNtWTGfdkxlyc%~dOqE7%nguE zT2F&QygYM*1xNToxMD3DjoDxIQ!f^&Yu|6?4(QKriR=oCR_xjiKF zB>6D{16k&(-&5EM&nK{+s~_(lf?|~+$2SR`0bYnoj3^WOoHg9jkX2uk4_nKWV>ok>P@Ru?jJCBC!Ey z+D@~YnUpq3< zGcK7~@;IJfe}#-bi7{8vG7=hGfdDx`PlK2=!SLS5f)8J0l?Hih9|RB(2;^Xe7J8l& zuOWas>=4(zFMB9vqy-j&y7$$^srk>$G<(>zS*FKK`r)|ascIrIz?l>FgBO|=}!y|&Gx zYsbar7h!Tql*0z1%O%wzj09m#QUMze`zfx-O!es6JQTk%=NKWYB+8@ah)stR2Zp!_ z8dM{N+qifsC#Qv5BH(#i>bW8B6q&4&iHhyGGpL9yagl)FGd*|`i$k<}XLrh{)L3sM zEn{Lq(Wyy+AvJzY+zS$F@f&Gr*cb_fNLeJc=%FcX5=eM1v!&r^A z2fu*O@zQoTX`5ENo<)=yamw7i3}GuJcY55A2>cf^?tCmIM$zukAK&1TCJxMu_^+oG z#6M>hPP}2x2)Qleu~5NQ3CcIGJD;?Mo=$elBa=f#4sYC`LMj$NtE>s{AU&{dJGjM+( z6`(U;?|wYd;h{iMn|^Ww_KO5oC~W+BBp(A0R?wtL0l;6lu`v(Z-a`4F%?IbaxF_TG z?)|(#o^*OwuX`1q8LQ!C+O|5L+3NJPh)IwzH6&hp`6drrfIX{)eFf@WN%Rc=EBSxGb@(^@G!fc>z){9#jmFP~JqU90XbT8%6U!#SZ!(WPfT$nWV@?`btpdBgcPYGvR$BltPfUK!? zr*T+2<$#*SzTPeIcn^X1|C5T-h=4i-YYRQocT9Y}{v}IJpJgy$s?3m~<^FGKg~{kd z{Qc?;_JqKLug>{u6)cbceB;z{cZALTJd>L~j2>$!G@r?Q08A+}tX|CV_iLGE8W;AW zdf2unleGX<+mlorbQqDsGD9AR7pu6o6(c=tXKmq#%-4ED;59Aibr&qfyGUUrFcB~4 zoH#nlw;OdC_cP^j8q#2z>+?xRRZvWE)X=-~yY@H*Gx?_F9Aa?T%TXwVE6thjv$QOi z)Z`EdAb=2^+I>{0iFN{Xn{r==922bvQ%kB9SI4R5Gs%P)MS+ECSl5fn^Cg>`i$y|e zj_?ZVsLEPjzNZ#ZRt8A4-iXBWioj8`-6XA~lBPGvPPtYS^dSB6(Ozg9c}DO}TH4q8 z$KiXx<=D3j`}#e2YHUElK`$#tlL!hKF|vNmCHA~@q*79GqhY;Qi4O7FjS;u(937o3 z)f$Il0P2NeJ3$fS)uhGG7HG(hp