From a09092de0735662614717591240ed3864c5d5e4f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 8 Aug 2023 10:49:34 +0200 Subject: [PATCH] [Example 12] Controller chaining example (#318) Co-authored-by: Christoph Froehlich (cherry picked from commit 2968016272f9be66d52e5d3a6bb1b8a9516e1f28) --- README.md | 4 +- doc/index.rst | 3 + example_12/CMakeLists.txt | 115 +++++++++ example_12/README.md | 5 + .../config/rrbot_chained_controllers.yaml | 44 ++++ .../launch_chained_controllers.launch.py | 51 ++++ example_12/bringup/launch/rrbot.launch.py | 115 +++++++++ .../passthrough_controller.hpp | 89 +++++++ .../visibility_control.h | 49 ++++ .../src/passthrough_controller.cpp | 184 ++++++++++++++ .../passthrough_controller_parameters.yaml | 10 + .../description/gazebo/rrbot.gazebo.xacro | 38 +++ .../description/launch/view_robot.launch.py | 99 ++++++++ .../ros2_control/rrbot.ros2_control.xacro | 37 +++ example_12/description/rviz/rrbot.rviz | 187 ++++++++++++++ .../description/urdf/rrbot.materials.xacro | 44 ++++ example_12/description/urdf/rrbot.urdf.xacro | 35 +++ .../urdf/rrbot_description.urdf.xacro | 119 +++++++++ example_12/doc/rrbot.png | Bin 0 -> 29549 bytes example_12/doc/userdoc.rst | 182 ++++++++++++++ .../ros2_control_demo_example_12/rrbot.hpp | 81 ++++++ .../visibility_control.h | 56 +++++ example_12/hardware/rrbot.cpp | 237 ++++++++++++++++++ example_12/package.xml | 42 ++++ example_12/passthrough_controller.xml | 9 + example_12/ros2_control_demo_example_12.xml | 9 + 26 files changed, 1843 insertions(+), 1 deletion(-) create mode 100644 example_12/CMakeLists.txt create mode 100644 example_12/README.md create mode 100644 example_12/bringup/config/rrbot_chained_controllers.yaml create mode 100644 example_12/bringup/launch/launch_chained_controllers.launch.py create mode 100644 example_12/bringup/launch/rrbot.launch.py create mode 100644 example_12/controllers/include/passthrough_controller/passthrough_controller.hpp create mode 100644 example_12/controllers/include/passthrough_controller/visibility_control.h create mode 100644 example_12/controllers/src/passthrough_controller.cpp create mode 100644 example_12/controllers/src/passthrough_controller_parameters.yaml create mode 100644 example_12/description/gazebo/rrbot.gazebo.xacro create mode 100644 example_12/description/launch/view_robot.launch.py create mode 100644 example_12/description/ros2_control/rrbot.ros2_control.xacro create mode 100644 example_12/description/rviz/rrbot.rviz create mode 100644 example_12/description/urdf/rrbot.materials.xacro create mode 100644 example_12/description/urdf/rrbot.urdf.xacro create mode 100644 example_12/description/urdf/rrbot_description.urdf.xacro create mode 100644 example_12/doc/rrbot.png create mode 100644 example_12/doc/userdoc.rst create mode 100644 example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp create mode 100644 example_12/hardware/include/ros2_control_demo_example_12/visibility_control.h create mode 100644 example_12/hardware/rrbot.cpp create mode 100644 example_12/package.xml create mode 100644 example_12/passthrough_controller.xml create mode 100644 example_12/ros2_control_demo_example_12.xml diff --git a/README.md b/README.md index bfb7b9573..8a1276e73 100644 --- a/README.md +++ b/README.md @@ -56,7 +56,9 @@ The following examples are part of this demo repository: * Example 11: "Car-like robot using steering controller library (tba.)" -* Example 12: "Controller chaining example (tba.)" +* Example 12: ["Controller chaining"](example_12) + + The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. * Example 13: "Multi-robot example (tba.)" diff --git a/doc/index.rst b/doc/index.rst index f081dbc84..dc58d0467 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -67,6 +67,8 @@ Example 8: "Using transmissions" Example 9: "Gazebo Classic" Demonstrates how to switch between simulation and hardware. +Example 12: "Controller chaining" + The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. .. _ros2_control_demos_install: @@ -254,3 +256,4 @@ Examples Example 7: Full tutorial with a 6DOF robot <../example_7/doc/userdoc.rst> Example 8: Using transmissions <../example_8/doc/userdoc.rst> Example 9: Gazebo classic <../example_9/doc/userdoc.rst> + Example 12: Controller chaining <../example_12/doc/userdoc.rst> diff --git a/example_12/CMakeLists.txt b/example_12/CMakeLists.txt new file mode 100644 index 000000000..4637d22d0 --- /dev/null +++ b/example_12/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.16) +project(ros2_control_demo_example_12 LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + control_msgs + generate_parameter_library + controller_interface + parameter_traits + realtime_tools + std_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +## COMPILE +add_library( + ros2_control_demo_example_12 + SHARED + hardware/rrbot.cpp +) +target_compile_features(ros2_control_demo_example_12 PUBLIC cxx_std_17) +target_include_directories(ros2_control_demo_example_12 PUBLIC +$ +$ +) +ament_target_dependencies( + ros2_control_demo_example_12 PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# 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_EXAMPLE_9_BUILDING_DLL") + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_example_12.xml) + +# Add library of the controller and export it +generate_parameter_library(passthrough_controller_parameters + controllers/src/passthrough_controller_parameters.yaml +) + +add_library(passthrough_controller SHARED + controllers/src/passthrough_controller.cpp +) +target_include_directories(passthrough_controller PUBLIC +$ +$ +) +target_link_libraries(passthrough_controller PUBLIC passthrough_controller_parameters) +ament_target_dependencies(passthrough_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(passthrough_controller PRIVATE "passthrough_controller_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface passthrough_controller.xml) + + +# INSTALL +install( + DIRECTORY hardware/include/ + DESTINATION include/ros2_control_demo_example_12 +) +install( + DIRECTORY description/launch description/ros2_control description/urdf description/gazebo description/rviz + DESTINATION share/ros2_control_demo_example_12 +) +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/ros2_control_demo_example_12 +) +install(TARGETS ros2_control_demo_example_12 + EXPORT export_ros2_control_demo_example_12 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY controllers/include/ + DESTINATION include/passthrough_controller +) + +install(TARGETS + passthrough_controller + passthrough_controller_parameters + EXPORT export_passthrough_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) +endif() + +## EXPORTS +ament_export_targets(export_passthrough_controller HAS_LIBRARY_TARGET) +ament_export_targets(export_ros2_control_demo_example_12 HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/example_12/README.md b/example_12/README.md new file mode 100644 index 000000000..e6e0bbe7e --- /dev/null +++ b/example_12/README.md @@ -0,0 +1,5 @@ +# ros2_control_demo_example_12 + + This example demonstrates the switching between simulation and real hardware by means of the *RRBot* - or ''Revolute-Revolute Manipulator Robot''. + +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_12/doc/userdoc.html). diff --git a/example_12/bringup/config/rrbot_chained_controllers.yaml b/example_12/bringup/config/rrbot_chained_controllers.yaml new file mode 100644 index 000000000..e85198763 --- /dev/null +++ b/example_12/bringup/config/rrbot_chained_controllers.yaml @@ -0,0 +1,44 @@ +controller_manager: + ros__parameters: + update_rate: 10 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint1_position_controller: + type: passthrough_controller/PassthroughController + + joint2_position_controller: + type: passthrough_controller/PassthroughController + + position_controller: + type: passthrough_controller/PassthroughController + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + +# First-level controllers +joint1_position_controller: + ros__parameters: + interfaces: ["joint1/position"] + + +joint2_position_controller: + ros__parameters: + interfaces: ["joint2/position"] + + +# Second-level controller +position_controller: + ros__parameters: + interfaces: + - joint1_position_controller/joint1/position + - joint2_position_controller/joint2/position + +# Third-level controllers +forward_position_controller: + ros__parameters: + joints: + - position_controller/joint1_position_controller/joint1 + - position_controller/joint2_position_controller/joint2 + interface_name: position diff --git a/example_12/bringup/launch/launch_chained_controllers.launch.py b/example_12/bringup/launch/launch_chained_controllers.launch.py new file mode 100644 index 000000000..d12717700 --- /dev/null +++ b/example_12/bringup/launch/launch_chained_controllers.launch.py @@ -0,0 +1,51 @@ +# 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_ros.actions import Node + + +def generate_launch_description(): + + position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["position_controller", "--controller-manager", "/controller_manager"], + ) + + forward_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], + ) + + # Delay start of forward_position_controller_spawner after `position_controller_spawner` + delay_forward_position_controller_spawner_after_position_controller_spawner = ( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=position_controller_spawner, + on_exit=[forward_position_controller_spawner], + ) + ) + ) + + nodes = [ + position_controller_spawner, + delay_forward_position_controller_spawner_after_position_controller_spawner, + ] + + return LaunchDescription(nodes) diff --git a/example_12/bringup/launch/rrbot.launch.py b/example_12/bringup/launch/rrbot.launch.py new file mode 100644 index 000000000..51e8a88c9 --- /dev/null +++ b/example_12/bringup/launch/rrbot.launch.py @@ -0,0 +1,115 @@ +# 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_12"), + "urdf", + "rrbot.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_12"), + "config", + "rrbot_chained_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_12"), "rviz", "rrbot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + j1_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint1_position_controller", "--controller-manager", "/controller_manager"], + ) + + j2_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint2_position_controller", "--controller-manager", "/controller_manager"], + ) + + # 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_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[j1_controller_spawner, j2_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(nodes) diff --git a/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp b/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp new file mode 100644 index 000000000..08ec81496 --- /dev/null +++ b/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2023, PAL Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ +#define PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ + +// system +#include +#include +#include +#include + +#include "controller_interface/chainable_controller_interface.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" +// auto-generated by generate_parameter_library +#include "passthrough_controller_parameters.hpp" + +#include "passthrough_controller/visibility_control.h" + +/** + * PassthroughController is a simple chainable controller that exposes reference interfaces equal to + * the number of it's command interfaces. This controller simply forwards the information commanded + * to it's reference interface to it's own command interfaces without any modifications. + */ +namespace passthrough_controller +{ +using DataType = std_msgs::msg::Float64MultiArray; +class PassthroughController : public controller_interface::ChainableControllerInterface +{ +public: + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + PASSTHROUGH_CONTROLLER_PUBLIC + bool on_set_chained_mode(bool chained_mode) override; + + PASSTHROUGH_CONTROLLER_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::vector on_export_reference_interfaces() override; + + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + std::shared_ptr param_listener_; + Params params_; + + realtime_tools::RealtimeBuffer> rt_buffer_ptr_; + rclcpp::Subscription::SharedPtr joints_cmd_sub_; + + std::vector reference_interface_names_; + + std::vector command_interface_names_; +}; +} // namespace passthrough_controller + +#endif // PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ diff --git a/example_12/controllers/include/passthrough_controller/visibility_control.h b/example_12/controllers/include/passthrough_controller/visibility_control.h new file mode 100644 index 000000000..84888056d --- /dev/null +++ b/example_12/controllers/include/passthrough_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, PAL Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PASSTHROUGH_CONTROLLER__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 PASSTHROUGH_CONTROLLER_EXPORT __attribute__((dllexport)) +#define PASSTHROUGH_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define PASSTHROUGH_CONTROLLER_EXPORT __declspec(dllexport) +#define PASSTHROUGH_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef PASSTHROUGH_CONTROLLER_BUILDING_DLL +#define PASSTHROUGH_CONTROLLER_PUBLIC PASSTHROUGH_CONTROLLER_EXPORT +#else +#define PASSTHROUGH_CONTROLLER_PUBLIC PASSTHROUGH_CONTROLLER_IMPORT +#endif +#define PASSTHROUGH_CONTROLLER_PUBLIC_TYPE PASSTHROUGH_CONTROLLER_PUBLIC +#define PASSTHROUGH_CONTROLLER_LOCAL +#else +#define PASSTHROUGH_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define PASSTHROUGH_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define PASSTHROUGH_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define PASSTHROUGH_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define PASSTHROUGH_CONTROLLER_PUBLIC +#define PASSTHROUGH_CONTROLLER_LOCAL +#endif +#define PASSTHROUGH_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/example_12/controllers/src/passthrough_controller.cpp b/example_12/controllers/src/passthrough_controller.cpp new file mode 100644 index 000000000..92660438a --- /dev/null +++ b/example_12/controllers/src/passthrough_controller.cpp @@ -0,0 +1,184 @@ +// Copyright (c) 2023, PAL Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "passthrough_controller/passthrough_controller.hpp" +#include "controller_interface/helpers.hpp" +#include "pluginlib/class_list_macros.hpp" + +namespace passthrough_controller +{ + +controller_interface::CallbackReturn PassthroughController::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +PassthroughController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_names_; + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PassthroughController::state_interface_configuration() + const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::CallbackReturn PassthroughController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + command_interface_names_ = params_.interfaces; + + joints_cmd_sub_ = this->get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const DataType::SharedPtr msg) + { + // check if message is correct size, if not ignore + if (msg->data.size() == command_interface_names_.size()) + { + rt_buffer_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + this->get_node()->get_logger(), "Invalid command received of %zu size, expected %zu size", + msg->data.size(), command_interface_names_.size()); + } + }); + + // pre-reserve command interfaces + command_interfaces_.reserve(command_interface_names_.size()); + + RCLCPP_INFO(this->get_node()->get_logger(), "configure successful"); + + // The names should be in the same order as for command interfaces for easier matching + reference_interface_names_ = command_interface_names_; + // for any case make reference interfaces size of command interfaces + reference_interfaces_.resize( + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PassthroughController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // check if we have all resources defined in the "points" parameter + // also verify that we *only* have the resources defined in the "points" parameter + // ATTENTION(destogl): Shouldn't we use ordered interface all the time? + std::vector> + ordered_interfaces; + if ( + !controller_interface::get_ordered_interfaces( + command_interfaces_, command_interface_names_, std::string(""), ordered_interfaces) || + command_interface_names_.size() != ordered_interfaces.size()) + { + RCLCPP_ERROR( + this->get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + command_interface_names_.size(), ordered_interfaces.size()); + return controller_interface::CallbackReturn::ERROR; + } + + // reset command buffer if a command came through callback when controller was inactive + rt_buffer_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + + RCLCPP_INFO(this->get_node()->get_logger(), "activate successful"); + + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PassthroughController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // reset command buffer + rt_buffer_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool PassthroughController::on_set_chained_mode(bool /*chained_mode*/) { return true; } + +controller_interface::return_type PassthroughController::update_and_write_commands( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isnan(reference_interfaces_[i])) + { + command_interfaces_[i].set_value(reference_interfaces_[i]); + } + } + + return controller_interface::return_type::OK; +} + +std::vector +PassthroughController::on_export_reference_interfaces() +{ + std::vector reference_interfaces; + + for (size_t i = 0; i < reference_interface_names_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + } + + return reference_interfaces; +} + +controller_interface::return_type PassthroughController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + auto joint_commands = rt_buffer_ptr_.readFromRT(); + // message is valid + if (!(!joint_commands || !(*joint_commands))) + { + if (reference_interfaces_.size() != (*joint_commands)->data.size()) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "command size (%zu) does not match number of reference interfaces (%zu)", + (*joint_commands)->data.size(), reference_interfaces_.size()); + return controller_interface::return_type::ERROR; + } + reference_interfaces_ = (*joint_commands)->data; + } + + return controller_interface::return_type::OK; +} + +} // namespace passthrough_controller + +PLUGINLIB_EXPORT_CLASS( + passthrough_controller::PassthroughController, controller_interface::ChainableControllerInterface) diff --git a/example_12/controllers/src/passthrough_controller_parameters.yaml b/example_12/controllers/src/passthrough_controller_parameters.yaml new file mode 100644 index 000000000..cb1dbeec0 --- /dev/null +++ b/example_12/controllers/src/passthrough_controller_parameters.yaml @@ -0,0 +1,10 @@ +passthrough_controller: + interfaces: { + type: string_array, + default_value: [], + description: "Names of the interfaces to be forwarded", + validation: { + not_empty<>: null, + unique<>: null, + } + } diff --git a/example_12/description/gazebo/rrbot.gazebo.xacro b/example_12/description/gazebo/rrbot.gazebo.xacro new file mode 100644 index 000000000..d37ca424b --- /dev/null +++ b/example_12/description/gazebo/rrbot.gazebo.xacro @@ -0,0 +1,38 @@ + + + + + + + + + + $(find ros2_control_demo_example_12)/config/rrbot_chained_ontrollers.yaml + + + + + + Gazebo/Orange + + + + + 0.2 + 0.2 + Gazebo/Yellow + + + + + 0.2 + 0.2 + Gazebo/Orange + + + + + diff --git a/example_12/description/launch/view_robot.launch.py b/example_12/description/launch/view_robot.launch.py new file mode 100644 index 000000000..9737f2bfd --- /dev/null +++ b/example_12/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_12", + 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_12/description/ros2_control/rrbot.ros2_control.xacro b/example_12/description/ros2_control/rrbot.ros2_control.xacro new file mode 100644 index 000000000..a568ec31c --- /dev/null +++ b/example_12/description/ros2_control/rrbot.ros2_control.xacro @@ -0,0 +1,37 @@ + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + ros2_control_demo_example_12/RRBotSystemPositionOnlyHardware + 0 + 3.0 + 100 + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + diff --git a/example_12/description/rviz/rrbot.rviz b/example_12/description/rviz/rrbot.rviz new file mode 100644 index 000000000..8e78cd635 --- /dev/null +++ b/example_12/description/rviz/rrbot.rviz @@ -0,0 +1,187 @@ +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 + 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_12/description/urdf/rrbot.materials.xacro b/example_12/description/urdf/rrbot.materials.xacro new file mode 100644 index 000000000..eb8e0212c --- /dev/null +++ b/example_12/description/urdf/rrbot.materials.xacro @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_12/description/urdf/rrbot.urdf.xacro b/example_12/description/urdf/rrbot.urdf.xacro new file mode 100644 index 000000000..f7d3ccab4 --- /dev/null +++ b/example_12/description/urdf/rrbot.urdf.xacro @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_12/description/urdf/rrbot_description.urdf.xacro b/example_12/description/urdf/rrbot_description.urdf.xacro new file mode 100644 index 000000000..cfcb0388b --- /dev/null +++ b/example_12/description/urdf/rrbot_description.urdf.xacro @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/example_12/doc/rrbot.png b/example_12/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`__ + + Controllers: `rrbot.launch.py `__ +- ROS2 Controller: `passthrough_controller.cpp `__ +- 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.cpp `__ + + +Controllers from this demo +-------------------------- +- ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ +- ``Forward Command Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp new file mode 100644 index 000000000..561368798 --- /dev/null +++ b/example_12/hardware/include/ros2_control_demo_example_12/rrbot.hpp @@ -0,0 +1,81 @@ +// Copyright 2023 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROS2_CONTROL_DEMO_EXAMPLE_12__RRBOT_HPP_ +#define ROS2_CONTROL_DEMO_EXAMPLE_12__RRBOT_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "ros2_control_demo_example_12/visibility_control.h" + +namespace ros2_control_demo_example_12 +{ +class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + // Parameters for the RRBot simulation + double hw_start_sec_; + double hw_stop_sec_; + double hw_slowdown_; + + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_states_; +}; + +} // namespace ros2_control_demo_example_12 + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_12__RRBOT_HPP_ diff --git a/example_12/hardware/include/ros2_control_demo_example_12/visibility_control.h b/example_12/hardware/include/ros2_control_demo_example_12/visibility_control.h new file mode 100644 index 000000000..e4ad914e0 --- /dev/null +++ b/example_12/hardware/include/ros2_control_demo_example_12/visibility_control.h @@ -0,0 +1,56 @@ +// 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. +// 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_12__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_DEMO_EXAMPLE_12__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_12_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_DEMO_EXAMPLE_12_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_12_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_DEMO_EXAMPLE_12_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_DEMO_EXAMPLE_12_BUILDING_DLL +#define ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_12_EXPORT +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC ROS2_CONTROL_DEMO_EXAMPLE_12_IMPORT +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC_TYPE ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_12_LOCAL +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_12_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_12_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_DEMO_EXAMPLE_12_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC +#define ROS2_CONTROL_DEMO_EXAMPLE_12_LOCAL +#endif +#define ROS2_CONTROL_DEMO_EXAMPLE_12_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_DEMO_EXAMPLE_12__VISIBILITY_CONTROL_H_ diff --git a/example_12/hardware/rrbot.cpp b/example_12/hardware/rrbot.cpp new file mode 100644 index 000000000..a7d3a66cc --- /dev/null +++ b/example_12/hardware/rrbot.cpp @@ -0,0 +1,237 @@ +// Copyright 2023 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_control_demo_example_12/rrbot.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_example_12 +{ +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code + hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) + { + // RRBotSystemPositionOnly has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return hardware_interface::CallbackReturn::ERROR; + } + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", + hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // reset values always when configuring hardware + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_states_[i] = 0; + hw_commands_[i] = 0; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +std::vector +RRBotSystemPositionOnlyHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); + } + + return state_interfaces; +} + +std::vector +RRBotSystemPositionOnlyHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Activating ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", + hw_start_sec_ - i); + } + // END: This part here is for exemplary purposes - Please do not copy to your production code + + // command and state should be equal when starting + for (uint i = 0; i < hw_states_.size(); i++) + { + hw_commands_[i] = hw_states_[i]; + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Deactivating ...please wait..."); + + for (int i = 0; i < hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "%.1f seconds left...", + hw_stop_sec_ - i); + } + + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); + + for (uint i = 0; i < hw_states_.size(); i++) + { + // Simulate RRBot's movement + hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_; + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!", + hw_states_[i], i); + } + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); + + for (uint i = 0; i < hw_commands_.size(); i++) + { + // Simulate sending commands to the hardware + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!", + hw_commands_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_example_12 + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_example_12::RRBotSystemPositionOnlyHardware, + hardware_interface::SystemInterface) diff --git a/example_12/package.xml b/example_12/package.xml new file mode 100644 index 000000000..4bd54219f --- /dev/null +++ b/example_12/package.xml @@ -0,0 +1,42 @@ + + + + ros2_control_demo_example_12 + 0.0.0 + Demo package of `ros2_control` simulation with RRbot. + + Dr.-Ing. Denis Štogl + Bence Magyar + Christoph Froehlich + + Dr.-Ing. Denis Štogl + Christoph Froehlich + + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + forward_command_controller + gazebo_ros + gazebo_ros2_control + joint_state_broadcaster + joint_state_publisher_gui + joint_trajectory_controller + robot_state_publisher + ros2_controllers_test_nodes + ros2controlcli + rviz2 + xacro + + ament_cmake_gtest + + + ament_cmake + + diff --git a/example_12/passthrough_controller.xml b/example_12/passthrough_controller.xml new file mode 100644 index 000000000..7a679a575 --- /dev/null +++ b/example_12/passthrough_controller.xml @@ -0,0 +1,9 @@ + + + + A simple demo of chainable controllers. It passes commands through without change. + + + diff --git a/example_12/ros2_control_demo_example_12.xml b/example_12/ros2_control_demo_example_12.xml new file mode 100644 index 000000000..f8980721f --- /dev/null +++ b/example_12/ros2_control_demo_example_12.xml @@ -0,0 +1,9 @@ + + + + The ros2_control RRbot example using a system hardware interface-type. + + +