From 0866c4260c96b0273360ac4777753cfc2da5109b Mon Sep 17 00:00:00 2001 From: bailaC Date: Thu, 18 Feb 2021 22:36:39 +0530 Subject: [PATCH 01/17] Adding demo for Example 3 : Industrial Robots with integrated sensor --- .../include/rrbot_system_with_sensor.hpp | 77 ++++++ .../src/rrbot_system_with_sensor.cpp | 237 ++++++++++++++++++ ...ot_forward_controller_position_sensor.yaml | 33 +++ ...rbot_system_with_sensor.ros2_control.xacro | 42 ++++ .../rrbot_system_with_sensor.urdf.xacro | 39 +++ .../launch/rrbot_system_with_sensor.launch.py | 52 ++++ 6 files changed, 480 insertions(+) create mode 100644 ros2_control_demo_hardware/include/rrbot_system_with_sensor.hpp create mode 100644 ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp create mode 100644 ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml create mode 100644 ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro create mode 100644 ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro create mode 100755 ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py diff --git a/ros2_control_demo_hardware/include/rrbot_system_with_sensor.hpp b/ros2_control_demo_hardware/include/rrbot_system_with_sensor.hpp new file mode 100644 index 000000000..4c76b905a --- /dev/null +++ b/ros2_control_demo_hardware/include/rrbot_system_with_sensor.hpp @@ -0,0 +1,77 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ +#define ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ + +#include +#include +#include + +#include "rclcpp/macros.hpp" + +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "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_read_sec_; + double hw_write_sec_; + + // Store the command for the simulated robot + std::vector hw_joint_commands_; + std::vector hw_joint_states_; + std::vector hw_sensor_states_; +}; + +} // namespace ros2_control_demo_hardware + +#endif // ROS2_CONTROL_DEMO_HARDWARE__RRBOT_SYSTEM_WITH_SENSOR_HPP_ diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp new file mode 100644 index 000000000..775c2de21 --- /dev/null +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -0,0 +1,237 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_control_demo_hardware/rrbot_system_with_sensor.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace ros2_control_demo_hardware +{ + +return_type RRBotSystemWithSensorHardware::configure( + const hardware_interface::HardwareInfo & info) +{ + if (configure_default(info) != return_type::OK) { + return return_type::ERROR; + } + + hw_read_sec_ = stod(info_.hardware_parameters["example_param_read_for_sec"]); + hw_write_sec_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); + hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_sensor_states_.resize(info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + // RRBotSystemWithSensor has exactly one state and command interface on each joint + if (joint.command_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "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("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 return_type::ERROR; + } + + if (joint.state_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "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("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 return_type::ERROR; + } + } + + // RRBotSystemWithSensor has six state interfaces for one sensor + if (info_.sensors[0].state_interfaces.size() != 6) { + RCLCPP_FATAL( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Sensor '%s' has %d state interface. 6 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("RRBotSystemWithSensorHardware"), + "Starting ...please wait..."); + + // 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("RRBotSystemWithSensorHardware"), + "System Sucessfully started!"); + + return return_type::OK; +} + +return_type RRBotSystemWithSensorHardware::stop() +{ + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Stopping ...please wait..."); + + status_ = hardware_interface::status::STOPPED; + + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "System sucessfully stopped!"); + + return return_type::OK; +} + +hardware_interface::return_type RRBotSystemWithSensorHardware::read() +{ + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Reading...please wait..."); + for (int i = 0; i <= hw_read_sec_; i++) { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "%.1f seconds left...", hw_read_sec_ - i); + } + + 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]) / 2.0; + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Got state %.5f for joint %d!", hw_joint_states_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Joints sucessfully read!"); + + // Simulate RRBot's sensor data + hw_sensor_states_[0] = 1.1; + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Got state %.5f for sensor 0!", hw_sensor_states_[0]); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Sensors sucessfully read!"); + + return return_type::OK; +} + +hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemWithSensorHardware::write() +{ + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Writing...please wait..."); + + for (int i = 0; i <= hw_write_sec_; i++) { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "%.1f seconds left...", hw_write_sec_ - i); + } + + 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 %d!", hw_joint_commands_[i], i); + } + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Joints sucessfully written!"); + + return return_type::OK; +} + +} // namespace ros2_control_demo_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_demo_hardware::RRBotSystemWithSensorHardware, + hardware_interface::SystemInterface +) diff --git a/ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml b/ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml new file mode 100644 index 000000000..08d2a0f17 --- /dev/null +++ b/ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml @@ -0,0 +1,33 @@ +controller_manager: + ros__parameters: + update_rate: 2 # Hz + + forward_command_controller_position: + type: forward_command_controller/ForwardCommandController + + joint_state_controller: + type: joint_state_controller/JointStateController + + ur_controllers: + type: controller_plugins/ForceTorqueStateController + +forward_command_controller_position: + ros__parameters: + joints: + - joint1 + - joint2 + interface_name: position + +ur_controllers: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - fx + - fy + - fz + - tx + - ty + - tz + frame_id: kuka_tcp + lower_limits: -100 + upper_limits: 100 diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro new file mode 100644 index 000000000..14ec90873 --- /dev/null +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro @@ -0,0 +1,42 @@ + + + + + + + + ros2_control_demo_hardware/RRBotSystemWithSensorHardware + 2 + 2 + + + + -1 + 1 + + + + + + -1 + 1 + + + + + + + + + + + kuka_tcp + -100 + 100 + + + + + + + diff --git a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro new file mode 100644 index 000000000..0398005a0 --- /dev/null +++ b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + diff --git a/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py new file mode 100755 index 000000000..29f1c2ce7 --- /dev/null +++ b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py @@ -0,0 +1,52 @@ +# Copyright 2020 ROS2-Control Development Team (2020) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_path = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'description', + 'rrbot_system_with_sensor.urdf.xacro') + robot_description_config = xacro.process_file(robot_description_path) + robot_description = {'robot_description': robot_description_config.toxml()} + + rrbot_forward_controller = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'controllers', + 'rrbot_forward_controller_position_sensor.yaml' + ) + + return LaunchDescription([ + Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, rrbot_forward_controller], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + ) + + ]) From b22dd4dd763401777dc4ac964efd5ac0ee0fbdf2 Mon Sep 17 00:00:00 2001 From: bailaC Date: Thu, 18 Feb 2021 22:37:20 +0530 Subject: [PATCH 02/17] Adding demo for Example 3 : Industrial Robots with integrated sensor --- ros2_control_demo_hardware/CMakeLists.txt | 1 + ros2_control_demo_hardware/ros2_control_demo_hardware.xml | 7 +++++++ 2 files changed, 8 insertions(+) diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 831285099..6b55a87e7 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -21,6 +21,7 @@ add_library( ${PROJECT_NAME} SHARED src/rrbot_system_position_only.cpp + src/rrbot_system_with_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 22a220569..02c346801 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -6,4 +6,11 @@ The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. + + + The ROS2 Control robot with sensor protocol. This is example for start porting the robot from ROS 1. + + From ba8699ead56ef94a0e16155e5206cc514d4e8d18 Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 19 Feb 2021 19:26:17 +0530 Subject: [PATCH 03/17] Correcting directory --- .../{ => ros2_control_demo_hardware}/rrbot_system_with_sensor.hpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename ros2_control_demo_hardware/include/{ => ros2_control_demo_hardware}/rrbot_system_with_sensor.hpp (100%) diff --git a/ros2_control_demo_hardware/include/rrbot_system_with_sensor.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_sensor.hpp similarity index 100% rename from ros2_control_demo_hardware/include/rrbot_system_with_sensor.hpp rename to ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_with_sensor.hpp From 63294eb19a9f1e88e46492ca1377f84759762bd4 Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 19 Feb 2021 19:32:20 +0530 Subject: [PATCH 04/17] cpplint fixes --- ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 775c2de21..b9d8aaa2c 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -37,7 +37,8 @@ return_type RRBotSystemWithSensorHardware::configure( hw_write_sec_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_sensor_states_.resize(info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); + 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 @@ -179,7 +180,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() 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_joint_states_[i] - hw_joint_commands_[i]) / 2.0; RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), From 6310687e4fd898ce32a39d9cf4d324528e5068db Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 19 Feb 2021 19:34:56 +0530 Subject: [PATCH 05/17] cpplint fix --- ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b9d8aaa2c..c2a32b108 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -37,7 +37,7 @@ return_type RRBotSystemWithSensorHardware::configure( hw_write_sec_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_sensor_states_.resize(info_.sensors[0].state_interfaces.size(), + hw_sensor_states_.resize(info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { From a6f205283faf522aae65f06b4c5ab3d29500c6dc Mon Sep 17 00:00:00 2001 From: bailaC Date: Fri, 19 Feb 2021 19:56:55 +0530 Subject: [PATCH 06/17] uncrustify fixes --- .../src/rrbot_system_with_sensor.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) 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 c2a32b108..0ac9e9b5a 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -37,8 +37,9 @@ return_type RRBotSystemWithSensorHardware::configure( hw_write_sec_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); hw_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_sensor_states_.resize(info_.sensors[0].state_interfaces.size(), - std::numeric_limits::quiet_NaN()); + 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 @@ -103,8 +104,8 @@ RRBotSystemWithSensorHardware::export_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])); + hardware_interface::StateInterface( + info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &hw_sensor_states_[i])); } return state_interfaces; @@ -181,7 +182,7 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() 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]) / 2.0; + (hw_joint_states_[i] - hw_joint_commands_[i]) / 2.0; RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Got state %.5f for joint %d!", hw_joint_states_[i], i); @@ -193,8 +194,8 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() // Simulate RRBot's sensor data hw_sensor_states_[0] = 1.1; RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "Got state %.5f for sensor 0!", hw_sensor_states_[0]); + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "Got state %.5f for sensor 0!", hw_sensor_states_[0]); RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Sensors sucessfully read!"); From 76d8a4f530c8c40093e04a10f121250498d27282 Mon Sep 17 00:00:00 2001 From: bailaC Date: Mon, 22 Feb 2021 18:38:24 +0530 Subject: [PATCH 07/17] Addressing review comments --- .../rrbot_system_with_sensor.hpp | 8 ++-- .../ros2_control_demo_hardware.xml | 4 +- .../src/rrbot_system_with_sensor.cpp | 37 ++++++++++--------- ...aml => rrbot_with_sensor_controllers.yaml} | 8 +--- ...rbot_system_with_sensor.ros2_control.xacro | 14 +++---- .../rrbot_system_with_sensor.urdf.xacro | 2 +- .../launch/rrbot_system_with_sensor.launch.py | 4 +- 7 files changed, 37 insertions(+), 40 deletions(-) rename ros2_control_demo_robot/controllers/{rrbot_forward_controller_position_sensor.yaml => rrbot_with_sensor_controllers.yaml} (81%) 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 4c76b905a..4d78928c9 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 @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,7 +20,6 @@ #include #include -#include "rclcpp/macros.hpp" #include "hardware_interface/base_interface.hpp" #include "hardware_interface/system_interface.hpp" @@ -28,6 +27,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "rclcpp/macros.hpp" #include "ros2_control_demo_hardware/visibility_control.h" using hardware_interface::return_type; @@ -63,8 +63,8 @@ class RRBotSystemWithSensorHardware : public private: // Parameters for the RRBot simulation - double hw_read_sec_; - double hw_write_sec_; + double hw_start_sec_; + double hw_stop_sec_; // Store the command for the simulated robot std::vector hw_joint_commands_; diff --git a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml index 02c346801..0f7a770e2 100644 --- a/ros2_control_demo_hardware/ros2_control_demo_hardware.xml +++ b/ros2_control_demo_hardware/ros2_control_demo_hardware.xml @@ -3,14 +3,14 @@ type="ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware" base_class_type="hardware_interface::SystemInterface"> - The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. + The ros2_control RRbot example using a system hardware interface-type and only one command and state interface. This example is the starting point to implement a hardware interface for robots providing one communication path for all its joints. - The ROS2 Control robot with sensor protocol. This is example for start porting the robot from ROS 1. + The ros2_control RRbot example using a system hardware interface-type and only one command and state interface. This example is the starting point to implement a hardware interface for robots with sensors providing one communication path. 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 0ac9e9b5a..9dfb86d01 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 ros2_control Development Team +// Copyright 2021 ros2_control Development Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,8 +33,8 @@ return_type RRBotSystemWithSensorHardware::configure( return return_type::ERROR; } - hw_read_sec_ = stod(info_.hardware_parameters["example_param_read_for_sec"]); - hw_write_sec_ = stod(info_.hardware_parameters["example_param_write_for_sec"]); + 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_joint_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_joint_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_sensor_states_.resize( @@ -79,10 +79,10 @@ return_type RRBotSystemWithSensorHardware::configure( } // RRBotSystemWithSensor has six state interfaces for one sensor - if (info_.sensors[0].state_interfaces.size() != 6) { + if (info_.sensors[0].state_interfaces.size() != 2) { RCLCPP_FATAL( rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "Sensor '%s' has %d state interface. 6 expected.", + "Sensor '%s' has %d state interface. 2 expected.", info_.sensors[0].name.c_str(), info_.sensors[0].state_interfaces.size()); return return_type::ERROR; } @@ -130,6 +130,13 @@ return_type RRBotSystemWithSensorHardware::start() rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Starting ...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); + } + // set some default values for joints for (uint i = 0; i < hw_joint_states_.size(); i++) { if (std::isnan(hw_joint_states_[i])) { @@ -158,6 +165,13 @@ return_type RRBotSystemWithSensorHardware::stop() rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Stopping ...please wait..."); + for (int i = 0; i <= hw_stop_sec_; i++) { + rclcpp::sleep_for(std::chrono::seconds(1)); + RCLCPP_INFO( + rclcpp::get_logger("RRBotSystemWithSensorHardware"), + "%.1f seconds left...", hw_stop_sec_ - i); + } + status_ = hardware_interface::status::STOPPED; RCLCPP_INFO( @@ -172,12 +186,6 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() RCLCPP_INFO( rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Reading...please wait..."); - for (int i = 0; i <= hw_read_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "%.1f seconds left...", hw_read_sec_ - i); - } for (uint i = 0; i < hw_joint_states_.size(); i++) { // Simulate RRBot's movement @@ -209,13 +217,6 @@ hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemWithSenso rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Writing...please wait..."); - for (int i = 0; i <= hw_write_sec_; i++) { - rclcpp::sleep_for(std::chrono::seconds(1)); - RCLCPP_INFO( - rclcpp::get_logger("RRBotSystemWithSensorHardware"), - "%.1f seconds left...", hw_write_sec_ - i); - } - for (uint i = 0; i < hw_joint_commands_.size(); i++) { // Simulate sending commands to the hardware RCLCPP_INFO( diff --git a/ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml b/ros2_control_demo_robot/controllers/rrbot_with_sensor_controllers.yaml similarity index 81% rename from ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml rename to ros2_control_demo_robot/controllers/rrbot_with_sensor_controllers.yaml index 08d2a0f17..b90c235c9 100644 --- a/ros2_control_demo_robot/controllers/rrbot_forward_controller_position_sensor.yaml +++ b/ros2_control_demo_robot/controllers/rrbot_with_sensor_controllers.yaml @@ -9,7 +9,7 @@ controller_manager: type: joint_state_controller/JointStateController ur_controllers: - type: controller_plugins/ForceTorqueStateController + type: ur_controllers/ForceTorqueStateController forward_command_controller_position: ros__parameters: @@ -23,11 +23,7 @@ ur_controllers: sensor_name: tcp_fts_sensor state_interface_names: - fx - - fy - - fz - - tx - - ty - tz - frame_id: kuka_tcp + frame_id: rrbot_tcp lower_limits: -100 upper_limits: 100 diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro index 14ec90873..5f358a062 100644 --- a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro @@ -8,6 +8,10 @@ ros2_control_demo_hardware/RRBotSystemWithSensorHardware 2 2 + 2.0 + 3.0 + 2.0 + 5.0 @@ -25,14 +29,10 @@ - - - - - kuka_tcp - -100 - 100 + rrbot_tcp + 100 + 15 diff --git a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro index 0398005a0..5dc3bb86a 100644 --- a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro +++ b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro @@ -1,5 +1,5 @@ - + - - + @@ -68,7 +67,7 @@ - + diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro index a97ecdeeb..92f48aae7 100644 --- a/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_position_only.ros2_control.xacro @@ -1,14 +1,14 @@ - + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware 2.0 3.0 - 2.0 + ${slowdown} diff --git a/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro index c25494ab7..2ba654c3b 100644 --- a/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro +++ b/ros2_control_demo_robot/description/rrbot_system_position_only.urdf.xacro @@ -6,21 +6,20 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc --> + + + - - + - - + - - + - - + @@ -34,6 +33,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + diff --git a/ros2_control_demo_robot/launch/rrbot_system_position_only.launch.py b/ros2_control_demo_robot/launch/rrbot_system_position_only.launch.py index f98a855a8..da1d83ea9 100755 --- a/ros2_control_demo_robot/launch/rrbot_system_position_only.launch.py +++ b/ros2_control_demo_robot/launch/rrbot_system_position_only.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS2-Control Development Team (2020) +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -29,24 +29,46 @@ def generate_launch_description(): get_package_share_directory('ros2_control_demo_robot'), 'description', 'rrbot_system_position_only.urdf.xacro') - robot_description_config = xacro.process_file(robot_description_path) + robot_description_config = xacro.process_file(robot_description_path, + mappings={'slowdown': '3.0'}) robot_description = {'robot_description': robot_description_config.toxml()} rrbot_forward_controller = os.path.join( get_package_share_directory('ros2_control_demo_robot'), - 'controllers', - 'rrbot_forward_controller_position.yaml' + 'config', + 'rrbot_controllers.yaml' + ) + rviz_config_file = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'rviz', + 'rrbot.rviz' ) - return LaunchDescription([ - Node( - package='controller_manager', - executable='ros2_control_node', - parameters=[robot_description, rrbot_forward_controller], - output={ + control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, rrbot_forward_controller], + output={ 'stdout': 'screen', 'stderr': 'screen', - }, - ) + }, + ) + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description] + ) + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + ) + return LaunchDescription([ + control_node, + robot_state_publisher_node, + rviz_node, ]) diff --git a/ros2_control_demo_robot/launch/test_rrbot_description.launch.py b/ros2_control_demo_robot/launch/test_rrbot_description.launch.py new file mode 100644 index 000000000..c9b56c68a --- /dev/null +++ b/ros2_control_demo_robot/launch/test_rrbot_description.launch.py @@ -0,0 +1,63 @@ +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_path = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'description', + 'rrbot_system_position_only.urdf.xacro') + robot_description_config = xacro.process_file(robot_description_path) + robot_description = {'robot_description': robot_description_config.toxml()} + + rviz_config_file = os.path.join( + get_package_share_directory('ros2_control_demo_robot'), + 'rviz', + 'rrbot.rviz' + ) + + joint_state_publisher_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + ) + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description] + ) + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + ) + + return LaunchDescription([ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ]) diff --git a/ros2_control_demo_robot/package.xml b/ros2_control_demo_robot/package.xml index b0c761395..96855e6de 100644 --- a/ros2_control_demo_robot/package.xml +++ b/ros2_control_demo_robot/package.xml @@ -11,11 +11,10 @@ ament_cmake - hardware_interface - pluginlib - rclcpp - - ros2_control_demo_hardware + joint_state_publisher_gui + robot_state_publisher + ros2_control_demo_hardware + rviz2 ament_cmake_gtest diff --git a/ros2_control_demo_robot/rviz/rrbot.rviz b/ros2_control_demo_robot/rviz/rrbot.rviz new file mode 100644 index 000000000..a45fc6002 --- /dev/null +++ b/ros2_control_demo_robot/rviz/rrbot.rviz @@ -0,0 +1,184 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 780 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link_optical: + Alpha: 1 + Show Axes: false + Show Trail: false + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + 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: 3.5202667713165283 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.09957491606473923 + Y: -0.4662817120552063 + Z: 1.2063069343566895 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.325399249792099 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.675412654876709 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1051 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000003b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000003b0000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000044000003b0000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000504000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 5120 + Y: 180 From fbb840c2afe8f87f00ae101de91dacb822ece877 Mon Sep 17 00:00:00 2001 From: bailaC Date: Sun, 18 Apr 2021 15:15:36 +0530 Subject: [PATCH 15/17] Adding changes for SR_issue_2 --- .../config/rrbot_with_sensor_controllers.yaml | 6 +- ...rbot_system_with_sensor.ros2_control.xacro | 20 +++-- .../rrbot_system_with_sensor.urdf.xacro | 13 ++- .../launch/rrbot_system_with_sensor.launch.py | 83 +++++++++++-------- 4 files changed, 77 insertions(+), 45 deletions(-) diff --git a/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml b/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml index 541ff34b8..f3642c043 100644 --- a/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml +++ b/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml @@ -2,16 +2,16 @@ controller_manager: ros__parameters: update_rate: 2 # Hz - forward_command_controller_position: + forward_position_controller: type: forward_command_controller/ForwardCommandController fts_controllers: - type: force_torque_sensor_controller/ForceTorqueSensorController + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster joint_state_controller: type: joint_state_controller/JointStateController -forward_command_controller_position: +forward_position_controller: ros__parameters: joints: - joint1 diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro index a94f245a0..c9579e98f 100644 --- a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro @@ -1,15 +1,23 @@ - + - ros2_control_demo_hardware/RRBotSystemWithSensorHardware - 2.0 - 3.0 - ${slowdown} - 5.0 + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/RRBotSystemWithSensorHardware + 2.0 + 3.0 + ${slowdown} + 5.0 + diff --git a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro index 714048f4d..08e738937 100644 --- a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro +++ b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro @@ -1,12 +1,15 @@ + + + @@ -27,12 +30,16 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc true - + - + diff --git a/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py index c1f4f711d..6321c1052 100755 --- a/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py +++ b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py @@ -1,52 +1,69 @@ -# Copyright 2021 ROS2-Control Development Team +# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) # -# Licensed under the Apache License, Version 2.0 (the "License"); +# 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, +# distributed under the License is distributed on an 'AS IS' BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -import os - -from ament_index_python.packages import get_package_share_directory +# +# Authors: Subhas Das, Denis Stogl +# from launch import LaunchDescription -from launch_ros.actions import Node - -import xacro +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append(DeclareLaunchArgument( + 'prefix', default_value='""', description='Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers\' configuration \ + have to be updated.')) + declared_arguments.append(DeclareLaunchArgument( + 'controllers_file', default_value='rrbot_with_sensor_controllers.yaml', + description='YAML file with the controllers configuration.')) + 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.')) + declared_arguments.append(DeclareLaunchArgument( + 'robot_controller', default_value='forward_position_controller', + description='Robot controller to start.')) - # Get URDF via xacro - robot_description_path = os.path.join( - get_package_share_directory('ros2_control_demo_robot'), - 'description', - 'rrbot_system_with_sensor.urdf.xacro') - robot_description_config = xacro.process_file(robot_description_path) - robot_description = {'robot_description': robot_description_config.toxml()} - - rrbot_forward_controller = os.path.join( - get_package_share_directory('ros2_control_demo_robot'), - 'config', - 'rrbot_with_sensor_controllers.yaml' - ) + # Initialize Arguments + prefix = LaunchConfiguration('prefix') + use_fake_hardware = LaunchConfiguration('use_fake_hardware') + fake_sensor_commands = LaunchConfiguration('fake_sensor_commands') + slowdown = LaunchConfiguration('slowdown') + robot_controller = LaunchConfiguration('robot_controller') - return LaunchDescription([ - Node( - package='controller_manager', - executable='ros2_control_node', - parameters=[robot_description, rrbot_forward_controller], - output={ - 'stdout': 'screen', - 'stderr': 'screen', - }, - ) + base_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rrbot.launch.py']), + launch_arguments={'description_file': 'rrbot_with_sensor_controllers.urdf.xacro', + 'prefix': prefix, + 'use_fake_hardware': use_fake_hardware, + 'fake_sensor_commands': fake_sensor_commands, + 'slowdown': slowdown, + 'robot_controller': robot_controller, + }.items()) - ]) + return LaunchDescription( + declared_arguments + + [ + base_launch, + ]) From 263377c77f1411e4347e564682f8258321f5982e Mon Sep 17 00:00:00 2001 From: bailaC Date: Sun, 18 Apr 2021 15:30:03 +0530 Subject: [PATCH 16/17] Q003 error fix --- .../launch/rrbot_system_with_sensor.launch.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py index 6321c1052..ff84e68f8 100755 --- a/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py +++ b/ros2_control_demo_robot/launch/rrbot_system_with_sensor.launch.py @@ -26,9 +26,9 @@ 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.')) + '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( 'controllers_file', default_value='rrbot_with_sensor_controllers.yaml', description='YAML file with the controllers configuration.')) @@ -37,8 +37,8 @@ def generate_launch_description(): 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.')) + 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.')) declared_arguments.append(DeclareLaunchArgument( From 60cd85a73618b9b425a9a5a07e3c9c79897e5cfa Mon Sep 17 00:00:00 2001 From: bailaC Date: Sat, 12 Jun 2021 10:57:01 +0530 Subject: [PATCH 17/17] Example-3 : Adapting changes from PR-76 --- ros2_control_demo_hardware/CMakeLists.txt | 1 + .../src/rrbot_system_with_sensor.cpp | 8 +- .../config/rrbot_with_sensor_controllers.yaml | 14 ++- ...rbot_system_with_sensor.ros2_control.xacro | 49 ++++++---- .../rrbot_system_with_sensor.urdf.xacro | 10 +- .../launch/rrbot_system_with_sensor.launch.py | 93 +++++++++++-------- 6 files changed, 102 insertions(+), 73 deletions(-) diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index 0caa8cddd..fd28caab7 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -24,6 +24,7 @@ add_library( src/rrbot_system_position_only.cpp src/rrbot_system_with_sensor.cpp src/rrbot_system_multi_interface.cpp + src/rrbot_system_with_sensor.cpp ) target_include_directories( ${PROJECT_NAME} diff --git a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp index 11817aacb..b467dc7cb 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -1,5 +1,5 @@ -// Copyright 2021 ros2_control Development Team -// +// 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 @@ -12,6 +12,10 @@ // 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_sensor.hpp" #include diff --git a/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml b/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml index f3642c043..d680cf6dc 100644 --- a/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml +++ b/ros2_control_demo_robot/config/rrbot_with_sensor_controllers.yaml @@ -5,11 +5,11 @@ controller_manager: forward_position_controller: type: forward_command_controller/ForwardCommandController - fts_controllers: + fts_controller: type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster - joint_state_controller: - type: joint_state_controller/JointStateController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster forward_position_controller: ros__parameters: @@ -18,10 +18,8 @@ forward_position_controller: - joint2 interface_name: position -fts_controllers: +fts_controller: ros__parameters: - sensor_name: tcp_fts_sensor - interface_names: - - fx - - tz + interface_names.force.x: tcp_fts_sensor/mysensor_force.x + interface_names.torque.z: tcp_fts_sensor/mysensor_torque.z frame_id: rrbot_tcp diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro index c9579e98f..633e44b80 100644 --- a/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro +++ b/ros2_control_demo_robot/description/rrbot/rrbot_system_with_sensor.ros2_control.xacro @@ -1,24 +1,32 @@ - + - - - fake_components/GenericSystem - ${fake_sensor_commands} - 0.0 - - - ros2_control_demo_hardware/RRBotSystemWithSensorHardware - 2.0 - 3.0 - ${slowdown} - 5.0 - - + + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + ros2_control_demo_hardware/RRBotSystemWithSensorHardware + 2.0 + 3.0 + ${slowdown} + 5.0 + + + + -1 @@ -34,8 +42,12 @@ - - + + + + + + rrbot_tcp 100 15 @@ -45,4 +57,3 @@ - diff --git a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro index 08e738937..e5d7235b4 100644 --- a/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro +++ b/ros2_control_demo_robot/description/rrbot_system_with_sensor.urdf.xacro @@ -13,16 +13,16 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + - + - + - + @@ -34,7 +34,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - +