diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 92550edcc..fd28caab7 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -22,7 +22,9 @@ add_library( SHARED src/diffbot_system.cpp src/rrbot_system_position_only.cpp + src/rrbot_system_with_sensor.cpp src/rrbot_system_multi_interface.cpp + src/rrbot_system_with_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_sensor.hpp new file mode 100644 index 000000000..6ede55dce --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_sensor.hpp @@ -0,0 +1,79 @@ +// 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. + + +#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ + +#include +#include +#include + + +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "rclcpp/macros.hpp" +#include "ros2_control_demo_hardware/visibility_control.h" + +using hardware_interface::return_type; + +namespace ros2_control_demo_hardware +{ +class RRBotSystemWithSensorHardware : public + hardware_interface::BaseInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemWithSensorHardware); + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type configure(const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector export_state_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + std::vector export_command_interfaces() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type start() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type stop() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type read() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type write() override; + +private: + // Parameters for the RRBot simulation + double hw_start_sec_; + double hw_stop_sec_; + double hw_slowdown_; + double hw_sensor_change_; + + // Store the command for the simulated robot + std::vector hw_joint_commands_; + std::vector hw_joint_states_; + std::vector hw_sensor_states_; +}; + +} // namespace ros2_control_demo_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index c2e351943..1e5b4ec87 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -3,7 +3,14 @@ type="ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware" base_class_type="hardware_interface::SystemInterface"> - The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. + The ros2_control RRBot example using a system hardware interface-type and only one command and state interface. This example is the starting point to implement a hardware interface for robots providing one communication path for all its joints. + + + + + The ros2_control RRBot example using a system hardware interface-type and only one command and state interface. This example is the starting point to implement a hardware interface for robots with sensors providing one communication path. +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_hardware +{ + +const char SENSOR_LOGGER[] = "RRBotSystemWithSensorHardware"; + +return_type RRBotSystemWithSensorHardware::configure( + const hardware_interface::HardwareInfo & info) +{ + if (configure_default(info) != return_type::OK) { + return return_type::ERROR; + } + + 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"]); + hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); + hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_sensor_states_.resize( + info_.sensors[0].state_interfaces.size(), + std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + // RRBotSystemWithSensor has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger(SENSOR_LOGGER), + "Joint '%s' has %d command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); + return return_type::ERROR; + } + + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger(SENSOR_LOGGER), + "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 return_type::ERROR; + } + + if (joint.state_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger(SENSOR_LOGGER), + "Joint '%s' has %d state interface. 1 expected.", + joint.name.c_str(), joint.state_interfaces.size()); + return return_type::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger(SENSOR_LOGGER), + "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 return_type::ERROR; + } + } + + // RRBotSystemWithSensor has six state interfaces for one sensor + if (info_.sensors[0].state_interfaces.size() != 2) { + RCLCPP_FATAL( + rclcpp::get_logger(SENSOR_LOGGER), + "Sensor '%s' has %d state interface. 2 expected.", + info_.sensors[0].name.c_str(), info_.sensors[0].state_interfaces.size()); + return return_type::ERROR; + } + + status_ = hardware_interface::status::CONFIGURED; + return return_type::OK; +} + +std::vector +RRBotSystemWithSensorHardware::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_joint_states_[i])); + } + + // export sensor state interface + for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); + } + + return state_interfaces; +} + +std::vector +RRBotSystemWithSensorHardware::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_joint_commands_[i])); + } + + return command_interfaces; +} + +return_type RRBotSystemWithSensorHardware::start() +{ + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Starting ...please wait..."); + + for (int i = 0; i <= hw_start_sec_; i++) { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "%.1f seconds left...", hw_start_sec_ - i); + } + + // set some default values for joints + for (uint i = 0; i < hw_joint_states_.size(); i++) { + if (std::isnan(hw_joint_states_[i])) { + hw_joint_states_[i] = 0; + hw_joint_commands_[i] = 0; + } + } + + // set default value for sensor + if (std::isnan(hw_sensor_states_[0])) { + hw_sensor_states_[0] = 0; + } + + status_ = hardware_interface::status::STARTED; + + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "System Sucessfully started!"); + + return return_type::OK; +} + +return_type RRBotSystemWithSensorHardware::stop() +{ + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Stopping ...please wait..."); + + for (int i = 0; i <= hw_stop_sec_; i++) { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "%.1f seconds left...", hw_stop_sec_ - i); + } + + status_ = hardware_interface::status::STOPPED; + + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "System sucessfully stopped!"); + + return return_type::OK; +} + +hardware_interface::return_type RRBotSystemWithSensorHardware::read() +{ + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Reading...please wait..."); + + for (uint i = 0; i < hw_joint_states_.size(); i++) { + // Simulate RRBot's movement + hw_joint_states_[i] = hw_joint_commands_[i] + + (hw_joint_states_[i] - hw_joint_commands_[i]) / hw_slowdown_; + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Got state %.5f for joint %d!", hw_joint_states_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Joints sucessfully read!"); + + for (uint i = 0; i < hw_sensor_states_.size(); i++) { + // Simulate RRBot's sensor data + unsigned int seed = time(NULL) + i; + hw_sensor_states_[i] = static_cast(rand_r(&seed)) / + (static_cast(RAND_MAX / hw_sensor_change_)); + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Got state %.5f for sensor %d!", hw_sensor_states_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Sensors sucessfully read!"); + + return return_type::OK; +} + +hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemWithSensorHardware::write() +{ + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Writing...please wait..."); + + for (uint i = 0; i < hw_joint_commands_.size(); i++) { + // Simulate sending commands to the hardware + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Got command %.5f for joint %d!", hw_joint_commands_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger(SENSOR_LOGGER), + "Joints sucessfully written!"); + + return return_type::OK; +} + +} // namespace ros2_control_demo_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::RRBotSystemWithSensorHardware, + hardware_interface::SystemInterface +) diff --git a/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml b/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml new file mode 100644 index 000000000..d680cf6dc --- /dev/null +++ b/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml @@ -0,0 +1,25 @@ +controller_manager: + ros__parameters: + update_rate: 2 # Hz + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + fts_controller: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +forward_position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position + +fts_controller: + ros__parameters: + interface_names.force.x: tcp_fts_sensor/mysensor_force.x + interface_names.torque.z: tcp_fts_sensor/mysensor_torque.z + frame_id: rrbot_tcp diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro new file mode 100644 index 000000000..633e44b80 --- /dev/null +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro @@ -0,0 +1,59 @@ + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/RRBotSystemWithSensorHardware + 2.0 + 3.0 + ${slowdown} + 5.0 + + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + rrbot_tcp + 100 + 15 + + + + + + diff --git a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro new file mode 100644 index 000000000..e5d7235b4 --- /dev/null +++ b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + diff --git a/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py new file mode 100755 index 000000000..05663eb4d --- /dev/null +++ b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py @@ -0,0 +1,84 @@ +# Copyright (c) 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. + +# +# Authors: Subhas Das, Denis Stogl +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for multi-robot setup. \ + If changed than also joint names in the controllers configuration have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="rrbot_with_sensor_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "slowdown", + default_value="3.0", + description="Slowdown factor of the RRbot.", + ) + ) + + # Initialize Arguments + prefix = LaunchConfiguration("prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + slowdown = LaunchConfiguration("slowdown") + + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot.launch.py"]), + launch_arguments={ + "controllers_file": "rrbot_with_sensor_controllers.yaml", + "description_file": "rrbot_system_with_sensor.urdf.xacro", + "prefix": prefix, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "slowdown": slowdown, + }.items(), + ) + + return LaunchDescription(declared_arguments + [base_launch])