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..eb2ba8605 --- /dev/null +++ b/example_12/controllers/include/passthrough_controller/passthrough_controller.hpp @@ -0,0 +1,88 @@ +// 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() 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..ecd76be29 --- /dev/null +++ b/example_12/controllers/src/passthrough_controller.cpp @@ -0,0 +1,183 @@ +// 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() +{ + 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 000000000..9f5980b2c Binary files /dev/null and b/example_12/doc/rrbot.png differ diff --git a/example_12/doc/userdoc.rst b/example_12/doc/userdoc.rst new file mode 100644 index 000000000..766c55ddb --- /dev/null +++ b/example_12/doc/userdoc.rst @@ -0,0 +1,182 @@ +:github_url: https://github.com/ros-controls/ros2_control_demos/blob/{REPOS_FILE_BRANCH}/example_12/doc/userdoc.rst + +.. _ros2_control_demos_example_12_userdoc: + +Example 12: Controller chaining with RRBot +=========================================== + +The example shows how to write a simple chainable controller, and how to integrate it properly to have a functional controller chaining. + +For *example_12*, we will use RRBot, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm to demonstrate the controller chaining functionality in ROS2 control. + +For *example_12*, a simple chainable ros2_controller has been implemented that takes a vector of interfaces as an input and simple forwards them without any changes. Such a controller is simple known as a ``passthrough_controller``. + +.. include:: ../../doc/run_from_docker.rst + +Tutorial steps +-------------------------- + +1. To check that *RRBot* descriptions are working properly use following launch commands + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_12 view_robot.launch.py + + .. image:: rrbot.png + :width: 400 + :alt: Revolute-Revolute Manipulator Robot + + The ``joint_state_publisher_gui`` provides a GUI to change the configuration for *RRbot*. It is immediately displayed in *RViz*. + + +2. To start *RRBot* with the hardware interface, open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_12 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. It uses an identical hardware interface as already discussed with *example_1*, see its docs on details on the hardware interface. + + 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 controllers are running by + + .. code-block:: shell + + ros2 control list_controllers + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + joint2_position_controller[passthrough_controller/PassthroughController] active + joint1_position_controller[passthrough_controller/PassthroughController] active + +4. Check if the hardware interface loaded properly, by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + The output should be something like this: + + .. code-block:: shell + + command interfaces + joint1/position [available] [claimed] + joint1_position_controller/joint1/position [unavailable] [unclaimed] + joint2/position [available] [claimed] + joint2_position_controller/joint2/position [unavailable] [unclaimed] + state interfaces + joint1/position + joint2/position + + At this stage the reference interfaces of controllers are listed under ``command_interfaces`` when ``ros2 control list_hardware_interfaces`` command is executed. + + * Marker ``[available]`` by command interfaces means that the hardware interfaces are available and are ready to command. + + * Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + + * Marker ``[unavailable]`` by command interfaces means that the hardware interfaces are unavailable and cannot be commanded. For instance, when there is an error in reading or writing an actuator module, it's interfaces are automatically become unavailable. + + * Marker ``[unclaimed]`` by command interfaces means that the reference interfaces of ``joint1_position_controller`` and ``joint2_position_controller`` are not yet in chained mode. However, their reference interfaces are available to be chained, as the controllers are active. + + .. note:: + + In case of chained controllers, the command interfaces appear to be ``unavailable`` and ``unclaimed``, even though the controllers whose exposed reference interfaces are active, because these command interfaces become ``available`` only in chained mode i.e., when an another controller makes use of these command interface. In non-chained mode, it is expected for the chained controller to use references from subscribers, hence they are marked as ``unavailable``. + +5. To start the complete controller chain, open a terminal, source your ROS2-workspace and execute its launch file with + + .. code-block:: shell + + ros2 launch ros2_control_demo_example_12 launch_chained_controllers.launch.py + + This launch file starts the ``position_controller`` that uses the reference interfaces of both ``joint1_position_controller`` and ``joint2_position_controller`` and streamlines into one, and then the ``forward_position_controller`` uses the reference interfaces of the ``position_controller`` to command the *RRBot* joints. + + .. note:: + + The second level ``position_controller`` is only added for demonstration purposes, however, a new chainable controller can be configured to directly command the reference interfaces of both ``joint1_position_controller`` and ``joint2_position_controller``. + +6. Check if the new controllers are running by + + .. code-block:: shell + + ros2 control list_controllers + + .. code-block:: shell + + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + joint2_position_controller[passthrough_controller/PassthroughController] active + joint1_position_controller[passthrough_controller/PassthroughController] active + position_controller [passthrough_controller/PassthroughController] active + forward_position_controller[forward_command_controller/ForwardCommandController] active + +7. Now check if the interfaces are loaded properly, by opening another terminal and executing + + .. code-block:: shell + + ros2 control list_hardware_interfaces + + The output should be something like this: + + .. code-block:: shell + + command interfaces + joint1/position [available] [claimed] + joint1_position_controller/joint1/position [available] [claimed] + joint2/position [available] [claimed] + joint2_position_controller/joint2/position [available] [claimed] + position_controller/joint1_position_controller/joint1/position [available] [claimed] + position_controller/joint2_position_controller/joint2/position [available] [claimed] + state interfaces + joint1/position + joint2/position + + At this stage the reference interfaces of all the controllers are listed under ``command_interfaces`` should be ``available`` and ``claimed`` when ``ros2 control list_hardware_interfaces`` command is executed. Marker ``[claimed]`` by command interfaces means that a controller has access to command *RRBot*. + +8. If you get output from above you can send commands to *Forward Command Controller*: + + .. code-block:: shell + + ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: + - 0.5 + - 0.5" + + 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! + + If you echo the ``/joint_states`` or ``/dynamic_joint_states`` topics you should now get similar values, namely the simulated states of the robot + + .. code-block:: shell + + ros2 topic echo /joint_states + ros2 topic echo /dynamic_joint_states + + This clearly shows that the controller chaining is functional, as the commands sent to the ``forward_position_controller`` are passed through properly and then it is reflected in the hardware interfaces of the *RRBot*. + + +Files used for this demos +------------------------- + +- Launch files: + + + Hardware: `rrbot.launch.py `__ + + 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. + + +