From 3b205e7b8c92f51b14beab33f8e2f07947298ba3 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 30 Jun 2021 22:55:49 +0530 Subject: [PATCH 01/43] Adding Example-4 skeleton. Commenting position_only compilation due to duplicate class. --- ...rbot_system_with_external_sensor.launch.py | 89 ++++ ...em_with_external_sensor.ros2_control.xacro | 82 ++++ ...bot_system_with_external_sensor.urdf.xacro | 51 +++ ros2_control_demo_hardware/CMakeLists.txt | 3 +- .../rrbot_system_with_external_sensor.hpp | 118 +++++ .../src/rrbot_system_with_external_sensor.cpp | 417 ++++++++++++++++++ 6 files changed, 759 insertions(+), 1 deletion(-) create mode 100755 ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py create mode 100644 ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro create mode 100644 ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro create mode 100644 ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp create mode 100644 ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp diff --git a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py new file mode 100755 index 00000000..122485ea --- /dev/null +++ b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -0,0 +1,89 @@ +# 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_ros.actions import Node +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( + "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") + + # add the spawner node for the fts_broadcaster + fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["fts_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # collect all the nodes here + nodes = [ + fts_broadcaster_spawner, + ] + + 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] + nodes) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro new file mode 100644 index 00000000..24da0602 --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro @@ -0,0 +1,82 @@ + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + ${slowdown} + + + + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/RRBotSystemWithSensorHardware + 2.0 + 3.0 + ${slowdown} + 5.0 + + + + + + + + + + + + kuka_tcp + 100 + 15 + + + + + diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro new file mode 100644 index 00000000..4354cc55 --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + + diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 92550edc..40d63680 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -21,8 +21,9 @@ add_library( ${PROJECT_NAME} SHARED src/diffbot_system.cpp - src/rrbot_system_position_only.cpp + #src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp + src/rrbot_system_with_external_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp new file mode 100644 index 00000000..adda8397 --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp @@ -0,0 +1,118 @@ +// 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 +// + +#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_EXTERNAL_SENSOR_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_EXTERNAL_SENSOR_HPP_ + +#include +#include +#include + +#include "hardware_interface/base_interface.hpp" +#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 "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_; +}; + +class RRBotSystemPositionOnlyHardware +: public hardware_interface::BaseInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::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 + hardware_interface::return_type start() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type stop() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type read() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + hardware_interface::return_type write() 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_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_EXTERNAL_SENSOR_HPP_ diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp new file mode 100644 index 00000000..f67ecdff --- /dev/null +++ b/ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp @@ -0,0 +1,417 @@ +// 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 +// + +#include "ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_hardware +{ +hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( + const hardware_interface::HardwareInfo & info) +{ + if (configure_default(info) != hardware_interface::return_type::OK) + { + return hardware_interface::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_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 %d command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return hardware_interface::return_type::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::return_type::ERROR; + } + + if (joint.state_interfaces.size() != 1) + { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), + "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return hardware_interface::return_type::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::return_type::ERROR; + } + } + + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; +} + +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::return_type RRBotSystemPositionOnlyHardware::start() +{ + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Starting ...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); + } + + // set some default values when starting the first time + for (uint i = 0; i < hw_states_.size(); i++) + { + if (std::isnan(hw_states_[i])) + { + hw_states_[i] = 0; + hw_commands_[i] = 0; + } + else + { + hw_commands_[i] = hw_states_[i]; + } + } + + status_ = hardware_interface::status::STARTED; + + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System Successfully started!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop() +{ + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Stopping ...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); + } + + status_ = hardware_interface::status::STOPPED; + + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System successfully stopped!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() +{ + 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!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type RRBotSystemPositionOnlyHardware::write() +{ + 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!"); + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_hardware + +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) + { + std::cout << "here 11 ---------" << std::endl; + 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()); + + std::cout << "here 22 -------- : " << info_.sensors[0].state_interfaces.size() << std::endl; + 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 Successfully 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 successfully 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 successfully 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 successfully 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 successfully written!"); + + return return_type::OK; +} + +} // namespace ros2_control_demo_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface) + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::RRBotSystemWithSensorHardware, hardware_interface::SystemInterface) From 2b2f310b515e1d9808267356725cddf698278931 Mon Sep 17 00:00:00 2001 From: bailaC Date: Thu, 8 Jul 2021 22:24:47 +0530 Subject: [PATCH 02/43] Addressing review comments. --- ...bot_force_torque_sensor.ros2_control.xacro | 43 ++ ...em_with_external_sensor.ros2_control.xacro | 82 ---- ...bot_system_with_external_sensor.urdf.xacro | 15 +- ros2_control_demo_hardware/CMakeLists.txt | 4 +- .../external_rrbot_force_torque_sensor.hpp | 72 +++ .../rrbot_system_with_external_sensor.hpp | 118 ----- .../external_rrbot_force_torque_sensor.cpp | 102 +++++ .../src/rrbot_system_with_external_sensor.cpp | 417 ------------------ 8 files changed, 226 insertions(+), 627 deletions(-) create mode 100644 ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro delete mode 100644 ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro create mode 100644 ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp delete mode 100644 ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp create mode 100644 ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp delete mode 100644 ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro new file mode 100644 index 00000000..af67e255 --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -0,0 +1,43 @@ + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + 2.0 + 3.0 + ${slowdown} + 5.0 + + + + + + + + + + + + tool_link + 100 + 15 + + + + + diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro deleted file mode 100644 index 24da0602..00000000 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_external_sensor.ros2_control.xacro +++ /dev/null @@ -1,82 +0,0 @@ - - - - - - - - - - gazebo_ros2_control/GazeboSystem - - - - - - fake_components/GenericSystem - ${fake_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware - 2.0 - 3.0 - ${slowdown} - - - - - - - -1 - 1 - - - - - - -1 - 1 - - - - - - - - - gazebo_ros2_control/GazeboSystem - - - - - - fake_components/GenericSystem - ${fake_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/RRBotSystemWithSensorHardware - 2.0 - 3.0 - ${slowdown} - 5.0 - - - - - - - - - - - - kuka_tcp - 100 - 15 - - - - - diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 4354cc55..8e4387dd 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -14,16 +14,16 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + - + - + - + @@ -43,9 +43,8 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc fake_sensor_commands="$(arg fake_sensor_commands)" slowdown="$(arg slowdown)" /> - + fake_sensor_commands="$(arg fake_sensor_commands)" /> diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 40d63680..049e04ad 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -21,9 +21,9 @@ add_library( ${PROJECT_NAME} SHARED src/diffbot_system.cpp - #src/rrbot_system_position_only.cpp + src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp - src/rrbot_system_with_external_sensor.cpp + src/external_rrbot_force_torque_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp new file mode 100644 index 00000000..a3401aeb --- /dev/null +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -0,0 +1,72 @@ +// 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 +// + +#ifndef ROS2_CONTROL_DEMO_HARDWARE__EXTERNAL_RRBOT_FORCE_TORQUE_SENSOR_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__EXTERNAL_RRBOT_FORCE_TORQUE_SENSOR_HPP_ + +#include +#include +#include + +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/sensor_interface.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 ExternalRRBotForceTorqueSensorHardware +: public hardware_interface::BaseInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); + + 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 + return_type start() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type stop() override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + return_type read() override; + +private: + // Parameters for the RRBot simulation + double hw_start_sec_; + double hw_stop_sec_; + double hw_sensor_change_; + + // Store the sensor states for the simulated robot + std::vector hw_sensor_states_; +}; + +} // namespace ros2_control_demo_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__EXTERNAL_RRBOT_FORCE_TORQUE_SENSOR_HPP_ diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp deleted file mode 100644 index adda8397..00000000 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp +++ /dev/null @@ -1,118 +0,0 @@ -// 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 -// - -#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_EXTERNAL_SENSOR_HPP_ -#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_EXTERNAL_SENSOR_HPP_ - -#include -#include -#include - -#include "hardware_interface/base_interface.hpp" -#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 "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_; -}; - -class RRBotSystemPositionOnlyHardware -: public hardware_interface::BaseInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); - - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::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 - hardware_interface::return_type start() override; - - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type stop() override; - - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type read() override; - - ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type write() 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_hardware - -#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_EXTERNAL_SENSOR_HPP_ diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp new file mode 100644 index 00000000..909a9ee2 --- /dev/null +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -0,0 +1,102 @@ +// 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 +// + +#include "ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp" + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_hardware +{ +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( + const hardware_interface::HardwareInfo & info) +{ + if (configure_default(info) != hardware_interface::return_type::OK) + { + return hardware_interface::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"]); + + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() +{ + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); + + for (int i = 0; i <= hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", + hw_start_sec_ - i); + } + + status_ = hardware_interface::status::STARTED; + + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System Successfully started!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() +{ + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); + + for (int i = 0; i <= hw_stop_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", + hw_stop_sec_ - i); + } + + status_ = hardware_interface::status::STOPPED; + + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System successfully stopped!"); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() +{ + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); + + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); + + return hardware_interface::return_type::OK; +} + +} // namespace ros2_control_demo_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, hardware_interface::SensorInterface) diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp deleted file mode 100644 index f67ecdff..00000000 --- a/ros2_control_demo_hardware/src/rrbot_system_with_external_sensor.cpp +++ /dev/null @@ -1,417 +0,0 @@ -// 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 -// - -#include "ros2_control_demo_hardware/rrbot_system_with_external_sensor.hpp" - -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace ros2_control_demo_hardware -{ -hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( - const hardware_interface::HardwareInfo & info) -{ - if (configure_default(info) != hardware_interface::return_type::OK) - { - return hardware_interface::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_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 %d command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); - return hardware_interface::return_type::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::return_type::ERROR; - } - - if (joint.state_interfaces.size() != 1) - { - RCLCPP_FATAL( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), - "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(), - joint.state_interfaces.size()); - return hardware_interface::return_type::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::return_type::ERROR; - } - } - - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; -} - -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::return_type RRBotSystemPositionOnlyHardware::start() -{ - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Starting ...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); - } - - // set some default values when starting the first time - for (uint i = 0; i < hw_states_.size(); i++) - { - if (std::isnan(hw_states_[i])) - { - hw_states_[i] = 0; - hw_commands_[i] = 0; - } - else - { - hw_commands_[i] = hw_states_[i]; - } - } - - status_ = hardware_interface::status::STARTED; - - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System Successfully started!"); - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop() -{ - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Stopping ...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); - } - - status_ = hardware_interface::status::STOPPED; - - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System successfully stopped!"); - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() -{ - 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!"); - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type RRBotSystemPositionOnlyHardware::write() -{ - 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!"); - - return hardware_interface::return_type::OK; -} - -} // namespace ros2_control_demo_hardware - -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) - { - std::cout << "here 11 ---------" << std::endl; - 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()); - - std::cout << "here 22 -------- : " << info_.sensors[0].state_interfaces.size() << std::endl; - 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 Successfully 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 successfully 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 successfully 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 successfully 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 successfully written!"); - - return return_type::OK; -} - -} // namespace ros2_control_demo_hardware - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware, hardware_interface::SystemInterface) - -PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotSystemWithSensorHardware, hardware_interface::SystemInterface) From 7c65e254967900cc3a8f77f4dec7687e866b3043 Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 9 Jul 2021 18:04:22 +0530 Subject: [PATCH 03/43] Update ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 8e4387dd..b9efc96a 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -1,5 +1,5 @@ - + + + + diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index a3401aeb..33d1167c 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -37,7 +37,7 @@ using hardware_interface::return_type; namespace ros2_control_demo_hardware { class ExternalRRBotForceTorqueSensorHardware -: public hardware_interface::BaseInterface + : public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index f1743942..fd34eb4a 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -32,13 +32,13 @@ namespace ros2_control_demo_hardware hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) - { + if (configure_default(info) != hardware_interface::return_type::OK) { return hardware_interface::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_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); status_ = hardware_interface::status::CONFIGURED; return hardware_interface::return_type::OK; @@ -46,10 +46,11 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configur hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), + "Starting ...please wait..."); - for (int i = 0; i <= hw_start_sec_; i++) - { + for (int i = 0; i <= hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -66,10 +67,11 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() { - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), + "Stopping ...please wait..."); - for (int i = 0; i <= hw_stop_sec_; i++) - { + for (int i = 0; i <= hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -88,7 +90,18 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() { RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully 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("ExternalRRBotForceTorqueSensorHardware"), "Got state %e for sensor %zu!", + hw_sensor_states_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), + "Joints successfully read!"); return hardware_interface::return_type::OK; } @@ -98,4 +111,5 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, hardware_interface::SensorInterface) + ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, + hardware_interface::SensorInterface) From 84f42b635589d7aaeec89b264e1eb8ff828c435e Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 18:36:29 +0530 Subject: [PATCH 06/43] Update ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../src/external_rrbot_force_torque_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index fd34eb4a..0831a5a6 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -50,7 +50,7 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); - for (int i = 0; i <= hw_start_sec_; i++) { + for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", From e5729a334cf27ff428fc3aeb0947aeb1d3060c8f Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 18:36:47 +0530 Subject: [PATCH 07/43] Update ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../src/external_rrbot_force_torque_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index 0831a5a6..b61e3739 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -71,7 +71,7 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); - for (int i = 0; i <= hw_stop_sec_; i++) { + for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", From c0c9b2d9660038899d5d42df4046815b2db26e96 Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 18:36:59 +0530 Subject: [PATCH 08/43] Update ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 70169b7e..a7b126c5 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -18,7 +18,7 @@ ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 2.0 + 0.0 3.0 5.0 From edaeebb2c1600fa5905f7fb30ff3fccbdce56dfe Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 19:15:29 +0530 Subject: [PATCH 09/43] Addressing review comments. --- ...bot_force_torque_sensor.ros2_control.xacro | 37 +++++++++---------- ...bot_system_with_external_sensor.urdf.xacro | 5 --- 2 files changed, 17 insertions(+), 25 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index a7b126c5..725a3617 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -4,26 +4,19 @@ - - - gazebo_ros2_control/GazeboSystem - - - - - - fake_components/GenericSystem - ${fake_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 0.0 - 3.0 - 5.0 - - - + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + 2.0 + 3.0 + 5.0 + + @@ -34,6 +27,10 @@ tool_link 100 + 100 + 100 + 15 + 15 15 diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index afdf027d..e760bbe7 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -16,9 +16,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - - - @@ -38,8 +35,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - - Date: Thu, 15 Jul 2021 20:09:31 +0530 Subject: [PATCH 10/43] Corrections. --- ...rbot_with_external_sensor_controllers.yaml | 25 +++++++++++++++++++ ...rbot_system_with_external_sensor.launch.py | 7 +++--- ...bot_system_with_external_sensor.urdf.xacro | 4 +-- .../ros2_control_demo_hardware.xml | 7 ++++++ .../external_rrbot_force_torque_sensor.cpp | 15 +++++++++++ 5 files changed, 53 insertions(+), 5 deletions(-) create mode 100644 ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml diff --git a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml new file mode 100644 index 00000000..95478f9e --- /dev/null +++ b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -0,0 +1,25 @@ +controller_manager: + ros__parameters: + update_rate: 2 # Hz + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + fts_broadcaster: + 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_broadcaster: + ros__parameters: + interface_names.force.x: tcp_fts_sensor/force.x + interface_names.torque.z: tcp_fts_sensor/torque.z + frame_id: tool_link diff --git a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py index 122485ea..f8130f4e 100755 --- a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "use_fake_hardware", - default_value="true", + default_value="false", description="Start robot with fake hardware mirroring command to its states.", ) ) @@ -48,6 +48,7 @@ def generate_launch_description(): Used only if 'use_fake_hardware' parameter is true.", ) ) + declared_arguments.append( DeclareLaunchArgument( "slowdown", @@ -77,8 +78,8 @@ def generate_launch_description(): 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", + "controllers_file": "rrbot_with_external_sensor_controllers.yaml", + "description_file": "rrbot_system_with_external_sensor.urdf.xacro", "prefix": prefix, "use_fake_hardware": use_fake_hardware, "fake_sensor_commands": fake_sensor_commands, diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index e760bbe7..f7aedf1e 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -20,7 +20,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + @@ -35,7 +35,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + + + The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot. + + diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index b61e3739..a87314db 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -44,6 +44,21 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configur return hardware_interface::return_type::OK; } +std::vector +ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() +{ + std::vector state_interfaces; + + // 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; +} + hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { RCLCPP_INFO( From 8dcb652b80046ca98fe819edb971a721862e3963 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 28 Jul 2021 21:15:49 +0530 Subject: [PATCH 11/43] Update ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 725a3617..2a0e5973 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -12,7 +12,7 @@ ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 2.0 + 0.0 3.0 5.0 From 7c3312a2f1f1cebe68f90e6e6680af67b01444bc Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 28 Jul 2021 21:16:19 +0530 Subject: [PATCH 12/43] Update ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index f7aedf1e..ba02932b 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -11,7 +11,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + From 20fddec406d6f61864053f0aabbb1328682f1380 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 28 Jul 2021 21:16:33 +0530 Subject: [PATCH 13/43] Update ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../config/rrbot_with_external_sensor_controllers.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml index 95478f9e..88a17dfd 100644 --- a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml +++ b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -20,6 +20,5 @@ forward_position_controller: fts_broadcaster: ros__parameters: - interface_names.force.x: tcp_fts_sensor/force.x - interface_names.torque.z: tcp_fts_sensor/torque.z + sensor_name: tcp_fts_sensor frame_id: tool_link From cc76781be4a46785c4bcd5f2348e08530dfa3725 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 9 Aug 2021 14:49:10 +0200 Subject: [PATCH 14/43] Update ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml --- .../config/rrbot_with_external_sensor_controllers.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml index 88a17dfd..c7759ea6 100644 --- a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml +++ b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 2 # Hz + update_rate: 100 # Hz forward_position_controller: type: forward_command_controller/ForwardCommandController From c71bb59d1ef01a27618abdf0fa4bc77de94d214e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 26 Aug 2021 14:25:18 +0200 Subject: [PATCH 15/43] Correct format. --- .../external_rrbot_force_torque_sensor.hpp | 2 +- .../external_rrbot_force_torque_sensor.cpp | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index 33d1167c..a3401aeb 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -37,7 +37,7 @@ using hardware_interface::return_type; namespace ros2_control_demo_hardware { class ExternalRRBotForceTorqueSensorHardware - : public hardware_interface::BaseInterface +: public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index a87314db..c72c4a65 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -32,7 +32,8 @@ namespace ros2_control_demo_hardware hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) { + if (configure_default(info) != hardware_interface::return_type::OK) + { return hardware_interface::return_type::ERROR; } @@ -62,10 +63,10 @@ ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), - "Starting ...please wait..."); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); - for (int i = 0; i < hw_start_sec_; i++) { + for (int i = 0; i < hw_start_sec_; i++) + { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -83,10 +84,10 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() { RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), - "Stopping ...please wait..."); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); - for (int i = 0; i < hw_stop_sec_; i++) { + for (int i = 0; i < hw_stop_sec_; i++) + { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -105,7 +106,8 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() { RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); - for (uint i = 0; i < hw_sensor_states_.size(); i++) { + 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] = @@ -115,8 +117,7 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() hw_sensor_states_[i], i); } RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), - "Joints successfully read!"); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); return hardware_interface::return_type::OK; } From 31e0f40f886a24e4d3260bf8e64a2af7c2210697 Mon Sep 17 00:00:00 2001 From: bailaC Date: Sat, 31 Jul 2021 21:27:48 +0530 Subject: [PATCH 16/43] Addressing review comments --- ...rbot_system_with_external_sensor.launch.py | 26 +++++++++---------- ...bot_system_with_external_sensor.urdf.xacro | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py index f8130f4e..3a669a0a 100755 --- a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -52,7 +52,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "slowdown", - default_value="3.0", + default_value="50.0", description="Slowdown factor of the RRbot.", ) ) @@ -63,6 +63,18 @@ def generate_launch_description(): fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") slowdown = LaunchConfiguration("slowdown") + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "controllers_file": "rrbot_with_external_sensor_controllers.yaml", + "description_file": "rrbot_system_with_external_sensor.urdf.xacro", + "prefix": prefix, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "slowdown": slowdown, + }.items(), + ) + # add the spawner node for the fts_broadcaster fts_broadcaster_spawner = Node( package="controller_manager", @@ -75,16 +87,4 @@ def generate_launch_description(): fts_broadcaster_spawner, ] - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot.launch.py"]), - launch_arguments={ - "controllers_file": "rrbot_with_external_sensor_controllers.yaml", - "description_file": "rrbot_system_with_external_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] + nodes) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index ba02932b..642a0f62 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -14,7 +14,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + From 33cf31980b13761a8a824d7a3232be60cab4ca6e Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 27 Aug 2021 19:52:53 +0530 Subject: [PATCH 17/43] Adding example text --- README.md | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/README.md b/README.md index 9f0b7535..e6d2cf04 100644 --- a/README.md +++ b/README.md @@ -329,6 +329,33 @@ angular: You should now see an orange box circling in `RViz`. +### Example 6: "Industrial Robots with externally connected sensor" + +- Launch file: [rrbot_system_with_external_sensor.launch.py](ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py) +- URDF: [rrbot_with_external_sensor_controllers.urdf.xacro](ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml) +- ros2_control URDF: [external_rrbot_force_torque_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro) + +- Command interfaces: + - joint1/position + - joint2/position +- State interfaces: + - joint1/position + - joint2/position + - tcp_fts_sensor/force.x + - tcp_fts_sensor/torque.z + +Available controllers: +- `forward_position_controller[forward_command_controller/ForwardCommandController]` +- `fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` +- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + +Commanding the robot: see the commands below. + +Accessing Wrench data from 2D FTS: +``` +ros2 topic echo /fts_broadcaster/wrench +``` + ## Controllers and moving hardware From f1f4736c484b235cf96d910e94ca3e81b899a3f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 1 Sep 2021 23:35:25 +0200 Subject: [PATCH 18/43] Bugfix: Do not ignore prefix argument for joint names. --- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- .../rrbot_system_position_only.ros2_control.xacro | 4 ++-- .../rrbot_system_with_sensor.ros2_control.xacro | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 2a0e5973..53d4059c 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -18,7 +18,7 @@ - + diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro index 9a75fcf3..077ff7c3 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro @@ -26,14 +26,14 @@ - + -1 1 - + -1 1 diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro index 28a2ddbf..e8f713ed 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro @@ -20,21 +20,21 @@ - + -1 1 - + -1 1 - + tool_link From e2278e735837b4a801dd60cf647beeaedebdb2e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 1 Sep 2021 23:39:49 +0200 Subject: [PATCH 19/43] Update example hardware to support new lifecycle of hardware. --- .../diffbot_system.hpp | 6 +- .../external_rrbot_force_torque_sensor.hpp | 10 ++- .../rrbot_system_multi_interface.hpp | 14 ++-- .../rrbot_system_position_only.hpp | 9 ++- .../rrbot_system_with_sensor.hpp | 9 ++- .../src/diffbot_system.cpp | 38 +++++------ .../external_rrbot_force_torque_sensor.cpp | 24 +++---- .../src/rrbot_system_multi_interface.cpp | 48 ++++++------- .../src/rrbot_system_position_only.cpp | 67 +++++++++++-------- .../src/rrbot_system_with_sensor.cpp | 55 ++++++++++----- 10 files changed, 151 insertions(+), 129 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp index e9c06127..b60cfca3 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp @@ -37,7 +37,7 @@ class DiffBotSystemHardware RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -46,10 +46,10 @@ class DiffBotSystemHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type start() override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type stop() override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index a3401aeb..a02d7a4a 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -32,8 +32,6 @@ #include "rclcpp/macros.hpp" #include "ros2_control_demo_hardware/visibility_control.h" -using hardware_interface::return_type; - namespace ros2_control_demo_hardware { class ExternalRRBotForceTorqueSensorHardware @@ -43,19 +41,19 @@ class ExternalRRBotForceTorqueSensorHardware RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type start() override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type stop() override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type read() override; + hardware_interface::return_type read() override; private: // Parameters for the RRBot simulation diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp index b9579edc..e4193dbd 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp @@ -30,8 +30,6 @@ #include "hardware_interface/types/hardware_interface_status_values.hpp" #include "ros2_control_demo_hardware/visibility_control.h" -using hardware_interface::return_type; - namespace ros2_control_demo_hardware { class RRBotSystemMultiInterfaceHardware @@ -41,7 +39,7 @@ class RRBotSystemMultiInterfaceHardware RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemMultiInterfaceHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -50,21 +48,21 @@ class RRBotSystemMultiInterfaceHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type prepare_command_mode_switch( + hardware_interface::return_type prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type start() override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type stop() override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type read() override; + hardware_interface::return_type read() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type write() override; + hardware_interface::return_type write() override; private: // Parameters for the RRBot simulation diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp index 7c4d53a5..b3f9ae5a 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp @@ -37,7 +37,10 @@ class RRBotSystemPositionOnlyHardware RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + CallbackReturn on_configure() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -46,10 +49,10 @@ class RRBotSystemPositionOnlyHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type start() override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type stop() override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; 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 index 8871428b..9e7b4914 100644 --- 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 @@ -41,7 +41,10 @@ class RRBotSystemWithSensorHardware RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemWithSensorHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + ROS2_CONTROL_DEMO_HARDWARE_PUBLIC + CallbackReturn on_configure() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -50,10 +53,10 @@ class RRBotSystemWithSensorHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type start() override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type stop() override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; diff --git a/ros2_control_demo_hardware/src/diffbot_system.cpp b/ros2_control_demo_hardware/src/diffbot_system.cpp index c7888128..6557aeb7 100644 --- a/ros2_control_demo_hardware/src/diffbot_system.cpp +++ b/ros2_control_demo_hardware/src/diffbot_system.cpp @@ -25,18 +25,17 @@ namespace ros2_control_demo_hardware { -hardware_interface::return_type DiffBotSystemHardware::configure( - const hardware_interface::HardwareInfo & info) +CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::HardwareInfo & info) { + if (on_init_default(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + base_x_ = 0.0; base_y_ = 0.0; base_theta_ = 0.0; - if (configure_default(info) != hardware_interface::return_type::OK) - { - return hardware_interface::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_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -52,7 +51,7 @@ hardware_interface::return_type DiffBotSystemHardware::configure( rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) @@ -61,7 +60,7 @@ hardware_interface::return_type DiffBotSystemHardware::configure( rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 2) @@ -70,7 +69,7 @@ hardware_interface::return_type DiffBotSystemHardware::configure( rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' has %d state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -80,7 +79,7 @@ hardware_interface::return_type DiffBotSystemHardware::configure( "Joint '%s' have '%s' as first state interface. '%s' and '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) @@ -89,12 +88,11 @@ hardware_interface::return_type DiffBotSystemHardware::configure( rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } } - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } std::vector DiffBotSystemHardware::export_state_interfaces() @@ -123,7 +121,7 @@ std::vector DiffBotSystemHardware::export_ return command_interfaces; } -hardware_interface::return_type DiffBotSystemHardware::start() +CallbackReturn DiffBotSystemHardware::on_activate() { RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Starting ...please wait..."); @@ -145,14 +143,12 @@ hardware_interface::return_type DiffBotSystemHardware::start() } } - status_ = hardware_interface::status::STARTED; - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "System Successfully started!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } -hardware_interface::return_type DiffBotSystemHardware::stop() +CallbackReturn DiffBotSystemHardware::on_deactivate() { RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Stopping ...please wait..."); @@ -163,11 +159,9 @@ hardware_interface::return_type DiffBotSystemHardware::stop() rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); } - status_ = hardware_interface::status::STOPPED; - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "System successfully stopped!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } hardware_interface::return_type DiffBotSystemHardware::read() diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index c72c4a65..7b6166bf 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -29,20 +29,22 @@ namespace ros2_control_demo_hardware { -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) + if (on_init_default(info) != CallbackReturn::SUCCESS) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::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_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + hw_sensor_states_.resize( + info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + return CallbackReturn::SUCCESS; } std::vector @@ -60,7 +62,7 @@ ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() return state_interfaces; } -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate() { RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); @@ -73,15 +75,13 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() hw_start_sec_ - i); } - status_ = hardware_interface::status::STARTED; - RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System Successfully started!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate() { RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); @@ -94,12 +94,10 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() hw_stop_sec_ - i); } - status_ = hardware_interface::status::STOPPED; - RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System successfully stopped!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() diff --git a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp index c62236a4..d0120635 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp @@ -26,12 +26,12 @@ namespace ros2_control_demo_hardware { -return_type RRBotSystemMultiInterfaceHardware::configure( +CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != return_type::OK) + if (on_init_default(info) != CallbackReturn::SUCCESS) { - return return_type::ERROR; + return CallbackReturn::ERROR; } hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -54,7 +54,7 @@ return_type RRBotSystemMultiInterfaceHardware::configure( RCLCPP_FATAL( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Joint '%s' has %d command interfaces. 3 expected.", joint.name.c_str()); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -66,7 +66,7 @@ return_type RRBotSystemMultiInterfaceHardware::configure( "Joint '%s' has %s command interface. Expected %s, %s, or %s.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 3) @@ -74,7 +74,7 @@ return_type RRBotSystemMultiInterfaceHardware::configure( RCLCPP_FATAL( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Joint '%s'has %d state interfaces. 3 expected.", joint.name.c_str()); - return return_type::ERROR; + return CallbackReturn::ERROR; } if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -86,12 +86,11 @@ return_type RRBotSystemMultiInterfaceHardware::configure( "Joint '%s' has %s state interface. Expected %s, %s, or %s.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); - return return_type::ERROR; + return CallbackReturn::ERROR; } } - status_ = hardware_interface::status::CONFIGURED; - return return_type::OK; + return CallbackReturn::SUCCESS; } std::vector @@ -129,7 +128,7 @@ RRBotSystemMultiInterfaceHardware::export_command_interfaces() return command_interfaces; } -return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( +hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces) { @@ -156,14 +155,14 @@ return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( // Example criteria: All joints must be given new command mode at the same time if (new_modes.size() != info_.joints.size()) { - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } // Example criteria: All joints must have the same command mode if (!std::all_of(new_modes.begin() + 1, new_modes.end(), [&](integration_level_t mode) { return mode == new_modes[0]; })) { - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } // Stop motion on all relevant joints that are stopping @@ -185,14 +184,14 @@ return_type RRBotSystemMultiInterfaceHardware::prepare_command_mode_switch( if (control_level_[i] != integration_level_t::UNDEFINED) { // Something else is using the joint! Abort! - return return_type::ERROR; + return hardware_interface::return_type::ERROR; } control_level_[i] = new_modes[i]; } - return return_type::OK; + return hardware_interface::return_type::OK; } -return_type RRBotSystemMultiInterfaceHardware::start() +CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate() { RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Starting... please wait..."); @@ -234,15 +233,14 @@ return_type RRBotSystemMultiInterfaceHardware::start() } control_level_[i] = integration_level_t::UNDEFINED; } - status_ = hardware_interface::status::STARTED; RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully started! %u", control_level_[0]); - return return_type::OK; + return CallbackReturn::SUCCESS; } -return_type RRBotSystemMultiInterfaceHardware::stop() +CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate() { RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Stopping... please wait..."); @@ -255,15 +253,13 @@ return_type RRBotSystemMultiInterfaceHardware::stop() hw_stop_sec_ - i); } - status_ = hardware_interface::status::STOPPED; - RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully stopped!"); - return return_type::OK; + return CallbackReturn::SUCCESS; } -return_type RRBotSystemMultiInterfaceHardware::read() +hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read() { for (std::size_t i = 0; i < hw_positions_.size(); i++) { @@ -273,7 +269,7 @@ return_type RRBotSystemMultiInterfaceHardware::read() RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Nothing is using the hardware interface!"); - return return_type::OK; + return hardware_interface::return_type::OK; break; case integration_level_t::POSITION: hw_accelerations_[i] = 0; @@ -296,10 +292,10 @@ return_type RRBotSystemMultiInterfaceHardware::read() "Got pos: %.5f, vel: %.5f, acc: %.5f for joint %d!", hw_positions_[i], hw_velocities_[i], hw_accelerations_[i], i); } - return return_type::OK; + return hardware_interface::return_type::OK; } -return_type RRBotSystemMultiInterfaceHardware::write() +hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write() { /*RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), @@ -313,7 +309,7 @@ return_type RRBotSystemMultiInterfaceHardware::write() hw_commands_positions_[i], hw_commands_velocities_[i], hw_commands_accelerations_[i], i, control_level_[i]); } - return return_type::OK; + return hardware_interface::return_type::OK; } } // namespace ros2_control_demo_hardware diff --git a/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp index b07e5ff4..dc1babcc 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp @@ -25,12 +25,12 @@ namespace ros2_control_demo_hardware { -hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( +CallbackReturn RRBotSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) + if (on_init_default(info) != CallbackReturn::SUCCESS) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -48,7 +48,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -57,7 +57,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( 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::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) @@ -66,7 +66,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joint '%s' has %d state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -75,12 +75,37 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure( 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::return_type::ERROR; + return CallbackReturn::ERROR; } } - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; +} + +CallbackReturn RRBotSystemPositionOnlyHardware::on_configure() +{ + 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); + } + + // 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"), "System Successfully configured!"); + + return CallbackReturn::SUCCESS; } std::vector @@ -109,7 +134,7 @@ RRBotSystemPositionOnlyHardware::export_command_interfaces() return command_interfaces; } -hardware_interface::return_type RRBotSystemPositionOnlyHardware::start() +CallbackReturn RRBotSystemPositionOnlyHardware::on_activate() { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Starting ...please wait..."); @@ -121,29 +146,19 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::start() hw_start_sec_ - i); } - // set some default values when starting the first time + // command and state should be equal when starting for (uint i = 0; i < hw_states_.size(); i++) { - if (std::isnan(hw_states_[i])) - { - hw_states_[i] = 0; - hw_commands_[i] = 0; - } - else - { - hw_commands_[i] = hw_states_[i]; - } + hw_commands_[i] = hw_states_[i]; } - status_ = hardware_interface::status::STARTED; - RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System Successfully started!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } -hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop() +CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate() { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Stopping ...please wait..."); @@ -155,12 +170,10 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop() hw_stop_sec_ - i); } - status_ = hardware_interface::status::STOPPED; - RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "System successfully stopped!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp index bed33310..f7de2f43 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -29,12 +29,11 @@ namespace ros2_control_demo_hardware { -hardware_interface::return_type RRBotSystemWithSensorHardware::configure( - const hardware_interface::HardwareInfo & info) +CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) + if (on_init_default(info) != CallbackReturn::SUCCESS) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -56,7 +55,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::configure( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -65,7 +64,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::configure( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "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::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) @@ -74,7 +73,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::configure( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return hardware_interface::return_type::ERROR; + return CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -83,12 +82,36 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::configure( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "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::return_type::ERROR; + return CallbackReturn::ERROR; } } - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; +} + +CallbackReturn RRBotSystemWithSensorHardware::on_configure() +{ + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Configuring ...please wait..."); + + for (int i = 0; i < hw_start_sec_; i++) + { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "%.1f seconds left...", + hw_start_sec_ - i); + } + + // reset values always when configuring hardware + for (uint i = 0; i < hw_joint_states_.size(); i++) + { + hw_joint_states_[i] = 0; + hw_joint_commands_[i] = 0; + } + + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "System Successfully configured!"); + + return CallbackReturn::SUCCESS; } std::vector @@ -124,7 +147,7 @@ RRBotSystemWithSensorHardware::export_command_interfaces() return command_interfaces; } -hardware_interface::return_type RRBotSystemWithSensorHardware::start() +CallbackReturn RRBotSystemWithSensorHardware::on_activate() { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Starting ...please wait..."); @@ -152,14 +175,12 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::start() hw_sensor_states_[0] = 0; } - status_ = hardware_interface::status::STARTED; - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "System Successfully started!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } -hardware_interface::return_type RRBotSystemWithSensorHardware::stop() +CallbackReturn RRBotSystemWithSensorHardware::on_deactivate() { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Stopping ...please wait..."); @@ -171,11 +192,9 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::stop() hw_stop_sec_ - i); } - status_ = hardware_interface::status::STOPPED; - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "System successfully stopped!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } hardware_interface::return_type RRBotSystemWithSensorHardware::read() From 97abb688ac7c62b7fc381bb6609b4ac32c99c791 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 1 Sep 2021 23:53:02 +0200 Subject: [PATCH 20/43] Add 'ThreeDofBot' and 'Three Robots' example for component lifecycle. --- README.md | 262 ++++++++++++++ .../config/three_robots_controllers.yaml | 118 ++++++ ...ee_robots_position_command_publishers.yaml | 37 ++ .../launch/three_robots.launch.py | 225 ++++++++++++ .../config/three_robots.rviz | 340 ++++++++++++++++++ .../threedofbot.ros2_control.xacro | 42 +++ .../urdf/three_robots.urdf.xacro | 67 ++++ .../urdf/threedofbot_description.urdf.xacro | 240 +++++++++++++ 8 files changed, 1331 insertions(+) create mode 100644 ros2_control_demo_bringup/config/three_robots_controllers.yaml create mode 100644 ros2_control_demo_bringup/config/three_robots_position_command_publishers.yaml create mode 100644 ros2_control_demo_bringup/launch/three_robots.launch.py create mode 100644 ros2_control_demo_description/rrbot_description/config/three_robots.rviz create mode 100644 ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro create mode 100644 ros2_control_demo_description/rrbot_description/urdf/three_robots.urdf.xacro create mode 100644 ros2_control_demo_description/rrbot_description/urdf/threedofbot_description.urdf.xacro diff --git a/README.md b/README.md index e2b97794..3d183f2c 100644 --- a/README.md +++ b/README.md @@ -387,6 +387,268 @@ Accessing Wrench data from 2D FTS: ros2 topic echo /fts_broadcaster/wrench ``` +### Example XX: "Multi-robot system with tools" + +- Launch file: [three_robots.launch.py](ros2_control_demo_bringup/launch/three_robots.launch.py) +- Controllers: [three_robots_controllers.yaml](ros2_control_demo_bringup/config/three_robots_controllers.yaml) +- URDF: [three_robots.urdf.xacro](ros2_control_demo_bringup/config/three_robots.urdf.xacro) +- ros2_control URDF: [three_robots.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/three_robots.ros2_control.xacro) + + +**Hardware and interfaces**: +- RRBotSystemPositionOnly (auto-start) + - Command interfaces: + - rrbot_joint1/position + - rrbot_joint2/position + - State interfaces: + - rrbot_joint1/position + - rrbot_joint2/position + +- ExternalRRBotFTSensor (auto-start) + - State interfaces: + - rrbot_tcp_fts_sensor/force.x + - rrbot_tcp_fts_sensor/force.y + - rrbot_tcp_fts_sensor/force.z + - rrbot_tcp_fts_sensor/torque.x + - rrbot_tcp_fts_sensor/torque.y + - rrbot_tcp_fts_sensor/torque.z + +- RRBotSystemWithSensor (auto-configure) + - Command interfaces: + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + - State interfaces: + - rrbot_with_sensor_joint1/position + - rrbot_with_sensor_joint2/position + - rrbot_with_sensor_tcp_fts_sensor/force.x + - rrbot_with_sensor_tcp_fts_sensor/torque.z + +- ThreeDofBot + - Command interfaces + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + - State interfaces: + - threedofbot_joint1/position + - threedofbot_joint1/pid_gain + - threedofbot_joint2/position + - threedofbot_joint2/pid_gain + - threedofbot_joint3/position + - threedofbot_joint3/pid_gain + +**Available controllers**: +- `joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + +- RRBotSystemPositionOnly + - `rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `rrbot_position_controller[forward_command_controller/ForwardCommandController]` + +- ExternalRRBotFTSensor + - `external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` + +- RRBotSystemPositionOnly + - `rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController]` + - `rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster]` + +- FakeThreeDofBot + - `threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster]` + - `threedofbot_position_controller[forward_command_controller/ForwardCommandController]` + - `threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController]` + + +Caveats on hardware lifecycling: +- There is currently no synchronization between available interface and controllers using them. + This means that you should stop controller before making interfaces they are using unavailable. + If you don't do this and deactivate/cleanup your interface first your computer will catch fire! +- Global Joint State Broadcaster will not broadcast interfaces that become available after it is started. + To solve this restart it manually, for now. During restart TF-transforms are not available. +- There is a possibility that hardware lifecycling (state changes) interfere with the `update`-loop if you are trying to start/stop a controller at the same time. + + +Notes: + 1. After starting the example there should be the following scene: + - right robot is moving (RRBotSystemPositionOnly - using auto-start) + - All interfaces are available and position controller is started and receives commands + - all controllers running + - left robot is standing upright (RRBotWithSensor - using auto-configure) + - only state interfaces are available therefore it can visualized, but not moved + - only position command controller is not running + - middle robot is "broken" (FakeThreeDofBot - it is only initialized) + - no interfaces are available + - all controllers inactive + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + ``` + + 1. Activate `RRBotWithSensor` and its position controller. Call + ``` + ros2 service call /controller_manager/manage_hardware_activity controller_manager_msgs/srv/ManageHardwareActivity "{activate: [RRBotSystemWithSensor], deactivate: []}" + ros2 control switch_controllers --start rrbot_with_sensor_position_controller + ``` + + Scenario state: + - right robot is moving + - left robot is moving + - middle robot is "broken" + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] inactive + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + ``` + + 1. Configure `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call + ``` + ros2 service call /controller_manager/configure_hardware_component controller_manager_msgs/srv/ConfigureHardwareComponent "{name: FakeThreeDofBot}" + ros2 control switch_controllers --start threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller + ``` + + Scenario state: + - right robot is moving + - left robot is moving + - middle robot is still "broken" + + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] inactive + ``` + + 1. Restart global joint state broadcaster to broadcast all available states from the framework. + First check output to have comparison: + ``` + ros2 topic echo /joint_states + ``` + Restart: + ``` + ros2 control switch_controllers --stop joint_state_broadcaster + ros2 control switch_controllers --start joint_state_broadcaster + ``` + Check output to for comparison + ``` + ros2 topic echo /joint_states + ``` + + Scenario state (everything is broken during `joint_state_broadcaster` restart): + - right robot is moving + - left robot is moving + - middle robot is still "standing" + + 1. Activate `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call + ``` + ros2 service call /controller_manager/manage_hardware_activity controller_manager_msgs/srv/ManageHardwareActivity "{activate: [FakeThreeDofBot], deactivate: []}" + ros2 control switch_controllers --start threedofbot_position_controller + ``` + + Scenario state: + - right robot is moving + - left robot is moving + - middle robot is moving + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] active + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + ``` + + 1. Deactivate `RRBotSystemPositionOnly` and its position controller (first). Call + ``` + ros2 control switch_controllers --stop rrbot_position_controller + ros2 service call /controller_manager/manage_hardware_activity controller_manager_msgs/srv/ManageHardwareActivity "{activate: [], deactivate: [RRBotSystemPositionOnly]}" + ``` + + Scenario state: + - right robot is "standing" + - left robot is moving + - middle robot is moving + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + ``` + + 1. Cleanup `RRBotSystemPositionOnly` and its joint state broadcaster. + Also restart global joint state broadcaster. Call + ``` + ros2 control switch_controllers --stop rrbot_position_controller joint_state_broadcaster + ros2 service call /controller_manager/cleanup_hardware_component controller_manager_msgs/srv/CleanupHardwareComponent "{name: RRBotSystemPositionOnly}" + ros2 control switch_controllers --start joint_state_broadcaster + ``` + + Scenario state (everything is broken during `joint_state_broadcaster` restart): + - right robot is "broken" + - left robot is moving + - middle robot is moving + + Hardware status: + Controllers status: (`ros2 control list_controllers`) + ``` + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_external_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] inactive + rrbot_position_controller[forward_command_controller/ForwardCommandController] inactive + rrbot_with_sensor_fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active + rrbot_with_sensor_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + rrbot_with_sensor_position_controller[forward_command_controller/ForwardCommandController] active + threedofbot_joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active + threedofbot_pid_gain_controller[forward_command_controller/ForwardCommandController] active + threedofbot_position_controller[forward_command_controller/ForwardCommandController] active + ``` + ## Controllers and moving hardware diff --git a/ros2_control_demo_bringup/config/three_robots_controllers.yaml b/ros2_control_demo_bringup/config/three_robots_controllers.yaml new file mode 100644 index 00000000..2e3cbec4 --- /dev/null +++ b/ros2_control_demo_bringup/config/three_robots_controllers.yaml @@ -0,0 +1,118 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + # Decide which hardware component should be started immediately + autostart_components: + - RRBotSystemPositionOnly + - ExternalRRBotFTSensor + # Decide which hardware component should start configured + autoconfigure_components: + - RRBotSystemWithSensor + + # Global controllers + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + # RRBot controllers + rrbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rrbot_position_controller: + type: forward_command_controller/ForwardCommandController + + rrbot_external_fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # RRBot with sensor controllers + rrbot_with_sensor_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + rrbot_with_sensor_position_controller: + type: forward_command_controller/ForwardCommandController + + rrbot_with_sensor_fts_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + # ThreeDofBot controllers + threedofbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + threedofbot_position_controller: + type: forward_command_controller/ForwardCommandController + + threedofbot_pid_gain_controller: + type: forward_command_controller/ForwardCommandController + + +# RRBot controllers +rrbot_joint_state_broadcaster: + ros__parameters: + joints: + - rrbot_joint1 + - rrbot_joint2 + interfaces: + - position + +rrbot_position_controller: + ros__parameters: + joints: + - rrbot_joint1 + - rrbot_joint2 + interface_name: position + +rrbot_external_fts_broadcaster: + ros__parameters: + sensor_name: rrbot_tcp_fts_sensor + frame_id: tool_link + + +# RRBot with sensor controllers +rrbot_with_sensor_joint_state_broadcaster: + ros__parameters: + joints: + - rrbot_with_sensor_joint1 + - rrbot_with_sensor_joint2 + interfaces: + - position + +rrbot_with_sensor_position_controller: + ros__parameters: + joints: + - rrbot_with_sensor_joint1 + - rrbot_with_sensor_joint2 + interface_name: position + +rrbot_with_sensor_fts_broadcaster: + ros__parameters: + interface_names.force.x: rrbot_with_sensor_tcp_fts_sensor/force.x + interface_names.torque.z: rrbot_with_sensor_tcp_fts_sensor/torque.z + frame_id: tool_link + + +# ThreeDofBot controllers +threedofbot_joint_state_broadcaster: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interfaces: + - position + - pid_gain + +threedofbot_position_controller: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interface_name: position + +threedofbot_pid_gain_controller: + ros__parameters: + joints: + - threedofbot_joint1 + - threedofbot_joint2 + - threedofbot_joint3 + interface_name: pid_gain diff --git a/ros2_control_demo_bringup/config/three_robots_position_command_publishers.yaml b/ros2_control_demo_bringup/config/three_robots_position_command_publishers.yaml new file mode 100644 index 00000000..512931a1 --- /dev/null +++ b/ros2_control_demo_bringup/config/three_robots_position_command_publishers.yaml @@ -0,0 +1,37 @@ +rrbot_position_command_publisher: + ros__parameters: + + controller_name: "rrbot_position_controller" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] + + +rrbot_with_sensor_position_command_publisher: + ros__parameters: + + controller_name: "rrbot_with_sensor_position_controller" + wait_sec_between_publish: 4 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785] + pos2: [0, 0] + pos3: [-0.785, -0.785] + pos4: [0, 0] + + +threedofbot_position_command_publisher: + ros__parameters: + + controller_name: "threedofbot_position_controller" + wait_sec_between_publish: 3 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785] + pos2: [0, 0, 0] + pos3: [-0.785, -0.785, -0.785] + pos4: [0, 0, 0] diff --git a/ros2_control_demo_bringup/launch/three_robots.launch.py b/ros2_control_demo_bringup/launch/three_robots.launch.py new file mode 100644 index 00000000..481cd5ac --- /dev/null +++ b/ros2_control_demo_bringup/launch/three_robots.launch.py @@ -0,0 +1,225 @@ +# 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: Denis Stogl + +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( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ) + ) + + # Initialize Arguments + slowdown = LaunchConfiguration("slowdown") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("rrbot_description"), + "urdf", + "three_robots.urdf.xacro", + ] + ), + " slowdown:=", + slowdown, + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_bringup"), + "config", + "three_robots_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("rrbot_description"), "config", "three_robots.rviz"] + ) + + position_goals = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_bringup"), + "config", + "three_robots_position_command_publishers.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + remappings=[ + ("/joint_state_broadcaster/joint_states", "joint_states"), + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + 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], + ) + + # Separate robot state publishers for each robot + + # Global joint state broadcaster + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + # RRBot controllers + rrbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rrbot_joint_state_broadcaster", "-c", "/controller_manager"], + ) + rrbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rrbot_position_controller", "-c", "/controller_manager"], + ) + # External FTS broadcaster + rrbot_external_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rrbot_external_fts_broadcaster", "-c", "/controller_manager"], + ) + + # RRBot controllers + rrbot_with_sensor_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rrbot_with_sensor_joint_state_broadcaster", "-c", "/controller_manager"], + ) + rrbot_with_sensor_position_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=[ + "rrbot_with_sensor_position_controller", + "-c", + "/controller_manager", + "--stopped", + ], + ) + rrbot_with_sensor_fts_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["rrbot_with_sensor_fts_broadcaster", "-c", "/controller_manager"], + ) + + # ThreeDofBot controllers + threedofbot_joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=[ + "threedofbot_joint_state_broadcaster", + "-c", + "/controller_manager", + "--stopped", + ], + ) + threedofbot_position_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["threedofbot_position_controller", "-c", "/controller_manager", "--stopped"], + ) + threedofbot_pid_gain_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--stopped"], + ) + + # Command publishers + rrbot_position_command_publisher = Node( + package="ros2_control_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + rrbot_with_sensor_position_command_publisher = Node( + package="ros2_control_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_with_sensor_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + threedofbot_position_command_publisher = Node( + package="ros2_control_test_nodes", + executable="publisher_forward_position_controller", + name="threedofbot_position_command_publisher", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + nodes = [ + control_node, + robot_state_pub_node, + rviz_node, + joint_state_broadcaster_spawner, + rrbot_joint_state_broadcaster_spawner, + rrbot_position_controller_spawner, + rrbot_external_fts_broadcaster_spawner, + rrbot_with_sensor_joint_state_broadcaster_spawner, + rrbot_with_sensor_position_controller_spawner, + rrbot_with_sensor_fts_broadcaster_spawner, + threedofbot_joint_state_broadcaster_spawner, + threedofbot_position_controller_spawner, + threedofbot_pid_gain_controller_spawner, + rrbot_position_command_publisher, + rrbot_with_sensor_position_command_publisher, + threedofbot_position_command_publisher, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/ros2_control_demo_description/rrbot_description/config/three_robots.rviz b/ros2_control_demo_description/rrbot_description/config/three_robots.rviz new file mode 100644 index 00000000..aa589c99 --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/config/three_robots.rviz @@ -0,0 +1,340 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1110 + - 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 + rrbot_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_with_sensor_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + rrbot_with_sensor_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rrbot_with_sensor_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + threedofbot_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + threedofbot_hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + threedofbot_tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: All Bots + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrobt_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RRBotSystemPositionOnly + TF Prefix: rrbot_ + Update Interval: 0 + Value: false + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rrbot_with_sensor_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RRBotWithSensor + TF Prefix: rrbot_with_sensor_ + Update Interval: 0 + Value: false + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /threedofbot_robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: ThreeDofBot + TF Prefix: threedofbot_ + Update Interval: 0 + Value: false + Visual Enabled: true + - Alpha: 1 + Arrow Width: 0.5 + Class: rviz_default_plugins/Wrench + Enabled: true + Force Arrow Scale: 2 + Force Color: 204; 51; 51 + History Length: 1 + Name: Wrench + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /fts_broadcaster/wrench + Torque Arrow Scale: 2 + Torque Color: 204; 204; 51 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + 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: 6.157698154449463 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.23817427456378937 + Y: -0.0642336755990982 + Z: 2.174346685409546 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3603994846343994 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.690403461456299 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1381 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000004fafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000004fa000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000004fafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000004fa000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000784000004fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 30 diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro new file mode 100644 index 00000000..a0fef083 --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro @@ -0,0 +1,42 @@ + + + + + + + + fake_components/GenericSystem + + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + + + diff --git a/ros2_control_demo_description/rrbot_description/urdf/three_robots.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/three_robots.urdf.xacro new file mode 100644 index 00000000..273a00fe --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/urdf/three_robots.urdf.xacro @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_control_demo_description/rrbot_description/urdf/threedofbot_description.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/threedofbot_description.urdf.xacro new file mode 100644 index 00000000..5f9f4eac --- /dev/null +++ b/ros2_control_demo_description/rrbot_description/urdf/threedofbot_description.urdf.xacro @@ -0,0 +1,240 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 3c5bfa0ffe19add51213cacb0a59b60b7221cee9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 1 Sep 2021 23:53:52 +0200 Subject: [PATCH 21/43] Bugfix: Change view_robot to be nicer and to actually work. --- .../launch/view_robot.launch.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/ros2_control_demo_bringup/launch/view_robot.launch.py b/ros2_control_demo_bringup/launch/view_robot.launch.py index 759b4b3e..ab476d11 100644 --- a/ros2_control_demo_bringup/launch/view_robot.launch.py +++ b/ros2_control_demo_bringup/launch/view_robot.launch.py @@ -90,12 +90,10 @@ def generate_launch_description(): arguments=["-d", rviz_config_file], ) - return LaunchDescription( - declared_arguments.append( - [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - ) - ) + nodes = declared_arguments + [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(nodes) From 36bfcbd47e495f31faf4c9a3131e17d3ab865121 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Tue, 7 Sep 2021 21:28:41 +0200 Subject: [PATCH 22/43] Update to newest interface state. --- .../ros2_control_demo_hardware/diffbot_system.hpp | 10 ++++------ .../external_rrbot_force_torque_sensor.hpp | 10 ++++------ .../rrbot_system_multi_interface.hpp | 13 +++++-------- .../rrbot_system_position_only.hpp | 12 +++++------- .../rrbot_system_with_sensor.hpp | 12 +++++------- ros2_control_demo_hardware/src/diffbot_system.cpp | 8 +++++--- .../src/external_rrbot_force_torque_sensor.cpp | 8 +++++--- .../src/rrbot_system_multi_interface.cpp | 8 +++++--- .../src/rrbot_system_position_only.cpp | 11 +++++++---- .../src/rrbot_system_with_sensor.cpp | 11 +++++++---- 10 files changed, 52 insertions(+), 51 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp index b60cfca3..ccf729f8 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp @@ -19,19 +19,17 @@ #include #include -#include "hardware_interface/base_interface.hpp" #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 "hardware_interface/types/hardware_interface_status_values.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" namespace ros2_control_demo_hardware { -class DiffBotSystemHardware -: public hardware_interface::BaseInterface +class DiffBotSystemHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); @@ -46,10 +44,10 @@ class DiffBotSystemHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate() override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index a02d7a4a..bba150a2 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -23,19 +23,17 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" namespace ros2_control_demo_hardware { -class ExternalRRBotForceTorqueSensorHardware -: public hardware_interface::BaseInterface +class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::SensorInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); @@ -47,10 +45,10 @@ class ExternalRRBotForceTorqueSensorHardware std::vector export_state_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate() override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp index e4193dbd..14dc42aa 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp @@ -20,20 +20,17 @@ #include #include -#include "rclcpp/macros.hpp" - -#include "hardware_interface/base_interface.hpp" #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 "hardware_interface/types/hardware_interface_status_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" namespace ros2_control_demo_hardware { -class RRBotSystemMultiInterfaceHardware -: public hardware_interface::BaseInterface +class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemMultiInterfaceHardware); @@ -53,10 +50,10 @@ class RRBotSystemMultiInterfaceHardware const std::vector & stop_interfaces) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate() override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp index b3f9ae5a..b15858eb 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp @@ -19,19 +19,17 @@ #include #include -#include "hardware_interface/base_interface.hpp" #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 "hardware_interface/types/hardware_interface_status_values.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" namespace ros2_control_demo_hardware { -class RRBotSystemPositionOnlyHardware -: public hardware_interface::BaseInterface +class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); @@ -40,7 +38,7 @@ class RRBotSystemPositionOnlyHardware CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_configure() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -49,10 +47,10 @@ class RRBotSystemPositionOnlyHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate() override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; 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 index 9e7b4914..cd6ace2f 100644 --- 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 @@ -23,19 +23,17 @@ #include #include -#include "hardware_interface/base_interface.hpp" #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 "hardware_interface/types/hardware_interface_status_values.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" namespace ros2_control_demo_hardware { -class RRBotSystemWithSensorHardware -: public hardware_interface::BaseInterface +class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemWithSensorHardware); @@ -44,7 +42,7 @@ class RRBotSystemWithSensorHardware CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_configure() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -53,10 +51,10 @@ class RRBotSystemWithSensorHardware std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate() override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; diff --git a/ros2_control_demo_hardware/src/diffbot_system.cpp b/ros2_control_demo_hardware/src/diffbot_system.cpp index 6557aeb7..e6d78dac 100644 --- a/ros2_control_demo_hardware/src/diffbot_system.cpp +++ b/ros2_control_demo_hardware/src/diffbot_system.cpp @@ -27,7 +27,7 @@ namespace ros2_control_demo_hardware { CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::HardwareInfo & info) { - if (on_init_default(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -121,7 +121,8 @@ std::vector DiffBotSystemHardware::export_ return command_interfaces; } -CallbackReturn DiffBotSystemHardware::on_activate() +CallbackReturn DiffBotSystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Starting ...please wait..."); @@ -148,7 +149,8 @@ CallbackReturn DiffBotSystemHardware::on_activate() return CallbackReturn::SUCCESS; } -CallbackReturn DiffBotSystemHardware::on_deactivate() +CallbackReturn DiffBotSystemHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Stopping ...please wait..."); diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index 7b6166bf..34f25488 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -32,7 +32,7 @@ namespace ros2_control_demo_hardware CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (on_init_default(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SensorInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -62,7 +62,8 @@ ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() return state_interfaces; } -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate() +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); @@ -81,7 +82,8 @@ CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate() return CallbackReturn::SUCCESS; } -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate() +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); diff --git a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp index d0120635..ea18da0c 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp @@ -29,7 +29,7 @@ namespace ros2_control_demo_hardware CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (on_init_default(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -191,7 +191,8 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_comma return hardware_interface::return_type::OK; } -CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate() +CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Starting... please wait..."); @@ -240,7 +241,8 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate() return CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate() +CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Stopping... please wait..."); diff --git a/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp b/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp index dc1babcc..6d07a62a 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp @@ -28,7 +28,7 @@ namespace ros2_control_demo_hardware CallbackReturn RRBotSystemPositionOnlyHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (on_init_default(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -82,7 +82,8 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_init( return CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemPositionOnlyHardware::on_configure() +CallbackReturn RRBotSystemPositionOnlyHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Configuring ...please wait..."); @@ -134,7 +135,8 @@ RRBotSystemPositionOnlyHardware::export_command_interfaces() return command_interfaces; } -CallbackReturn RRBotSystemPositionOnlyHardware::on_activate() +CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Starting ...please wait..."); @@ -158,7 +160,8 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_activate() return CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate() +CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Stopping ...please wait..."); diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp index f7de2f43..6f58c484 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -31,7 +31,7 @@ namespace ros2_control_demo_hardware { CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface::HardwareInfo & info) { - if (on_init_default(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -89,7 +89,8 @@ CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface:: return CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemWithSensorHardware::on_configure() +CallbackReturn RRBotSystemWithSensorHardware::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Configuring ...please wait..."); @@ -147,7 +148,8 @@ RRBotSystemWithSensorHardware::export_command_interfaces() return command_interfaces; } -CallbackReturn RRBotSystemWithSensorHardware::on_activate() +CallbackReturn RRBotSystemWithSensorHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Starting ...please wait..."); @@ -180,7 +182,8 @@ CallbackReturn RRBotSystemWithSensorHardware::on_activate() return CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemWithSensorHardware::on_deactivate() +CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Stopping ...please wait..."); From f587d2b72f1ad9f71003799bec1e355a07ebf102 Mon Sep 17 00:00:00 2001 From: bailaC Date: Thu, 8 Jul 2021 22:24:47 +0530 Subject: [PATCH 23/43] Addressing review comments. --- ...bot_force_torque_sensor.ros2_control.xacro | 44 ++++++------ ...bot_system_with_external_sensor.urdf.xacro | 27 ++++---- ros2_control_demo_hardware/CMakeLists.txt | 3 +- .../external_rrbot_force_torque_sensor.hpp | 16 +++-- .../external_rrbot_force_torque_sensor.cpp | 69 ++++++------------- 5 files changed, 71 insertions(+), 88 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 53d4059c..af67e255 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -4,21 +4,29 @@ - - - fake_components/GenericSystem - ${fake_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 0.0 - 3.0 - 5.0 - - + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + 2.0 + 3.0 + ${slowdown} + 5.0 + + + - + @@ -26,12 +34,8 @@ tool_link - 100 - 100 - 100 - 15 - 15 - 15 + 100 + 15 diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 642a0f62..7bc085fe 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -1,5 +1,5 @@ - + - + + + + - + - - - - + @@ -35,14 +35,17 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + + - + fake_sensor_commands="$(arg fake_sensor_commands)" + slowdown="$(arg slowdown)" /> diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 9a9aa7db..6f87bf93 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -21,10 +21,11 @@ add_library( ${PROJECT_NAME} SHARED src/diffbot_system.cpp - src/external_rrbot_force_torque_sensor.cpp src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp src/rrbot_system_with_sensor.cpp + src/rrbot_actuator.cpp + src/external_rrbot_force_torque_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index bba150a2..a3401aeb 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -23,35 +23,39 @@ #include #include +#include "hardware_interface/base_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" +using hardware_interface::return_type; + namespace ros2_control_demo_hardware { -class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::SensorInterface +class ExternalRRBotForceTorqueSensorHardware +: public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + 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 - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + return_type start() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + return_type stop() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type read() override; + return_type read() override; private: // Parameters for the RRBot simulation diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index 34f25488..909a9ee2 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -29,46 +30,26 @@ namespace ros2_control_demo_hardware { -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SensorInterface::on_init(info) != CallbackReturn::SUCCESS) + if (configure_default(info) != hardware_interface::return_type::OK) { - return CallbackReturn::ERROR; + return hardware_interface::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_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); - hw_sensor_states_.resize( - info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - - return CallbackReturn::SUCCESS; -} - -std::vector -ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() -{ - std::vector state_interfaces; - - // 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; + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; } -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); - for (int i = 0; i < hw_start_sec_; i++) + for (int i = 0; i <= hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( @@ -76,19 +57,19 @@ CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( hw_start_sec_ - i); } + status_ = hardware_interface::status::STARTED; + RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System Successfully started!"); - return CallbackReturn::SUCCESS; + return hardware_interface::return_type::OK; } -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() { - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); - for (int i = 0; i < hw_stop_sec_; i++) + for (int i = 0; i <= hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( @@ -96,28 +77,19 @@ CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate( hw_stop_sec_ - i); } + status_ = hardware_interface::status::STOPPED; + RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System successfully stopped!"); - return CallbackReturn::SUCCESS; + return hardware_interface::return_type::OK; } hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() { RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); - 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("ExternalRRBotForceTorqueSensorHardware"), "Got state %e for sensor %zu!", - hw_sensor_states_[i], i); - } - RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); + RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); return hardware_interface::return_type::OK; } @@ -127,5 +99,4 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, - hardware_interface::SensorInterface) + ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, hardware_interface::SensorInterface) From 235d08930aee04de89c17412a12e335233d74566 Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 9 Jul 2021 18:04:22 +0530 Subject: [PATCH 24/43] Update ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 7bc085fe..a9b149d7 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -1,5 +1,5 @@ - + + + + diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index a3401aeb..33d1167c 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -37,7 +37,7 @@ using hardware_interface::return_type; namespace ros2_control_demo_hardware { class ExternalRRBotForceTorqueSensorHardware -: public hardware_interface::BaseInterface + : public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index f1743942..fd34eb4a 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -32,13 +32,13 @@ namespace ros2_control_demo_hardware hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) - { + if (configure_default(info) != hardware_interface::return_type::OK) { return hardware_interface::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_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); status_ = hardware_interface::status::CONFIGURED; return hardware_interface::return_type::OK; @@ -46,10 +46,11 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configur hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), + "Starting ...please wait..."); - for (int i = 0; i <= hw_start_sec_; i++) - { + for (int i = 0; i <= hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -66,10 +67,11 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() { - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), + "Stopping ...please wait..."); - for (int i = 0; i <= hw_stop_sec_; i++) - { + for (int i = 0; i <= hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -88,7 +90,18 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() { RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); - RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully 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("ExternalRRBotForceTorqueSensorHardware"), "Got state %e for sensor %zu!", + hw_sensor_states_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), + "Joints successfully read!"); return hardware_interface::return_type::OK; } @@ -98,4 +111,5 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, hardware_interface::SensorInterface) + ros2_control_demo_hardware::ExternalRRBotForceTorqueSensorHardware, + hardware_interface::SensorInterface) From 284744598d15e9bc06c319fa6d490e40a1c495e2 Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 18:36:29 +0530 Subject: [PATCH 27/43] Update ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../src/external_rrbot_force_torque_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index fd34eb4a..0831a5a6 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -50,7 +50,7 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); - for (int i = 0; i <= hw_start_sec_; i++) { + for (int i = 0; i < hw_start_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", From deabda3dc996be495294a0668ceb0fb4bf683bb1 Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 18:36:47 +0530 Subject: [PATCH 28/43] Update ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../src/external_rrbot_force_torque_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index 0831a5a6..b61e3739 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -71,7 +71,7 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); - for (int i = 0; i <= hw_stop_sec_; i++) { + for (int i = 0; i < hw_stop_sec_; i++) { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", From 206fa3a7af151d388011e6a8721cd00148c59a64 Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 18:36:59 +0530 Subject: [PATCH 29/43] Update ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 70169b7e..a7b126c5 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -18,7 +18,7 @@ ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 2.0 + 0.0 3.0 5.0 From e1d9929ebe8919a4b545e2bc38424f1c189225c5 Mon Sep 17 00:00:00 2001 From: bailaC Date: Tue, 13 Jul 2021 19:15:29 +0530 Subject: [PATCH 30/43] Addressing review comments. --- ...bot_force_torque_sensor.ros2_control.xacro | 37 +++++++++---------- ...bot_system_with_external_sensor.urdf.xacro | 5 --- 2 files changed, 17 insertions(+), 25 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index a7b126c5..725a3617 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -4,26 +4,19 @@ - - - gazebo_ros2_control/GazeboSystem - - - - - - fake_components/GenericSystem - ${fake_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 0.0 - 3.0 - 5.0 - - - + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware + 2.0 + 3.0 + 5.0 + + @@ -34,6 +27,10 @@ tool_link 100 + 100 + 100 + 15 + 15 15 diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 72da192e..22cd5a06 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -16,9 +16,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - - - @@ -38,8 +35,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - - Date: Wed, 29 Jun 2022 20:30:46 +0530 Subject: [PATCH 31/43] resolving merge conflicts --- .../rrbot_with_external_sensor_controllers.yaml | 9 +++++++++ .../rrbot_system_with_external_sensor.launch.py | 15 +++++++++++++++ .../rrbot_system_with_external_sensor.urdf.xacro | 4 ++-- .../ros2_control_demo_hardware.xml | 3 +++ .../src/external_rrbot_force_torque_sensor.cpp | 15 +++++++++++++++ 5 files changed, 44 insertions(+), 2 deletions(-) diff --git a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml index c7759ea6..b78f6cec 100644 --- a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml +++ b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -1,6 +1,10 @@ controller_manager: ros__parameters: +<<<<<<< HEAD update_rate: 100 # Hz +======= + update_rate: 2 # Hz +>>>>>>> e87ddc4 (Corrections.) forward_position_controller: type: forward_command_controller/ForwardCommandController @@ -20,5 +24,10 @@ forward_position_controller: fts_broadcaster: ros__parameters: +<<<<<<< HEAD sensor_name: tcp_fts_sensor +======= + interface_names.force.x: tcp_fts_sensor/force.x + interface_names.torque.z: tcp_fts_sensor/torque.z +>>>>>>> e87ddc4 (Corrections.) frame_id: tool_link diff --git a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py index 3a669a0a..3fa623e6 100755 --- a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -87,4 +87,19 @@ def generate_launch_description(): fts_broadcaster_spawner, ] +<<<<<<< HEAD +======= + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot.launch.py"]), + launch_arguments={ + "controllers_file": "rrbot_with_external_sensor_controllers.yaml", + "description_file": "rrbot_system_with_external_sensor.urdf.xacro", + "prefix": prefix, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "slowdown": slowdown, + }.items(), + ) + +>>>>>>> e87ddc4 (Corrections.) return LaunchDescription(declared_arguments + [base_launch] + nodes) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 22cd5a06..89390868 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -20,7 +20,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + @@ -35,7 +35,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - +<<<<<<< HEAD @@ -34,4 +35,6 @@ 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. +======= +>>>>>>> e87ddc4 (Corrections.) diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index b61e3739..a87314db 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -44,6 +44,21 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configur return hardware_interface::return_type::OK; } +std::vector +ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() +{ + std::vector state_interfaces; + + // 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; +} + hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { RCLCPP_INFO( From 5eb6858d1f2a8b9464caee10717193fc76cb7136 Mon Sep 17 00:00:00 2001 From: bailaC Date: Thu, 15 Jul 2021 20:09:31 +0530 Subject: [PATCH 32/43] Corrections. --- ...rbot_with_external_sensor_controllers.yaml | 9 --- ...rbot_system_with_external_sensor.launch.py | 15 ---- ros2_control_demo_hardware/CMakeLists.txt | 5 +- .../diffbot_system.hpp | 19 +++-- .../external_rrbot_force_torque_sensor.hpp | 2 - .../rrbot_system_multi_interface.hpp | 17 +++-- .../rrbot_system_with_sensor.hpp | 18 +++-- ros2_control_demo_hardware/package.xml | 2 + .../ros2_control_demo_hardware.xml | 10 --- .../src/diffbot_system.cpp | 70 +++++++++++------- .../src/rrbot_system_multi_interface.cpp | 35 +++++---- .../src/rrbot_system_with_sensor.cpp | 73 +++++++++++-------- 12 files changed, 146 insertions(+), 129 deletions(-) diff --git a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml index b78f6cec..c7759ea6 100644 --- a/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml +++ b/ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml @@ -1,10 +1,6 @@ controller_manager: ros__parameters: -<<<<<<< HEAD update_rate: 100 # Hz -======= - update_rate: 2 # Hz ->>>>>>> e87ddc4 (Corrections.) forward_position_controller: type: forward_command_controller/ForwardCommandController @@ -24,10 +20,5 @@ forward_position_controller: fts_broadcaster: ros__parameters: -<<<<<<< HEAD sensor_name: tcp_fts_sensor -======= - interface_names.force.x: tcp_fts_sensor/force.x - interface_names.torque.z: tcp_fts_sensor/torque.z ->>>>>>> e87ddc4 (Corrections.) frame_id: tool_link diff --git a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py index 3fa623e6..3a669a0a 100755 --- a/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -87,19 +87,4 @@ def generate_launch_description(): fts_broadcaster_spawner, ] -<<<<<<< HEAD -======= - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot.launch.py"]), - launch_arguments={ - "controllers_file": "rrbot_with_external_sensor_controllers.yaml", - "description_file": "rrbot_system_with_external_sensor.urdf.xacro", - "prefix": prefix, - "use_fake_hardware": use_fake_hardware, - "fake_sensor_commands": fake_sensor_commands, - "slowdown": slowdown, - }.items(), - ) - ->>>>>>> e87ddc4 (Corrections.) return LaunchDescription(declared_arguments + [base_launch] + nodes) diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 6f87bf93..9ed959a4 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -21,11 +21,10 @@ add_library( ${PROJECT_NAME} SHARED src/diffbot_system.cpp - src/rrbot_system_position_only.cpp + #src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp src/rrbot_system_with_sensor.cpp - src/rrbot_actuator.cpp - src/external_rrbot_force_torque_sensor.cpp + #src/external_rrbot_force_torque_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp index ccf729f8..1becc4dd 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp @@ -23,7 +23,11 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" @@ -35,7 +39,8 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface RCLCPP_SHARED_PTR_DEFINITIONS(DiffBotSystemHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -44,16 +49,20 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type read() override; + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type write() override; + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; private: // Parameters for the DiffBot simulation diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index 33d1167c..450c86e3 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -23,12 +23,10 @@ #include #include -#include "hardware_interface/base_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/sensor_interface.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" diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp index 14dc42aa..74c00ed3 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp @@ -36,7 +36,8 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemMultiInterfaceHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -50,16 +51,22 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter const std::vector & stop_interfaces) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type read() override; + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period + ) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type write() override; + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period + ) override; private: // Parameters for the RRBot simulation 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 index cd6ace2f..2c3b2a3a 100644 --- 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 @@ -39,10 +39,12 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemWithSensorHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -51,16 +53,20 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type read() override; + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::return_type write() override; + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; private: // Parameters for the RRBot simulation diff --git a/ros2_control_demo_hardware/package.xml b/ros2_control_demo_hardware/package.xml index 1798d238..71dafa62 100644 --- a/ros2_control_demo_hardware/package.xml +++ b/ros2_control_demo_hardware/package.xml @@ -11,6 +11,8 @@ ament_cmake + lifecycle_msgs + hardware_interface pluginlib rclcpp diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 73dab551..e648c917 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -27,14 +27,4 @@ The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot. -<<<<<<< HEAD - - - 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. - - -======= ->>>>>>> e87ddc4 (Corrections.) diff --git a/ros2_control_demo_hardware/src/diffbot_system.cpp b/ros2_control_demo_hardware/src/diffbot_system.cpp index a66d04ad..b2792c94 100644 --- a/ros2_control_demo_hardware/src/diffbot_system.cpp +++ b/ros2_control_demo_hardware/src/diffbot_system.cpp @@ -25,19 +25,24 @@ namespace ros2_control_demo_hardware { -CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::HardwareInfo & info) +hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( + const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } base_x_ = 0.0; base_y_ = 0.0; base_theta_ = 0.0; - 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"]); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); + hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -51,7 +56,7 @@ CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::Hardware rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) @@ -60,7 +65,7 @@ CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::Hardware rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 2) @@ -69,7 +74,7 @@ CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::Hardware rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -78,7 +83,7 @@ CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::Hardware rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); - return hardware_interface::return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) @@ -87,11 +92,11 @@ CallbackReturn DiffBotSystemHardware::on_init(const hardware_interface::Hardware rclcpp::get_logger("DiffBotSystemHardware"), "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(), joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector DiffBotSystemHardware::export_state_interfaces() @@ -120,10 +125,11 @@ std::vector DiffBotSystemHardware::export_ return command_interfaces; } -CallbackReturn DiffBotSystemHardware::on_activate( +hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Starting ...please wait..."); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Activating ...please wait..."); for (auto i = 0; i < hw_start_sec_; i++) { @@ -131,6 +137,7 @@ CallbackReturn DiffBotSystemHardware::on_activate( RCLCPP_INFO( rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code // set some default values for (auto i = 0u; i < hw_positions_.size(); i++) @@ -143,15 +150,16 @@ CallbackReturn DiffBotSystemHardware::on_activate( } } - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "System Successfully started!"); + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully activated!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn DiffBotSystemHardware::on_deactivate( +hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Stopping ...please wait..."); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Deactivating ...please wait..."); for (auto i = 0; i < hw_stop_sec_; i++) { @@ -159,31 +167,32 @@ CallbackReturn DiffBotSystemHardware::on_deactivate( RCLCPP_INFO( rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "System successfully stopped!"); + RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully deactivated!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type DiffBotSystemHardware::read() +hardware_interface::return_type DiffBotSystemHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Reading..."); - double radius = 0.02; // radius of the wheels double dist_w = 0.1; // distance between the wheels - double dt = 0.01; // Control period for (uint i = 0; i < hw_commands_.size(); i++) { // Simulate DiffBot wheels's movement as a first-order system // Update the joint status: this is a revolute joint without any limit. // Simply integrates - hw_positions_[i] = hw_positions_[1] + dt * hw_commands_[i]; + hw_positions_[i] = hw_positions_[1] + period.seconds() * hw_commands_[i]; hw_velocities_[i] = hw_commands_[i]; + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( rclcpp::get_logger("DiffBotSystemHardware"), "Got position state %.5f and velocity state %.5f for '%s'!", hw_positions_[i], hw_velocities_[i], info_.joints[i].name.c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code } // Update the free-flyer, i.e. the base notation using the classical @@ -191,19 +200,23 @@ hardware_interface::return_type DiffBotSystemHardware::read() double base_dx = 0.5 * radius * (hw_commands_[0] + hw_commands_[1]) * cos(base_theta_); double base_dy = 0.5 * radius * (hw_commands_[0] + hw_commands_[1]) * sin(base_theta_); double base_dtheta = radius * (hw_commands_[0] - hw_commands_[1]) / dist_w; - base_x_ += base_dx * dt; - base_y_ += base_dy * dt; - base_theta_ += base_dtheta * dt; + base_x_ += base_dx * period.seconds(); + base_y_ += base_dy * period.seconds(); + base_theta_ += base_dtheta * period.seconds(); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO( rclcpp::get_logger("DiffBotSystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)", base_x_, base_y_, base_theta_); + // 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 ros2_control_demo_hardware::DiffBotSystemHardware::write() +hardware_interface::return_type ros2_control_demo_hardware::DiffBotSystemHardware::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("DiffBotSystemHardware"), "Writing..."); for (auto i = 0u; i < hw_commands_.size(); i++) @@ -214,6 +227,7 @@ hardware_interface::return_type ros2_control_demo_hardware::DiffBotSystemHardwar info_.joints[i].name.c_str()); } RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "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; } diff --git a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp index 09dee859..0b52bd38 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp @@ -26,12 +26,13 @@ namespace ros2_control_demo_hardware { -CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( +hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); @@ -55,7 +56,7 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Joint '%s' has %zu command interfaces. 3 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -67,7 +68,7 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( "Joint '%s' has %s command interface. Expected %s, %s, or %s.", joint.name.c_str(), joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 3) @@ -76,7 +77,7 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "Joint '%s'has %zu state interfaces. 3 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return return_type::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -88,11 +89,11 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( "Joint '%s' has %s state interface. Expected %s, %s, or %s.", joint.name.c_str(), joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector @@ -193,7 +194,7 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_comma return hardware_interface::return_type::OK; } -CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate( +hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( @@ -238,12 +239,12 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate( } RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully started! %hhu", + rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully started! %d", control_level_[0]); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate( +hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO( @@ -260,10 +261,11 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate( RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully stopped!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read() +hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { for (std::size_t i = 0; i < hw_positions_.size(); i++) { @@ -299,7 +301,8 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read() return hardware_interface::return_type::OK; } -hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write() +hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { /*RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), @@ -309,7 +312,7 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::write() // Simulate sending commands to the hardware RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), - "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %lu, control_lvl:%hhu", + "Got the commands pos: %.5f, vel: %.5f, acc: %.5f for joint %lu, control_lvl:%d", hw_commands_positions_[i], hw_commands_velocities_[i], hw_commands_accelerations_[i], i, control_level_[i]); } diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp index 6f58c484..dbeb9758 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -29,17 +29,22 @@ namespace ros2_control_demo_hardware { -CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface::HardwareInfo & info) +hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_init( + const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { - return CallbackReturn::ERROR; + 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"]); hw_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); + // END: This part here is for exemplary purposes - Please do not copy to your production code hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -55,7 +60,7 @@ CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface:: rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), joint.command_interfaces.size()); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -64,7 +69,7 @@ CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface:: rclcpp::get_logger("RRBotSystemWithSensorHardware"), "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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) @@ -73,7 +78,7 @@ CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface:: rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), joint.state_interfaces.size()); - return CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) @@ -82,16 +87,17 @@ CallbackReturn RRBotSystemWithSensorHardware::on_init(const hardware_interface:: rclcpp::get_logger("RRBotSystemWithSensorHardware"), "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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemWithSensorHardware::on_configure( +hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::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("RRBotSystemWithSensorHardware"), "Configuring ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) @@ -101,6 +107,7 @@ CallbackReturn RRBotSystemWithSensorHardware::on_configure( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "%.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_joint_states_.size(); i++) @@ -109,10 +116,9 @@ CallbackReturn RRBotSystemWithSensorHardware::on_configure( hw_joint_commands_[i] = 0; } - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "System Successfully configured!"); + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully configured!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector @@ -148,10 +154,11 @@ RRBotSystemWithSensorHardware::export_command_interfaces() return command_interfaces; } -CallbackReturn RRBotSystemWithSensorHardware::on_activate( +hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Starting ...please wait..."); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Activating ...please wait..."); for (int i = 0; i < hw_start_sec_; i++) { @@ -160,15 +167,12 @@ CallbackReturn RRBotSystemWithSensorHardware::on_activate( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "%.1f seconds left...", hw_start_sec_ - i); } + // END: This part here is for exemplary purposes - Please do not copy to your production code - // set some default values for joints + // command and state should be equal when starting 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; - } + hw_joint_states_[i] = hw_joint_states_[i]; } // set default value for sensor @@ -177,15 +181,17 @@ CallbackReturn RRBotSystemWithSensorHardware::on_activate( hw_sensor_states_[0] = 0; } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "System Successfully started!"); + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully activated!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( +hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Stopping ...please wait..."); + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Deactivating ...please wait..."); for (int i = 0; i < hw_stop_sec_; i++) { @@ -195,13 +201,16 @@ CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( hw_stop_sec_ - i); } - RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "System successfully stopped!"); + RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully deactivated!"); + // END: This part here is for exemplary purposes - Please do not copy to your production code - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type RRBotSystemWithSensorHardware::read() +hardware_interface::return_type RRBotSystemWithSensorHardware::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("RRBotSystemWithSensorHardware"), "Reading...please wait..."); for (uint i = 0; i < hw_joint_states_.size(); i++) @@ -210,7 +219,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() hw_joint_states_[i] = hw_joint_commands_[i] + (hw_joint_states_[i] - hw_joint_commands_[i]) / hw_slowdown_; RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %zu!", + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %u!", hw_joint_states_[i], i); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Joints successfully read!"); @@ -226,22 +235,26 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() hw_sensor_states_[i], info_.sensors[0].state_interfaces[i].name.c_str()); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Sensors 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 ros2_control_demo_hardware::RRBotSystemWithSensorHardware::write() +hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemWithSensorHardware::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("RRBotSystemWithSensorHardware"), "Writing...please wait..."); for (uint i = 0; i < hw_joint_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got command %.5f for joint %zu!", + rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got command %.5f for joint %u!", hw_joint_commands_[i], i); } RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "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; } From 49d860c22813e38dc93603071bfcf8acbcd78355 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 28 Jul 2021 21:15:49 +0530 Subject: [PATCH 33/43] Update ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 725a3617..2a0e5973 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -12,7 +12,7 @@ ros2_control_demo_hardware/ExternalRRBotForceTorqueSensorHardware - 2.0 + 0.0 3.0 5.0 From e77687b943893b5820895a58b6add18fd94772fd Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 28 Jul 2021 21:16:19 +0530 Subject: [PATCH 34/43] Update ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 89390868..263d9c51 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -11,7 +11,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + From 8946a9844f635e7fae32a929f586e48bffa2a73c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 26 Aug 2021 14:25:18 +0200 Subject: [PATCH 35/43] Correct format. --- .../external_rrbot_force_torque_sensor.hpp | 2 +- .../external_rrbot_force_torque_sensor.cpp | 21 ++++++++++--------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index 450c86e3..fc2e2908 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -35,7 +35,7 @@ using hardware_interface::return_type; namespace ros2_control_demo_hardware { class ExternalRRBotForceTorqueSensorHardware - : public hardware_interface::BaseInterface +: public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index a87314db..c72c4a65 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -32,7 +32,8 @@ namespace ros2_control_demo_hardware hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) { + if (configure_default(info) != hardware_interface::return_type::OK) + { return hardware_interface::return_type::ERROR; } @@ -62,10 +63,10 @@ ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() { RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), - "Starting ...please wait..."); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); - for (int i = 0; i < hw_start_sec_; i++) { + for (int i = 0; i < hw_start_sec_; i++) + { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -83,10 +84,10 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() { RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), - "Stopping ...please wait..."); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); - for (int i = 0; i < hw_stop_sec_; i++) { + for (int i = 0; i < hw_stop_sec_; i++) + { rclcpp::sleep_for(std::chrono::seconds(1)); RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "%.1f seconds left...", @@ -105,7 +106,8 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() { RCLCPP_INFO(rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); - for (uint i = 0; i < hw_sensor_states_.size(); i++) { + 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] = @@ -115,8 +117,7 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() hw_sensor_states_[i], i); } RCLCPP_INFO( - rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), - "Joints successfully read!"); + rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Joints successfully read!"); return hardware_interface::return_type::OK; } From 2b763cc1a876f7f2537daa282174fd3121c15fe9 Mon Sep 17 00:00:00 2001 From: bailaC Date: Sat, 31 Jul 2021 21:27:48 +0530 Subject: [PATCH 36/43] Addressing review comments --- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 263d9c51..fbb5d475 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -14,7 +14,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + From cc3f2ded1352ae23a7ab9418661a9d4cf73688da Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 6 Jul 2022 09:08:31 +0530 Subject: [PATCH 37/43] Adding example text --- README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 362ac6ba..df94fe47 100644 --- a/README.md +++ b/README.md @@ -64,9 +64,9 @@ git clone https://github.com/ros-controls/ros2_control_demos We provide officially released and maintained debian packages, which can easily be installed via aptitude. However, there might be cases in which not-yet released demos or features are only available through a source build in your own workspace. -* Install dependencies: +* Install dependencies (maybe you need `sudo`): ``` - rosdep install --from-paths src --ignore-src -r -y + apt install ros-foxy-realtime-tools ros-foxy-xacro ros-foxy-angles ``` * Build everything, e.g. with: @@ -80,6 +80,7 @@ However, there might be cases in which not-yet released demos or features are on # Getting Started with demos This repository provides the following simple example robots: a 2 degrees of freedom manipulator - *RRBot* - and a mobile differential drive base - *DiffBot*. + The first two examples demonstrate the minimal setup for those two robots to run. Later examples show more details about `ros2_control`-concepts and some more advanced use-cases. @@ -96,9 +97,9 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` ros2 launch rrbot_description view_robot.launch.py ``` **NOTE**: Getting the following output in terminal is OK: `Warning: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist`. - This happens because `joint_state_publisher_gui` node need some time to start. - The `joint_state_publisher_gui` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in `Rviz`. + This happens because `joint_state_publisher_gui` node need some time to start. +The `joint_state_publisher_gui` provides a GUI to generate a random configuration for rrbot. It is immediately displayed in `Rviz`. 1. To start *RRBot* example open open a terminal, source your ROS2-workspace and execute its launch file with: ``` @@ -721,7 +722,6 @@ Notes: threedofbot_position_controller[forward_command_controller/ForwardCommandController] active ``` - ## Controllers and moving hardware To move the robot you should load and start controllers. @@ -844,7 +844,7 @@ Now you should also see the *RRbot* represented correctly in `RViz`. ## Result 1. Independently from the controller you should see how the example's output changes. - Look for the following lines + Look for the following lines ``` [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 0! [RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 1! From ecf35a7b5fa65f218902c7c6c0e20e0bd0d46ef9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 1 Sep 2021 23:35:25 +0200 Subject: [PATCH 38/43] Bugfix: Do not ignore prefix argument for joint names. --- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro index 2a0e5973..53d4059c 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro @@ -18,7 +18,7 @@ - + From 1f57ad6b519c13ab0ff6e0a131f47de3b2be5553 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 1 Sep 2021 23:39:49 +0200 Subject: [PATCH 39/43] Update example hardware to support new lifecycle of hardware. --- .../diffbot_system.hpp | 2 +- .../external_rrbot_force_torque_sensor.hpp | 10 ++++---- .../rrbot_system_position_only.hpp | 11 +++++---- .../rrbot_system_with_sensor.hpp | 2 +- .../external_rrbot_force_torque_sensor.cpp | 24 +++++++++---------- .../src/rrbot_system_with_sensor.cpp | 2 +- 6 files changed, 25 insertions(+), 26 deletions(-) diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp index 1becc4dd..8c135fdd 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp @@ -80,4 +80,4 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface } // namespace ros2_control_demo_hardware -#endif // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_ +#endif // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_ \ No newline at end of file diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index fc2e2908..79cdcf54 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -30,8 +30,6 @@ #include "rclcpp/macros.hpp" #include "ros2_control_demo_hardware/visibility_control.h" -using hardware_interface::return_type; - namespace ros2_control_demo_hardware { class ExternalRRBotForceTorqueSensorHardware @@ -41,19 +39,19 @@ class ExternalRRBotForceTorqueSensorHardware RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type configure(const hardware_interface::HardwareInfo & info) override; + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type start() override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type stop() override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - return_type read() override; + hardware_interface::return_type read() override; private: // Parameters for the RRBot simulation diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp index b15858eb..40c0194d 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_position_only.hpp @@ -19,17 +19,20 @@ #include #include +#include "hardware_interface/base_interface.hpp" #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 "hardware_interface/types/hardware_interface_status_values.hpp" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_demo_hardware/visibility_control.h" namespace ros2_control_demo_hardware { -class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface +class RRBotSystemPositionOnlyHardware +: public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); @@ -38,7 +41,7 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_configure() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -47,10 +50,10 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa std::vector export_command_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC hardware_interface::return_type read() override; 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 index 2c3b2a3a..9801bd3c 100644 --- 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 @@ -83,4 +83,4 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface } // namespace ros2_control_demo_hardware -#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ \ No newline at end of file diff --git a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp index c72c4a65..7b6166bf 100644 --- a/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp +++ b/ros2_control_demo_hardware/src/external_rrbot_force_torque_sensor.cpp @@ -29,20 +29,22 @@ namespace ros2_control_demo_hardware { -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::configure( +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (configure_default(info) != hardware_interface::return_type::OK) + if (on_init_default(info) != CallbackReturn::SUCCESS) { - return hardware_interface::return_type::ERROR; + return CallbackReturn::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_sensor_change_ = stod(info_.hardware_parameters["example_param_max_sensor_change"]); - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + hw_sensor_states_.resize( + info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + return CallbackReturn::SUCCESS; } std::vector @@ -60,7 +62,7 @@ ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() return state_interfaces; } -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate() { RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Starting ...please wait..."); @@ -73,15 +75,13 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::start() hw_start_sec_ - i); } - status_ = hardware_interface::status::STARTED; - RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System Successfully started!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() +CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate() { RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Stopping ...please wait..."); @@ -94,12 +94,10 @@ hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::stop() hw_stop_sec_ - i); } - status_ = hardware_interface::status::STOPPED; - RCLCPP_INFO( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "System successfully stopped!"); - return hardware_interface::return_type::OK; + return CallbackReturn::SUCCESS; } hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp index dbeb9758..327dc044 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -264,4 +264,4 @@ hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemWithSenso #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotSystemWithSensorHardware, hardware_interface::SystemInterface) + ros2_control_demo_hardware::RRBotSystemWithSensorHardware, hardware_interface::SystemInterface) \ No newline at end of file From 5553e974e75a355357ea13f24a05674463dd544b Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 6 Jul 2022 20:21:08 +0530 Subject: [PATCH 40/43] Fixing format --- .gitignore | 4 +++- .../include/ros2_control_demo_hardware/diffbot_system.hpp | 2 +- .../ros2_control_demo_hardware/rrbot_system_with_sensor.hpp | 2 +- ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp | 2 +- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index 3147b10e..e5630424 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,7 @@ .kdev4/ - +build/ +install/ +log/ *~ *.kate-swp diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp index 8c135fdd..1becc4dd 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/diffbot_system.hpp @@ -80,4 +80,4 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface } // namespace ros2_control_demo_hardware -#endif // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_ \ No newline at end of file +#endif // ROS2_CONTROL_DEMO_HARDWARE__DIFFBOT_SYSTEM_HPP_ 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 index 9801bd3c..2c3b2a3a 100644 --- 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 @@ -83,4 +83,4 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface } // namespace ros2_control_demo_hardware -#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ \ No newline at end of file +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp index 327dc044..dbeb9758 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -264,4 +264,4 @@ hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemWithSenso #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_hardware::RRBotSystemWithSensorHardware, hardware_interface::SystemInterface) \ No newline at end of file + ros2_control_demo_hardware::RRBotSystemWithSensorHardware, hardware_interface::SystemInterface) From b2e005590a1d4e2fd9e024aaddefe5cf2da2871e Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 6 Jul 2022 20:23:12 +0530 Subject: [PATCH 41/43] Revert "Auxiliary commit to revert individual files from 5553e974e75a355357ea13f24a05674463dd544b" This reverts commit f3591bf89431a033192e7046b67b461dcc45304f. --- .gitignore | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index e5630424..3147b10e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,5 @@ .kdev4/ -build/ -install/ -log/ + *~ *.kate-swp From e8ea2ce3f229bd3867dba70d8949d81a3406a979 Mon Sep 17 00:00:00 2001 From: bailaC Date: Wed, 6 Jul 2022 21:24:51 +0530 Subject: [PATCH 42/43] restoring minor merge errors --- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 -- ros2_control_demo_hardware/CMakeLists.txt | 2 +- .../external_rrbot_force_torque_sensor.hpp | 1 - ros2_control_demo_hardware/package.xml | 2 -- ros2_control_demo_hardware/ros2_control_demo_hardware.xml | 8 ++++---- .../src/rrbot_system_position_only.cpp | 1 - 6 files changed, 5 insertions(+), 11 deletions(-) diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro index 12a7930a..708ed015 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_external_sensor.urdf.xacro @@ -5,7 +5,6 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - @@ -18,7 +17,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 5073a773..cb68dd70 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -23,7 +23,7 @@ add_library( ${PROJECT_NAME} SHARED src/diffbot_system.cpp - #src/rrbot_system_position_only.cpp + src/rrbot_system_position_only.cpp src/rrbot_system_multi_interface.cpp src/rrbot_system_with_sensor.cpp src/rrbot_actuator.cpp diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp index 9f669876..ce3b6e6e 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/external_rrbot_force_torque_sensor.hpp @@ -45,7 +45,6 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor std::vector export_state_interfaces() override; ROS2_CONTROL_DEMO_HARDWARE_PUBLIC - hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; diff --git a/ros2_control_demo_hardware/package.xml b/ros2_control_demo_hardware/package.xml index 7bf41cd0..866c80ba 100644 --- a/ros2_control_demo_hardware/package.xml +++ b/ros2_control_demo_hardware/package.xml @@ -11,8 +11,6 @@ ament_cmake - lifecycle_msgs - hardware_interface pluginlib rclcpp diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index a41e1209..519d5445 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -20,11 +20,11 @@ The ros2_control DiffBot example using a system hardware interface-type. It uses velocity command and position state interface. The example is the starting point to implement a hardware interface for differential-drive mobile robots. - + - The ros2_control RRBot example using external Force Torque Sensor where it is separate from the RRBot. + 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. Date: Sat, 9 Jul 2022 09:04:35 +0200 Subject: [PATCH 43/43] Updated manual and some small fixes of the demo. --- README.md | 44 ++++++++++++++----- .../launch/three_robots.launch.py | 20 ++++----- ...ot_system_position_only.ros2_control.xacro | 6 +-- ...rbot_system_with_sensor.ros2_control.xacro | 2 +- .../threedofbot.ros2_control.xacro | 2 +- .../rrbot_system_position_only.urdf.xacro | 1 + 6 files changed, 49 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 5dfc2e3a..5bd7e25b 100644 --- a/README.md +++ b/README.md @@ -647,8 +647,12 @@ Notes: 1. Activate `RRBotWithSensor` and its position controller. Call ``` - ros2 service call /controller_manager/manage_hardware_activity controller_manager_msgs/srv/ManageHardwareActivity "{activate: [RRBotSystemWithSensor], deactivate: []}" - ros2 control switch_controllers --start rrbot_with_sensor_position_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemWithSensor + target_state: + id: 0 + label: active" + ros2 control switch_controllers --activate rrbot_with_sensor_position_controller ``` Scenario state: @@ -673,8 +677,12 @@ Notes: 1. Configure `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call ``` - ros2 service call /controller_manager/configure_hardware_component controller_manager_msgs/srv/ConfigureHardwareComponent "{name: FakeThreeDofBot}" - ros2 control switch_controllers --start threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: inactive" + ros2 control switch_controllers --activate threedofbot_joint_state_broadcaster threedofbot_pid_gain_controller ``` Scenario state: @@ -705,8 +713,8 @@ Notes: ``` Restart: ``` - ros2 control switch_controllers --stop joint_state_broadcaster - ros2 control switch_controllers --start joint_state_broadcaster + ros2 control switch_controllers --deactivate joint_state_broadcaster + ros2 control switch_controllers --activate joint_state_broadcaster ``` Check output to for comparison ``` @@ -720,8 +728,12 @@ Notes: 1. Activate `FakeThreeDofBot` and its joint state broadcaster and non-movement command interfaces. Call ``` - ros2 service call /controller_manager/manage_hardware_activity controller_manager_msgs/srv/ManageHardwareActivity "{activate: [FakeThreeDofBot], deactivate: []}" - ros2 control switch_controllers --start threedofbot_position_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: FakeThreeDofBot + target_state: + id: 0 + label: active" + ros2 control switch_controllers --activate threedofbot_position_controller ``` Scenario state: @@ -746,8 +758,12 @@ Notes: 1. Deactivate `RRBotSystemPositionOnly` and its position controller (first). Call ``` - ros2 control switch_controllers --stop rrbot_position_controller - ros2 service call /controller_manager/manage_hardware_activity controller_manager_msgs/srv/ManageHardwareActivity "{activate: [], deactivate: [RRBotSystemPositionOnly]}" + ros2 control switch_controllers --deactivate rrbot_position_controller + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: inactive" ``` Scenario state: @@ -773,7 +789,13 @@ Notes: 1. Cleanup `RRBotSystemPositionOnly` and its joint state broadcaster. Also restart global joint state broadcaster. Call ``` - ros2 control switch_controllers --stop rrbot_position_controller joint_state_broadcaster + ros2 control switch_controllers --deactivate rrbot_position_controller joint_state_broadcaster + ros2 service call /controller_manager/set_hardware_component_state controller_manager_msgs/srv/SetHardwareComponentState " + name: RRBotSystemPositionOnly + target_state: + id: 0 + label: unconfigured" + ros2 service call /controller_manager/cleanup_hardware_component controller_manager_msgs/srv/CleanupHardwareComponent "{name: RRBotSystemPositionOnly}" ros2 control switch_controllers --start joint_state_broadcaster ``` diff --git a/ros2_control_demo_bringup/launch/three_robots.launch.py b/ros2_control_demo_bringup/launch/three_robots.launch.py index 481cd5ac..108207d1 100644 --- a/ros2_control_demo_bringup/launch/three_robots.launch.py +++ b/ros2_control_demo_bringup/launch/three_robots.launch.py @@ -105,37 +105,37 @@ def generate_launch_description(): # Global joint state broadcaster joint_state_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) # RRBot controllers rrbot_joint_state_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["rrbot_joint_state_broadcaster", "-c", "/controller_manager"], ) rrbot_position_controller_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["rrbot_position_controller", "-c", "/controller_manager"], ) # External FTS broadcaster rrbot_external_fts_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["rrbot_external_fts_broadcaster", "-c", "/controller_manager"], ) # RRBot controllers rrbot_with_sensor_joint_state_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["rrbot_with_sensor_joint_state_broadcaster", "-c", "/controller_manager"], ) rrbot_with_sensor_position_controller_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=[ "rrbot_with_sensor_position_controller", "-c", @@ -145,14 +145,14 @@ def generate_launch_description(): ) rrbot_with_sensor_fts_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["rrbot_with_sensor_fts_broadcaster", "-c", "/controller_manager"], ) # ThreeDofBot controllers threedofbot_joint_state_broadcaster_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=[ "threedofbot_joint_state_broadcaster", "-c", @@ -162,12 +162,12 @@ def generate_launch_description(): ) threedofbot_position_controller_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["threedofbot_position_controller", "-c", "/controller_manager", "--stopped"], ) threedofbot_pid_gain_controller_spawner = Node( package="controller_manager", - executable="spawner.py", + executable="spawner", arguments=["threedofbot_pid_gain_controller", "-c", "/controller_manager", "--stopped"], ) diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro index c00cbd68..8bebd019 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro @@ -5,15 +5,15 @@ - + gazebo_ros2_control/GazeboSystem - + - fake_components/GenericSystem + mock_components/GenericSystem ${fake_sensor_commands} 0.0 diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro index 045143ad..357c1f19 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro @@ -7,7 +7,7 @@ - fake_components/GenericSystem + mock_components/GenericSystem ${fake_sensor_commands} 0.0 diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro index a0fef083..95c072a2 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/threedofbot.ros2_control.xacro @@ -5,7 +5,7 @@ - fake_components/GenericSystem + mock_components/GenericSystem diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_position_only.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_position_only.urdf.xacro index 2cce876e..d3aa8d9d 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_position_only.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_position_only.urdf.xacro @@ -39,6 +39,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc