From 07e8d416624215602d58bbad9064d761cc145d09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 30 Mar 2022 10:31:57 +0200 Subject: [PATCH 01/25] Fix Python black pre-commit hook (#164) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index afead9e0c..e932ab654 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.1.0 + rev: 22.3.0 hooks: - id: black args: ["--line-length=99"] From 96ef3b5e28802189aa0c7ac54c36098aa581d037 Mon Sep 17 00:00:00 2001 From: mamueluth Date: Wed, 6 Apr 2022 11:57:59 +0200 Subject: [PATCH 02/25] Changed JointStateController to JointStateBroadcaster in instructions (#168) Changed JointStateController to JointStateBroadcaster in instructions --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 1ba00e45f..43f3d4ed5 100644 --- a/README.md +++ b/README.md @@ -517,7 +517,7 @@ Commanding the robot: see the commands below. ## Controllers and moving hardware To move the robot you should load and start controllers. -The `JointStateController` is used to publish the joint states to ROS topics. +The `JointStateBroadcaster` is used to publish the joint states to ROS topics. Direct joint commands are sent to this robot via the `ForwardCommandController` and `JointTrajectoryController`. The sections below describe their usage. Check the [Results](##result) section on how to ensure that things went well. @@ -528,11 +528,11 @@ ros2 control list_controllers ``` -### JointStateController +### JointStateBroadcaster -Open another terminal and load, configure and start `joint_state_controller`: +Open another terminal and load, configure and start `joint_state_broadcaster`: ``` -ros2 control set_controller_state joint_state_controller start +ros2 control set_controller_state joint_state_broadcaster start ``` Check if controller is loaded properly: ``` @@ -540,7 +540,7 @@ ros2 control list_controllers ``` You should get the response: ``` -joint_state_controller[joint_state_controller/JointStateController] active +joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active ``` Now you should also see the *RRbot* represented correctly in `RViz`. @@ -580,7 +580,7 @@ Now you should also see the *RRbot* represented correctly in `RViz`. ``` You should get `active` in the response: ``` - joint_state_controller[joint_state_controller/JointStateController] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active forward_position_controller[forward_command_controller/ForwardCommandController] active ``` @@ -623,13 +623,13 @@ Now you should also see the *RRbot* represented correctly in `RViz`. ``` You should get `active` in the response: ``` - joint_state_controller[joint_state_controller/JointStateController] active + joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active position_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active ``` -3. Send a command to the controller using demo node which sends two goals every 5 seconds in a loop: +3. Send a command to the controller using demo node which sends four goals every 6 seconds in a loop: ``` - ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py + ros2 launch ros2_control_demo_bringup test_joint_trajectory_controller.launch.py ``` You can adjust the goals in [rrbot_joint_trajectory_publisher.yaml](ros2_control_demo_bringup/config/rrbot_joint_trajectory_publisher.yaml). From c02942a892dde71f9e2df481b8007ef7f5e173fb Mon Sep 17 00:00:00 2001 From: Kutlay Acar <98892946+kutlayacar@users.noreply.github.com> Date: Thu, 21 Apr 2022 10:11:53 +0200 Subject: [PATCH 03/25] CallbackReturn Fix (#173) --- .../diffbot_system.hpp | 11 ++++--- .../external_rrbot_force_torque_sensor.hpp | 11 ++++--- .../rrbot_actuator.hpp | 11 ++++--- .../rrbot_system_multi_interface.hpp | 11 ++++--- .../rrbot_system_position_only.hpp | 14 +++++---- .../rrbot_system_with_sensor.hpp | 14 +++++---- .../src/diffbot_system.cpp | 29 +++++++++-------- .../external_rrbot_force_torque_sensor.cpp | 18 ++++++----- .../src/rrbot_actuator.cpp | 29 ++++++++++------- .../src/rrbot_system_multi_interface.cpp | 26 +++++++++------- .../src/rrbot_system_position_only.cpp | 30 +++++++++--------- .../src/rrbot_system_with_sensor.cpp | 31 ++++++++++--------- 12 files changed, 130 insertions(+), 105 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 6d372438c..85d2b810a 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 @@ -33,15 +33,14 @@ namespace ros2_control_demo_hardware { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class DiffBotSystemHardware : public hardware_interface::SystemInterface { public: 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; @@ -50,10 +49,12 @@ 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; 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 0ba4f1051..79338443e 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,24 +32,25 @@ namespace ros2_control_demo_hardware { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::SensorInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(ExternalRRBotForceTorqueSensorHardware); 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; 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; diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp index 6eb4a4f96..198b033da 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp @@ -33,15 +33,14 @@ namespace ros2_control_demo_hardware { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class RRBotModularJoint : public hardware_interface::ActuatorInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotModularJoint); 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,10 +49,12 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface 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; 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 fc57939c9..a9ebf7d64 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 @@ -35,15 +35,14 @@ namespace ros2_control_demo_hardware { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInterface { public: 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; @@ -57,10 +56,12 @@ 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; 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 7dee1a62f..9e563a2be 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 @@ -30,18 +30,18 @@ namespace ros2_control_demo_hardware { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); 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; @@ -50,10 +50,12 @@ 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; + 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; 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 e7168e561..c0dfdfaf9 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 @@ -33,18 +33,18 @@ namespace ros2_control_demo_hardware { -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface { public: 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; @@ -53,10 +53,12 @@ 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; diff --git a/ros2_control_demo_hardware/src/diffbot_system.cpp b/ros2_control_demo_hardware/src/diffbot_system.cpp index d8415f4aa..b8005c45a 100644 --- a/ros2_control_demo_hardware/src/diffbot_system.cpp +++ b/ros2_control_demo_hardware/src/diffbot_system.cpp @@ -26,11 +26,14 @@ 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; @@ -54,7 +57,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) @@ -63,7 +66,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) @@ -72,7 +75,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) @@ -81,7 +84,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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) @@ -90,13 +93,13 @@ 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; } } clock_ = rclcpp::Clock(); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector DiffBotSystemHardware::export_state_interfaces() @@ -125,7 +128,7 @@ std::vector DiffBotSystemHardware::export_ return command_interfaces; } -CallbackReturn DiffBotSystemHardware::on_activate( +hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -154,10 +157,10 @@ CallbackReturn DiffBotSystemHardware::on_activate( 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*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -173,7 +176,7 @@ CallbackReturn DiffBotSystemHardware::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully deactivated!"); - return CallbackReturn::SUCCESS; + return hardware_interface::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 56c5d98f1..64151aac8 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,12 +29,14 @@ namespace ros2_control_demo_hardware { -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( +hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( const hardware_interface::HardwareInfo & info) { - if (hardware_interface::SensorInterface::on_init(info) != CallbackReturn::SUCCESS) + if ( + hardware_interface::SensorInterface::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 @@ -46,7 +48,7 @@ CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_init( hw_sensor_states_.resize( info_.sensors[0].state_interfaces.size(), std::numeric_limits::quiet_NaN()); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector @@ -64,7 +66,7 @@ ExternalRRBotForceTorqueSensorHardware::export_state_interfaces() return state_interfaces; } -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( +hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -83,10 +85,10 @@ CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_activate( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "Successfully activated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate( +hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -105,7 +107,7 @@ CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_deactivate( rclcpp::get_logger("ExternalRRBotForceTorqueSensorHardware"), "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 ExternalRRBotForceTorqueSensorHardware::read() diff --git a/ros2_control_demo_hardware/src/rrbot_actuator.cpp b/ros2_control_demo_hardware/src/rrbot_actuator.cpp index 3a3e0b0df..7eff53718 100644 --- a/ros2_control_demo_hardware/src/rrbot_actuator.cpp +++ b/ros2_control_demo_hardware/src/rrbot_actuator.cpp @@ -30,11 +30,14 @@ namespace ros2_control_demo_hardware { -CallbackReturn RRBotModularJoint::on_init(const hardware_interface::HardwareInfo & info) +hardware_interface::CallbackReturn RRBotModularJoint::on_init( + const hardware_interface::HardwareInfo & info) { - if (hardware_interface::ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) + if ( + hardware_interface::ActuatorInterface::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"]); @@ -53,7 +56,7 @@ CallbackReturn RRBotModularJoint::on_init(const hardware_interface::HardwareInfo rclcpp::get_logger("RRBotModularJoint"), "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) @@ -62,7 +65,7 @@ CallbackReturn RRBotModularJoint::on_init(const hardware_interface::HardwareInfo rclcpp::get_logger("RRBotModularJoint"), "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) @@ -70,7 +73,7 @@ CallbackReturn RRBotModularJoint::on_init(const hardware_interface::HardwareInfo RCLCPP_FATAL( rclcpp::get_logger("RRBotModularJoint"), "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) @@ -79,10 +82,10 @@ CallbackReturn RRBotModularJoint::on_init(const hardware_interface::HardwareInfo rclcpp::get_logger("RRBotModularJoint"), "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; } std::vector RRBotModularJoint::export_state_interfaces() @@ -105,7 +108,8 @@ std::vector RRBotModularJoint::export_comm return command_interfaces; } -CallbackReturn RRBotModularJoint::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn RRBotModularJoint::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Activating ...please wait..."); @@ -126,10 +130,11 @@ CallbackReturn RRBotModularJoint::on_activate(const rclcpp_lifecycle::State & /* RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Successfully activated!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn RRBotModularJoint::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn RRBotModularJoint::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "Deactivating ...please wait..."); @@ -143,7 +148,7 @@ CallbackReturn RRBotModularJoint::on_deactivate(const rclcpp_lifecycle::State & RCLCPP_INFO(rclcpp::get_logger("RRBotModularJoint"), "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 RRBotModularJoint::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 92760d369..59d02030e 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp @@ -27,12 +27,14 @@ 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; } // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -58,7 +60,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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (!(joint.command_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -70,7 +72,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) @@ -79,7 +81,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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (!(joint.state_interfaces[0].name == hardware_interface::HW_IF_POSITION || @@ -91,13 +93,13 @@ 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; } } clock_ = rclcpp::Clock(); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector @@ -198,7 +200,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*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -249,10 +251,10 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_activate( RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully activated! %u", 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*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -270,7 +272,7 @@ CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "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 RRBotSystemMultiInterfaceHardware::read() 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 f5d21d60f..044e5fc8f 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,14 @@ namespace ros2_control_demo_hardware { -CallbackReturn RRBotSystemPositionOnlyHardware::on_init( +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::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 @@ -50,7 +52,7 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_init( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "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) @@ -59,7 +61,7 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_init( 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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } if (joint.state_interfaces.size() != 1) @@ -68,7 +70,7 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_init( rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "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) @@ -77,14 +79,14 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_init( 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 CallbackReturn::ERROR; + return hardware_interface::CallbackReturn::ERROR; } } - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemPositionOnlyHardware::on_configure( +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -109,7 +111,7 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_configure( RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector @@ -138,7 +140,7 @@ RRBotSystemPositionOnlyHardware::export_command_interfaces() return command_interfaces; } -CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -162,10 +164,10 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_activate( RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate( +hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -183,7 +185,7 @@ CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully deactivated!"); // END: This part here is for exemplary purposes - Please do not copy to your production code - return CallbackReturn::SUCCESS; + return hardware_interface::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 1a72e6880..1705eb032 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -29,11 +29,14 @@ 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 @@ -57,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) @@ -66,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) @@ -75,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) @@ -84,14 +87,14 @@ 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 @@ -115,7 +118,7 @@ CallbackReturn RRBotSystemWithSensorHardware::on_configure( RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithSensorHardware"), "Successfully configured!"); - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } std::vector @@ -151,7 +154,7 @@ RRBotSystemWithSensorHardware::export_command_interfaces() return command_interfaces; } -CallbackReturn RRBotSystemWithSensorHardware::on_activate( +hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -180,10 +183,10 @@ CallbackReturn RRBotSystemWithSensorHardware::on_activate( 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*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -201,7 +204,7 @@ CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( 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() From a3a792f8cb1afb60543bfe3bf1283c8aed90b758 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 11 May 2022 10:17:25 +0200 Subject: [PATCH 04/25] Fix up small error in example docs (#174) Co-authored-by: Bence Magyar --- README.md | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 43f3d4ed5..7b461cba0 100644 --- a/README.md +++ b/README.md @@ -346,6 +346,7 @@ This also demonstrates how launch files are usually reused for different scenari Files: - Launch file: [rrbot_system_position_only.launch.py](ros2_control_demo_bringup/launch/rrbot_system_position_only.launch.py) - Controllers yaml: [rrbot_controllers.yaml](ros2_control_demo_bringup/config/rrbot_controllers.yaml) + - URDF: [rrbot_system_position_only.urdf.xacro](ros2_control_demos/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_position_only.urdf.xacro) - `ros2_control` URDF tag: [rrbot_system_position_only.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro) Interfaces: @@ -379,6 +380,7 @@ Available launch file options: Files: - Launch file: [rrbot_system_multi_interface.launch.py](ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py) - Controllers yaml: [rrbot_multi_interface_forward_controllers.yaml](ros2_control_demo_bringup/config/rrbot_multi_interface_forward_controllers.yaml) + - URDF: [rrbot_system_multi_interface.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro) - `ros2_control` URDF tag: [rrbot_system_multi_interface.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro) Interfaces: @@ -434,7 +436,8 @@ Useful launch-file options: ### Example 3: "Industrial robot with integrated sensor" - Launch file: [rrbot_system_with_sensor.launch.py](ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py) -- URDF: [rrbot_system_with_sensor.urdf.xacro](ros2_control_demo_bringup/config/rrbot_with_sensor_controllers.yaml) +- Controllers: [rrbot_with_sensor_controllers.yaml](ros2_control_demo_bringup/config/rrbot_with_sensor_controllers.yaml) +- URDF: [rrbot_system_with_sensor.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro) - ros2_control URDF: [rrbot_system_with_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_with_sensor.ros2_control.xacro) - Command interfaces: @@ -465,8 +468,11 @@ ros2 topic echo /fts_broadcaster/wrench ### Example 4: "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) +- Controllers: [rrbot_with_external_sensor_controllers.yaml](ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml) +- URDF: [rrbot_with_external_sensor_controllers.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_with_external_sensor_controllers.urdf.xacro) +- ros2_control URDF: + - robot: [rrbot_system_position_only.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro) + - sensor: [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 @@ -497,7 +503,8 @@ ros2 topic echo /fts_broadcaster/wrench ### Example 5: "Modular Robots with separate communication to each actuator" - Launch file: [rrbot_modular_actuators.launch.py](ros2_control_demo_bringup/launch/rrbot_modular_actuators.launch.py) -- URDF: [rrbot_modular_actuators.urdf.xacro](ros2_control_demo_bringup/config/rrbot_modular_actuators.yaml) +- Controllers: [rrbot_modular_actuators.yaml](ros2_control_demo_bringup/config/rrbot_modular_actuators.yaml) +- URDF: [rrbot_modular_actuators.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators.urdf.xacro) - ros2_control URDF: [rrbot_modular_actuators.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro) - Command interfaces: From 6bd663106e05ae67dd50864d50959b040d5e7291 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 13 May 2022 17:31:55 +0100 Subject: [PATCH 05/25] Pass time and period to read() and write() (#179) * Pass time and period to read() and write() * Remove obsolete time variables * Fix whitespace --- README.md | 2 +- .../diffbot_system.hpp | 11 ++++----- .../external_rrbot_force_torque_sensor.hpp | 3 ++- .../rrbot_actuator.hpp | 6 +++-- .../rrbot_system_multi_interface.hpp | 11 ++++----- .../rrbot_system_position_only.hpp | 6 +++-- .../rrbot_system_with_sensor.hpp | 6 +++-- .../src/diffbot_system.cpp | 23 +++++++------------ .../external_rrbot_force_torque_sensor.cpp | 3 ++- .../src/rrbot_actuator.cpp | 6 +++-- .../src/rrbot_system_multi_interface.cpp | 21 ++++++----------- .../src/rrbot_system_position_only.cpp | 6 +++-- .../src/rrbot_system_with_sensor.cpp | 6 +++-- 13 files changed, 52 insertions(+), 58 deletions(-) diff --git a/README.md b/README.md index 7b461cba0..2937abc5d 100644 --- a/README.md +++ b/README.md @@ -470,7 +470,7 @@ ros2 topic echo /fts_broadcaster/wrench - Launch file: [rrbot_system_with_external_sensor.launch.py](ros2_control_demo_bringup/launch/rrbot_system_with_external_sensor.launch.py) - Controllers: [rrbot_with_external_sensor_controllers.yaml](ros2_control_demo_bringup/config/rrbot_with_external_sensor_controllers.yaml) - URDF: [rrbot_with_external_sensor_controllers.urdf.xacro](ros2_control_demo_description/rrbot_description/urdf/rrbot_with_external_sensor_controllers.urdf.xacro) -- ros2_control URDF: +- ros2_control URDF: - robot: [rrbot_system_position_only.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro) - sensor: [external_rrbot_force_torque_sensor.ros2_control.xacro](ros2_control_demo_description/rrbot_description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro) 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 85d2b810a..1becc4dd6 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 @@ -57,10 +57,12 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface 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 @@ -72,11 +74,6 @@ class DiffBotSystemHardware : public hardware_interface::SystemInterface std::vector hw_positions_; std::vector hw_velocities_; - // Store time between update loops - rclcpp::Clock clock_; - rclcpp::Time last_timestamp_; - rclcpp::Time current_timestamp; // Avoid initialization on each read - // Store the wheeled robot position double base_x_, base_y_, base_theta_; }; 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 79338443e..ce3b6e6e5 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 @@ -53,7 +53,8 @@ class ExternalRRBotForceTorqueSensorHardware : public hardware_interface::Sensor 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; private: // Parameters for the RRBot simulation diff --git a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp index 198b033da..be9d9414b 100644 --- a/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp +++ b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_actuator.hpp @@ -57,10 +57,12 @@ class RRBotModularJoint : public hardware_interface::ActuatorInterface 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_multi_interface.hpp b/ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp index a9ebf7d64..0607611e5 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 @@ -64,10 +64,12 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter 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 @@ -83,11 +85,6 @@ class RRBotSystemMultiInterfaceHardware : public hardware_interface::SystemInter std::vector hw_states_velocities_; std::vector hw_states_accelerations_; - // Store time between update loops - rclcpp::Clock clock_; - rclcpp::Time last_timestamp_; - rclcpp::Time current_timestamp; // Avoid initialization on each read - // Enum defining at which control level we are // Dumb way of maintaining the command_interface type per joint. enum integration_level_t : std::uint8_t 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 9e563a2be..cdf22b62e 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 @@ -58,10 +58,12 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa 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 c0dfdfaf9..2c3b2a3a6 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 @@ -61,10 +61,12 @@ class RRBotSystemWithSensorHardware : public hardware_interface::SystemInterface 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/src/diffbot_system.cpp b/ros2_control_demo_hardware/src/diffbot_system.cpp index b8005c45a..b2792c94b 100644 --- a/ros2_control_demo_hardware/src/diffbot_system.cpp +++ b/ros2_control_demo_hardware/src/diffbot_system.cpp @@ -21,7 +21,6 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" namespace ros2_control_demo_hardware @@ -97,8 +96,6 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_init( } } - clock_ = rclcpp::Clock(); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -153,8 +150,6 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_activate( } } - last_timestamp_ = clock_.now(); - RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -179,12 +174,9 @@ hardware_interface::CallbackReturn DiffBotSystemHardware::on_deactivate( 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) { - current_timestamp = clock_.now(); - rclcpp::Duration dt = current_timestamp - last_timestamp_; // Control period - last_timestamp_ = current_timestamp; - double radius = 0.02; // radius of the wheels double dist_w = 0.1; // distance between the wheels for (uint i = 0; i < hw_commands_.size(); i++) @@ -192,7 +184,7 @@ hardware_interface::return_type DiffBotSystemHardware::read() // 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.seconds() * 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 @@ -208,9 +200,9 @@ 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.seconds(); - base_y_ += base_dy * dt.seconds(); - base_theta_ += base_dtheta * dt.seconds(); + 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( @@ -221,7 +213,8 @@ hardware_interface::return_type DiffBotSystemHardware::read() 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..."); 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 64151aac8..850b8bac0 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 @@ -110,7 +110,8 @@ hardware_interface::CallbackReturn ExternalRRBotForceTorqueSensorHardware::on_de return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::read() +hardware_interface::return_type ExternalRRBotForceTorqueSensorHardware::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("ExternalRRBotForceTorqueSensorHardware"), "Reading..."); diff --git a/ros2_control_demo_hardware/src/rrbot_actuator.cpp b/ros2_control_demo_hardware/src/rrbot_actuator.cpp index 7eff53718..1ef1f8e38 100644 --- a/ros2_control_demo_hardware/src/rrbot_actuator.cpp +++ b/ros2_control_demo_hardware/src/rrbot_actuator.cpp @@ -151,7 +151,8 @@ hardware_interface::CallbackReturn RRBotModularJoint::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type RRBotModularJoint::read() +hardware_interface::return_type RRBotModularJoint::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("RRBotModularJoint"), "Reading..."); @@ -168,7 +169,8 @@ hardware_interface::return_type RRBotModularJoint::read() return hardware_interface::return_type::OK; } -hardware_interface::return_type ros2_control_demo_hardware::RRBotModularJoint::write() +hardware_interface::return_type ros2_control_demo_hardware::RRBotModularJoint::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("RRBotModularJoint"), "Writing...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 59d02030e..21b3e292a 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp @@ -22,7 +22,6 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" namespace ros2_control_demo_hardware @@ -97,8 +96,6 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_init( } } - clock_ = rclcpp::Clock(); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -246,8 +243,6 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_activat control_level_[i] = integration_level_t::UNDEFINED; } - last_timestamp_ = clock_.now(); - RCLCPP_INFO( rclcpp::get_logger("RRBotSystemMultiInterfaceHardware"), "System successfully activated! %u", control_level_[0]); @@ -275,12 +270,9 @@ hardware_interface::CallbackReturn RRBotSystemMultiInterfaceHardware::on_deactiv 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) { - current_timestamp = clock_.now(); - rclcpp::Duration dt = current_timestamp - last_timestamp_; - last_timestamp_ = current_timestamp; - for (std::size_t i = 0; i < hw_states_positions_.size(); i++) { switch (control_level_[i]) @@ -300,12 +292,12 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::read() case integration_level_t::VELOCITY: hw_states_accelerations_[i] = 0; hw_states_velocities_[i] = hw_commands_velocities_[i]; - hw_states_positions_[i] += (hw_states_velocities_[i] * dt.seconds()) / hw_slowdown_; + hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; break; case integration_level_t::ACCELERATION: hw_states_accelerations_[i] = hw_commands_accelerations_[i]; - hw_states_velocities_[i] += (hw_states_accelerations_[i] * dt.seconds()) / hw_slowdown_; - hw_states_positions_[i] += (hw_states_velocities_[i] * dt.seconds()) / hw_slowdown_; + hw_states_velocities_[i] += (hw_states_accelerations_[i] * period.seconds()) / hw_slowdown_; + hw_states_positions_[i] += (hw_states_velocities_[i] * period.seconds()) / hw_slowdown_; break; } // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code @@ -318,7 +310,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*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code for (std::size_t i = 0; i < hw_commands_positions_.size(); i++) 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 044e5fc8f..1871e3292 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_position_only.cpp @@ -188,7 +188,8 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_deactivat return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() +hardware_interface::return_type RRBotSystemPositionOnlyHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading..."); @@ -207,7 +208,8 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() return hardware_interface::return_type::OK; } -hardware_interface::return_type RRBotSystemPositionOnlyHardware::write() +hardware_interface::return_type RRBotSystemPositionOnlyHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing..."); 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 1705eb032..dbeb97585 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -207,7 +207,8 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_deactivate( 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..."); @@ -239,7 +240,8 @@ hardware_interface::return_type RRBotSystemWithSensorHardware::read() 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..."); From 48bb610bbef81c04de118ecbc088682f6583b2ae Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 23 May 2022 02:34:13 -0500 Subject: [PATCH 06/25] Add First-Time Users section to the README (#181) * Add First-Time Users section to the README * Format --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index 2937abc5d..0147b1c33 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,15 @@ This repository provides templates for the development of `ros2_control`-enabled robots and a simple simulations to demonstrate and prove `ros2_control` concepts. +### First-Time Users + +If you're just starting out, we suggest to look at the minimal example: `ros2_control_demo_bringup/launch/rrbot_system_position_only.launch.py`. + +Also pay attention to these files: + +- `ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_position_only.ros2_control.xacro` -- this file defines the ros2_control interfaces for each joint, e.g. position or velocity. The simulation can be launched with Gazebo or simulated with RViz only. +- `rrbot_controllers.yaml` -- list the controllers that will be launched. + ### Goals The repository has three goals: From 2fae87b04057ebd5e84d17138911f34dab4e62e5 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Mon, 23 May 2022 02:34:57 -0500 Subject: [PATCH 07/25] Fix warning about "output" from launch files (#183) --- ros2_control_demo_bringup/launch/diffbot.launch.py | 5 +---- ros2_control_demo_bringup/launch/rrbot.launch.py | 5 +---- ros2_control_demo_bringup/launch/rrbot_base.launch.py | 5 +---- 3 files changed, 3 insertions(+), 12 deletions(-) diff --git a/ros2_control_demo_bringup/launch/diffbot.launch.py b/ros2_control_demo_bringup/launch/diffbot.launch.py index 7dbdf8b90..cef13b492 100644 --- a/ros2_control_demo_bringup/launch/diffbot.launch.py +++ b/ros2_control_demo_bringup/launch/diffbot.launch.py @@ -49,10 +49,7 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="both", ) robot_state_pub_node = Node( package="robot_state_publisher", diff --git a/ros2_control_demo_bringup/launch/rrbot.launch.py b/ros2_control_demo_bringup/launch/rrbot.launch.py index 8a7773d09..3ce2b90d8 100644 --- a/ros2_control_demo_bringup/launch/rrbot.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot.launch.py @@ -60,10 +60,7 @@ def generate_launch_description(): "/position_commands", ), ], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="both", ) robot_state_pub_node = Node( package="robot_state_publisher", diff --git a/ros2_control_demo_bringup/launch/rrbot_base.launch.py b/ros2_control_demo_bringup/launch/rrbot_base.launch.py index a6b340389..f9b1d0f37 100644 --- a/ros2_control_demo_bringup/launch/rrbot_base.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_base.launch.py @@ -160,10 +160,7 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", parameters=[robot_description, robot_controllers], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="both", ) robot_state_pub_node = Node( package="robot_state_publisher", From 88d12f9bfd51669148087c76b8bfd2fdd72ad48f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Sat, 18 Jun 2022 12:59:50 -0500 Subject: [PATCH 08/25] Replace use_sim with use_gazebo (#182) --- ros2_control_demo_bringup/launch/rrbot_base.launch.py | 8 ++++---- .../launch/rrbot_system_position_only_gazebo.launch.py | 2 +- .../diffbot_description/urdf/diffbot_system.urdf.xacro | 2 +- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- .../rrbot_modular_actuators.ros2_control.xacro | 2 +- .../rrbot_system_multi_interface.ros2_control.xacro | 6 +++--- .../rrbot_system_position_only.ros2_control.xacro | 6 +++--- .../rrbot_system_with_sensor.ros2_control.xacro | 2 +- .../urdf/rrbot_modular_actuators.urdf.xacro | 2 +- .../urdf/rrbot_system_multi_interface.urdf.xacro | 2 +- .../urdf/rrbot_system_position_only.urdf.xacro | 2 +- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 2 +- .../urdf/rrbot_system_with_sensor.urdf.xacro | 2 +- 13 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ros2_control_demo_bringup/launch/rrbot_base.launch.py b/ros2_control_demo_bringup/launch/rrbot_base.launch.py index f9b1d0f37..877db94a1 100644 --- a/ros2_control_demo_bringup/launch/rrbot_base.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_base.launch.py @@ -65,7 +65,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "use_sim", + "use_gazebo", default_value="false", description="Start robot in Gazebo simulation.", ) @@ -111,7 +111,7 @@ def generate_launch_description(): description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") prefix = LaunchConfiguration("prefix") - use_sim = LaunchConfiguration("use_sim") + use_gazebo = LaunchConfiguration("use_gazebo") use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") slowdown = LaunchConfiguration("slowdown") @@ -130,8 +130,8 @@ def generate_launch_description(): "prefix:=", prefix, " ", - "use_sim:=", - use_sim, + "use_gazebo:=", + use_gazebo, " ", "use_fake_hardware:=", use_fake_hardware, diff --git a/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py index 7630a00d9..4436958c7 100644 --- a/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): "rrbot_system_position_only.urdf.xacro", ] ), - " use_sim:=true", + " use_gazebo:=true", ] ) robot_description = {"robot_description": robot_description_content} diff --git a/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro b/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro index 30fc1b632..277a5d87d 100644 --- a/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro +++ b/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro @@ -5,7 +5,7 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/diffbot_description/urdf/diffbot.xacro --> - + 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 53d4059c2..b2e80127f 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 @@ -1,7 +1,7 @@ - + diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro index 40c7f6a94..cac323371 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_modular_actuators.ros2_control.xacro @@ -1,7 +1,7 @@ - + diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro index d3e73a64d..463e7bfd3 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro @@ -1,16 +1,16 @@ - + - + gazebo_ros2_control/GazeboSystem - + fake_components/GenericSystem 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 e6c078868..c00cbd684 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 @@ -1,16 +1,16 @@ - + - + gazebo_ros2_control/GazeboSystem - + fake_components/GenericSystem 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 e8f713ed7..045143add 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 @@ -1,7 +1,7 @@ - + diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators.urdf.xacro index 5c3f11689..9f5f10fec 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_modular_actuators.urdf.xacro @@ -5,7 +5,7 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - + diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro index 0b145c08b..d20ee43d3 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro @@ -5,7 +5,7 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - + 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 8cabe5b92..2cce876e2 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 @@ -5,7 +5,7 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - + 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 642a0f62c..708ed0156 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,7 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - + diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro index f95324303..878602fc0 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro @@ -5,7 +5,7 @@ Copied and modified from ROS1 example - https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro --> - + From aabf2cd522ae8f9afe5fb4e2d4a42fca5b4b423e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 15 Jul 2022 12:51:25 +0200 Subject: [PATCH 09/25] Fix missing correction of output parameter on Nodes in launch files. (#192) * Fix missing correction of output parameter on Nodes in launch files. * Update test_joint_trajectory_controller.launch.py --- .../launch/test_forward_position_controller.launch.py | 5 +---- .../launch/test_joint_trajectory_controller.launch.py | 5 +---- 2 files changed, 2 insertions(+), 8 deletions(-) diff --git a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py b/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py index 0e5743388..2dbb17b9d 100644 --- a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py +++ b/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py @@ -35,10 +35,7 @@ def generate_launch_description(): executable="publisher_forward_position_controller", name="publisher_forward_position_controller", parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="both", ) ] ) diff --git a/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py b/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py index 026e8a8eb..3e6f2fe8a 100644 --- a/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py +++ b/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py @@ -35,10 +35,7 @@ def generate_launch_description(): executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, + output="both", ) ] ) From fe14bf3c0f2ee9b043767876e1a2c960bb721dea Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Fri, 15 Jul 2022 13:03:17 +0100 Subject: [PATCH 10/25] Use mock hardware instead of fake hardware plugin package (#188) --- .../ros2_control/diffbot_system.ros2_control.xacro | 2 +- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 2 +- .../rrbot_system_multi_interface.ros2_control.xacro | 2 +- .../ros2_control/rrbot_system_position_only.ros2_control.xacro | 2 +- .../ros2_control/rrbot_system_with_sensor.ros2_control.xacro | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro b/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro index 8473503a4..65109b165 100644 --- a/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro +++ b/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro @@ -6,7 +6,7 @@ - fake_components/GenericSystem + mock_components/GenericSystem ${fake_sensor_commands} 0.0 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 b2e80127f..244e04c87 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 @@ -6,7 +6,7 @@ - fake_components/GenericSystem + mock_components/GenericSystem ${fake_sensor_commands} 0.0 diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro index 463e7bfd3..11cd5313e 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro @@ -13,7 +13,7 @@ - fake_components/GenericSystem + mock_components/GenericSystem ${fake_sensor_commands} 0.0 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 c00cbd684..c2740db0c 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 @@ -13,7 +13,7 @@ - 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 045143add..357c1f191 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 From ee92329adcdaa195303906c37d912f4d50c7cc69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 15 Jul 2022 14:45:58 +0200 Subject: [PATCH 11/25] Fix DLL export defintion. (#195) --- ros2_control_demo_hardware/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ros2_control_demo_hardware/CMakeLists.txt b/ros2_control_demo_hardware/CMakeLists.txt index cb68dd708..e27bb5658 100644 --- a/ros2_control_demo_hardware/CMakeLists.txt +++ b/ros2_control_demo_hardware/CMakeLists.txt @@ -44,9 +44,9 @@ ament_target_dependencies( # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} PRIVATE "JOINT_STATE_BROADCASTER_BUILDING_DLL") -# prevent pluginlib from using boost -target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${PROJECT_NAME} PRIVATE "ROS2_CONTROL_DEMO_HARDWARE_BUILDING_DLL") + +# Export hardware pligins pluginlib_export_plugin_description_file(hardware_interface ros2_control_demo_hardware.xml) # INSTALL From e4c9ba4fcae41e5807a72fc11f090b85352d53bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 15 Jul 2022 14:46:35 +0200 Subject: [PATCH 12/25] Fix bug that doesn't reset commands properly on activation. (#194) Resolves #178 --- 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 dbeb97585..3f5e6d2c0 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_with_sensor.cpp @@ -172,7 +172,7 @@ hardware_interface::CallbackReturn RRBotSystemWithSensorHardware::on_activate( // command and state should be equal when starting for (uint i = 0; i < hw_joint_states_.size(); i++) { - hw_joint_states_[i] = hw_joint_states_[i]; + hw_joint_commands_[i] = hw_joint_states_[i]; } // set default value for sensor From fe2307235fefdef7ce81713a24206608a849e6a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 15 Jul 2022 14:47:27 +0200 Subject: [PATCH 13/25] Publish to correct topic in DiffBot example (#196) --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0147b1c33..21494d5fb 100644 --- a/README.md +++ b/README.md @@ -253,7 +253,7 @@ The *DiffBot* URDF files can be found in `urdf` folder of `diffbot_description` 1. If everything is fine, now you can send a command to *Diff Drive Controller* using ros2 cli interface: ``` - ros2 topic pub --rate 30 /cmd_vel geometry_msgs/msg/Twist "linear: + ros2 topic pub --rate 30 /diff_drive_controller/cmd_vel_unstamped geometry_msgs/msg/Twist "linear: x: 0.7 y: 0.0 z: 0.0 From 41a8fa5adb1a175042283b9526ad205271115ced Mon Sep 17 00:00:00 2001 From: HusamZain <97671800+HusamZain@users.noreply.github.com> Date: Fri, 15 Jul 2022 15:27:40 +0200 Subject: [PATCH 14/25] fixing command line in Step 5 in diffbot example (#155) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Denis Štogl --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 21494d5fb..f6f39231a 100644 --- a/README.md +++ b/README.md @@ -253,7 +253,7 @@ The *DiffBot* URDF files can be found in `urdf` folder of `diffbot_description` 1. If everything is fine, now you can send a command to *Diff Drive Controller* using ros2 cli interface: ``` - ros2 topic pub --rate 30 /diff_drive_controller/cmd_vel_unstamped geometry_msgs/msg/Twist "linear: + ros2 topic pub --rate 30 /diffbot_base_controller/cmd_vel_unstamped geometry_msgs/msg/Twist "linear: x: 0.7 y: 0.0 z: 0.0 From f42b06f92f8c28bcfa2465fa4f9eb61cbfa9c639 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Fri, 15 Jul 2022 15:29:47 +0200 Subject: [PATCH 15/25] Fix output parameter in launch files for testing controllers. (#197) From 4329be03fbceb31a86d72b202d885782777dc2f6 Mon Sep 17 00:00:00 2001 From: Interactics <56077549+Interactics@users.noreply.github.com> Date: Mon, 22 Aug 2022 01:46:51 +0900 Subject: [PATCH 16/25] Fix some typos (#205) --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index f6f39231a..a68d0a8ee 100644 --- a/README.md +++ b/README.md @@ -131,13 +131,13 @@ The *RRBot* URDF files can be found in the `urdf` folder of `rrbot_description` 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: +1. To start *RRBot* example open a terminal, source your ROS2-workspace and execute its launch file with: ``` ros2 launch ros2_control_demo_bringup rrbot.launch.py ``` The launch file loads and starts the robot hardware, controllers and opens `RViz`. In starting terminal you will see a lot of output from the hardware implementation showing its internal states. - This is only of exemplary purpuses and should be avoided as much as possible in a hardware interface implementation. + This is only of exemplary purposes and should be avoided as much as possible in a hardware interface implementation. If you can see two orange and one yellow rectangle in in `RViz` everything has started properly. Still, to be sure, let's introspect the control system before moving *RRBot*. From a5ed9040106a1c8640d567a41ab72cf664406adb Mon Sep 17 00:00:00 2001 From: Jaron <13423952+jaron-l@users.noreply.github.com> Date: Sun, 16 Oct 2022 04:41:37 -0500 Subject: [PATCH 17/25] ci: :construction_worker: update rhel container (#212) --- .github/workflows/rolling-rhel-binary-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 6df1dfbbc..341f4e2fa 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -17,7 +17,7 @@ jobs: runs-on: ubuntu-latest env: ROS_DISTRO: rolling - container: jaronl/ros:rolling-alma + container: ghcr.io/ros-controls/ros:${{ env.ROS_DISTRO }}-rhel steps: - uses: actions/checkout@v3 with: From 6478457edd8bc244ac08341342ad44a27d1c47b4 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 2 Nov 2022 18:46:30 +0000 Subject: [PATCH 18/25] Remove ros index links (#204) --- README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index a68d0a8ee..88ff1c509 100644 --- a/README.md +++ b/README.md @@ -23,12 +23,12 @@ The repository has three goals: ## Build status -ROS2 Distro | Branch | Build status | Documentation | Released packages -:---------: | :----: | :----------: | :-----------: | :---------------: -**Rolling** | [`master`](https://github.com/ros-controls/ros2_control_demos/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control_demos](https://index.ros.org/p/ros2_control_demos/#rolling) -**Rolling - last Focal** | [`master`](https://github.com/ros-controls/ros2_control_demos/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build-last-focal.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build-last-focal.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control_demos](https://index.ros.org/p/ros2_control_demos/#rolling) -**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control_demos/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-binary-build.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-semi-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-semi-binary-build.yml?branch=galactic)
[![Galactic Source Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-source-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-source-build.yml?branch=galactic) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control_demos](https://index.ros.org/p/ros2_control_demos/#galactic) -**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control_demos/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-binary-build.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-semi-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-semi-binary-build.yml?branch=foxy)
[![Foxy Source Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-source-build.yml?branch=foxy) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) | [ros2_control_demos](https://index.ros.org/p/ros2_control_demos/#foxy) +ROS2 Distro | Branch | Build status | Documentation +:---------: | :----: | :----------: | :-----------: +**Rolling** | [`master`](https://github.com/ros-controls/ros2_control_demos/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build.yml?branch=master)
[![Rolling Source Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-source-build.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-source-build.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) +**Rolling - last Focal** | [`master`](https://github.com/ros-controls/ros2_control_demos/tree/rolling) | [![Rolling Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-binary-build-last-focal.yml?branch=master)
[![Rolling Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build-last-focal.yml/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/rolling-semi-binary-build-last-focal.yml?branch=master) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) +**Galactic** | [`galactic`](https://github.com/ros-controls/ros2_control_demos/tree/galactic) | [![Galactic Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-binary-build.yml?branch=galactic)
[![Galactic Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-semi-binary-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-semi-binary-build.yml?branch=galactic)
[![Galactic Source Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-source-build.yml/badge.svg?branch=galactic)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/galactic-source-build.yml?branch=galactic) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) +**Foxy** | [`foxy`](https://github.com/ros-controls/ros2_control_demos/tree/foxy) | [![Foxy Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-binary-build.yml?branch=foxy)
[![Foxy Semi-Binary Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-semi-binary-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-semi-binary-build.yml?branch=foxy)
[![Foxy Source Build](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-source-build.yml/badge.svg?branch=foxy)](https://github.com/ros-controls/ros2_control_demos/actions/workflows/foxy-source-build.yml?branch=foxy) | [Documentation](https://control.ros.org)
[API Reference](https://control.ros.org/rolling/api/) ### Explanation of different build types From 4abae28a8388e76264a5eafb84a36ac00768a276 Mon Sep 17 00:00:00 2001 From: Kvk Praneeth <55596533+kvkpraneeth@users.noreply.github.com> Date: Thu, 3 Nov 2022 00:18:11 +0530 Subject: [PATCH 19/25] fake_sensor_commands replaced (#200) --- ros2_control_demo_bringup/launch/rrbot_base.launch.py | 8 ++++---- .../launch/rrbot_system_multi_interface.launch.py | 2 +- .../launch/rrbot_system_position_only.launch.py | 6 +++--- .../launch/rrbot_system_with_external_sensor.launch.py | 6 +++--- .../launch/rrbot_system_with_sensor.launch.py | 6 +++--- .../ros2_control/diffbot_system.ros2_control.xacro | 4 ++-- .../diffbot_description/urdf/diffbot_system.urdf.xacro | 4 ++-- .../external_rrbot_force_torque_sensor.ros2_control.xacro | 4 ++-- .../rrbot_system_multi_interface.ros2_control.xacro | 4 ++-- .../rrbot_system_position_only.ros2_control.xacro | 4 ++-- .../rrbot_system_with_sensor.ros2_control.xacro | 4 ++-- .../urdf/rrbot_system_multi_interface.urdf.xacro | 4 ++-- .../urdf/rrbot_system_position_only.urdf.xacro | 4 ++-- .../urdf/rrbot_system_with_external_sensor.urdf.xacro | 6 +++--- .../urdf/rrbot_system_with_sensor.urdf.xacro | 4 ++-- 15 files changed, 35 insertions(+), 35 deletions(-) diff --git a/ros2_control_demo_bringup/launch/rrbot_base.launch.py b/ros2_control_demo_bringup/launch/rrbot_base.launch.py index 877db94a1..796327f17 100644 --- a/ros2_control_demo_bringup/launch/rrbot_base.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_base.launch.py @@ -79,7 +79,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "fake_sensor_commands", + "mock_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.", @@ -113,7 +113,7 @@ def generate_launch_description(): prefix = LaunchConfiguration("prefix") use_gazebo = LaunchConfiguration("use_gazebo") use_fake_hardware = LaunchConfiguration("use_fake_hardware") - fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") robot_controller = LaunchConfiguration("robot_controller") start_rviz = LaunchConfiguration("start_rviz") @@ -136,8 +136,8 @@ def generate_launch_description(): "use_fake_hardware:=", use_fake_hardware, " ", - "fake_sensor_commands:=", - fake_sensor_commands, + "mock_sensor_commands:=", + mock_sensor_commands, " ", "slowdown:=", slowdown, diff --git a/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py index 3dd4834d5..2f132402d 100644 --- a/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py @@ -55,7 +55,7 @@ def generate_launch_description(): "description_file": "rrbot_system_multi_interface.urdf.xacro", "prefix": prefix, "use_fake_hardware": "false", - "fake_sensor_commands": "false", + "mock_sensor_commands": "false", "slowdown": slowdown, "robot_controller": robot_controller, }.items(), diff --git a/ros2_control_demo_bringup/launch/rrbot_system_position_only.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_position_only.launch.py index fc579b9d1..bbd15d7aa 100644 --- a/ros2_control_demo_bringup/launch/rrbot_system_position_only.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_position_only.launch.py @@ -39,7 +39,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "fake_sensor_commands", + "mock_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.", @@ -68,7 +68,7 @@ def generate_launch_description(): # Initialize Arguments prefix = LaunchConfiguration("prefix") use_fake_hardware = LaunchConfiguration("use_fake_hardware") - fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") robot_controller = LaunchConfiguration("robot_controller") start_rviz = LaunchConfiguration("start_rviz") @@ -79,7 +79,7 @@ def generate_launch_description(): "description_file": "rrbot_system_position_only.urdf.xacro", "prefix": prefix, "use_fake_hardware": use_fake_hardware, - "fake_sensor_commands": fake_sensor_commands, + "mock_sensor_commands": mock_sensor_commands, "slowdown": slowdown, "robot_controller": robot_controller, "start_rviz": start_rviz, 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 9c482c75e..2487b98f7 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 @@ -42,7 +42,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "fake_sensor_commands", + "mock_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.", @@ -60,7 +60,7 @@ def generate_launch_description(): # Initialize Arguments prefix = LaunchConfiguration("prefix") use_fake_hardware = LaunchConfiguration("use_fake_hardware") - fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") base_launch = IncludeLaunchDescription( @@ -70,7 +70,7 @@ def generate_launch_description(): "description_file": "rrbot_system_with_external_sensor.urdf.xacro", "prefix": prefix, "use_fake_hardware": use_fake_hardware, - "fake_sensor_commands": fake_sensor_commands, + "mock_sensor_commands": mock_sensor_commands, "slowdown": slowdown, }.items(), ) diff --git a/ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py index 6c30f641f..7503cfb89 100755 --- a/ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_system_with_sensor.launch.py @@ -42,7 +42,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "fake_sensor_commands", + "mock_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.", @@ -59,7 +59,7 @@ def generate_launch_description(): # Initialize Arguments prefix = LaunchConfiguration("prefix") use_fake_hardware = LaunchConfiguration("use_fake_hardware") - fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") base_launch = IncludeLaunchDescription( @@ -69,7 +69,7 @@ def generate_launch_description(): "description_file": "rrbot_system_with_sensor.urdf.xacro", "prefix": prefix, "use_fake_hardware": use_fake_hardware, - "fake_sensor_commands": fake_sensor_commands, + "mock_sensor_commands": mock_sensor_commands, "slowdown": slowdown, }.items(), ) diff --git a/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro b/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro index 65109b165..e071a2035 100644 --- a/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro +++ b/ros2_control_demo_description/diffbot_description/ros2_control/diffbot_system.ros2_control.xacro @@ -1,13 +1,13 @@ - + mock_components/GenericSystem - ${fake_sensor_commands} + ${mock_sensor_commands} 0.0 diff --git a/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro b/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro index 277a5d87d..eb00ec4d0 100644 --- a/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro +++ b/ros2_control_demo_description/diffbot_description/urdf/diffbot_system.urdf.xacro @@ -9,7 +9,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/diffbot_de - + @@ -30,6 +30,6 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/diffbot_de + mock_sensor_commands="$(arg mock_sensor_commands)"/> 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 244e04c87..ce91bbfe1 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 @@ -1,13 +1,13 @@ - + mock_components/GenericSystem - ${fake_sensor_commands} + ${mock_sensor_commands} 0.0 diff --git a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro index 11cd5313e..89caa06be 100644 --- a/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro +++ b/ros2_control_demo_description/rrbot_description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro @@ -1,7 +1,7 @@ - + @@ -14,7 +14,7 @@ mock_components/GenericSystem - ${fake_sensor_commands} + ${mock_sensor_commands} 0.0 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 c2740db0c..9aaaa7393 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 @@ -1,7 +1,7 @@ - + @@ -14,7 +14,7 @@ mock_components/GenericSystem - ${fake_sensor_commands} + ${mock_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 357c1f191..ba803fd91 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 @@ -1,14 +1,14 @@ - + mock_components/GenericSystem - ${fake_sensor_commands} + ${mock_sensor_commands} 0.0 diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro index d20ee43d3..497932749 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_multi_interface.urdf.xacro @@ -9,7 +9,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + @@ -40,7 +40,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc 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 2cce876e2..8254deff4 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 @@ -9,7 +9,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + @@ -40,7 +40,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc 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 708ed0156..4b92541de 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 @@ -9,7 +9,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc - + @@ -38,11 +38,11 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc + mock_sensor_commands="$(arg mock_sensor_commands)" /> diff --git a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro index 878602fc0..d820e2521 100644 --- a/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro +++ b/ros2_control_demo_description/rrbot_description/urdf/rrbot_system_with_sensor.urdf.xacro @@ -9,7 +9,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 From 041ab7ebfe714bb7f2544b064ab0c2ab415a562b Mon Sep 17 00:00:00 2001 From: t3ch9 Date: Tue, 13 Dec 2022 03:17:37 -0500 Subject: [PATCH 20/25] Demo 5b broken, update code fixed (#208) --- .../publisher_forward_position_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py index 33b0efa41..cbf3ab7c6 100644 --- a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py +++ b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py @@ -44,7 +44,7 @@ def __init__(self): float_goal.append(float(value)) self.goals.append(float_goal) - publish_topic = "/" + controller_name + "/" + "commands" + publish_topic = "/position_commands" self.get_logger().info( f'Publishing {len(goal_names)} goals on topic "{publish_topic}"\ From de1012a839c92da1c44c95324cfbfe568f380fb9 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jan 2023 08:58:34 +0100 Subject: [PATCH 21/25] =?UTF-8?q?=F0=9F=94=A7=20Fixes=20and=20updated=20on?= =?UTF-8?q?=20pre-commit=20hooks=20and=20their=20action.=20(#218)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Use pydocstyle instead of pep257 because it replaces it. Bump versions of hooks. * Remove fixed python version from formatting action. Fix action warnings about Node.js 12 by updating them. Bump version of clang-format. * correct formatting. --- .github/workflows/ci-format.yml | 8 +++---- .pre-commit-config.yaml | 22 +++++++++---------- .../src/rrbot_system_multi_interface.cpp | 6 ++--- .../publisher_forward_position_controller.py | 2 +- 4 files changed, 19 insertions(+), 19 deletions(-) diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 77c35301d..2d0286518 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,11 +13,11 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - - uses: actions/setup-python@v2 + - uses: actions/setup-python@v4.4.0 with: - python-version: 3.9.7 + python-version: '3.10' - name: Install system hooks - run: sudo apt install -qq clang-format-11 cppcheck - - uses: pre-commit/action@v2.0.3 + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 with: extra_args: --all-files --hook-stage manual diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e932ab654..c401416a7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.4.0 hooks: - id: check-added-large-files - id: check-ast @@ -33,26 +33,26 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v2.31.1 + rev: v3.3.1 hooks: - id: pyupgrade args: [--py36-plus] - repo: https://github.com/psf/black - rev: 22.3.0 + rev: 22.12.0 hooks: - id: black args: ["--line-length=99"] - # PEP 257 - - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257 - rev: v0.3.3 + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.2.3 hooks: - - id: pep257 + - id: pydocstyle args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 4.0.1 + rev: 6.0.0 hooks: - id: flake8 args: ["--ignore=E501"] @@ -63,7 +63,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-14 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] @@ -119,7 +119,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: 0.10.1 + rev: v1.1.1 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -136,7 +136,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.1.0 + rev: v2.2.2 hooks: - id: codespell args: ['--write-changes'] 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 21b3e292a..2df73d8cf 100644 --- a/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp +++ b/ros2_control_demo_hardware/src/rrbot_system_multi_interface.cpp @@ -164,9 +164,9 @@ hardware_interface::return_type RRBotSystemMultiInterfaceHardware::prepare_comma 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]; - })) + if (!std::all_of( + new_modes.begin() + 1, new_modes.end(), + [&](integration_level_t mode) { return mode == new_modes[0]; })) { return hardware_interface::return_type::ERROR; } diff --git a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py index cbf3ab7c6..92aedbbd1 100644 --- a/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py +++ b/ros2_control_test_nodes/ros2_control_test_nodes/publisher_forward_position_controller.py @@ -27,7 +27,7 @@ def __init__(self): self.declare_parameter("goal_names", ["pos1", "pos2"]) # Read parameters - controller_name = self.get_parameter("controller_name").value + # controller_name = self.get_parameter("controller_name").value wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value goal_names = self.get_parameter("goal_names").value From e38961cc55f6cfdc7530f5ebcdf2fa40c27a51cf Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jan 2023 14:13:58 +0100 Subject: [PATCH 22/25] [CI] Add dependabot to automatically update actions in workflows (#219) --- .github/dependabot.yml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 000000000..05a48fc65 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" From 9c7c04ea5e9282c38cf02b7acc31338f31061c73 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 13 Jan 2023 14:18:11 +0100 Subject: [PATCH 23/25] Bump codecov/codecov-action from 1.0.14 to 3.1.1 (#221) Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- .github/workflows/ci-coverage-build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-coverage-build.yml b/.github/workflows/ci-coverage-build.yml index 8e3685fd5..ebd5e1709 100644 --- a/.github/workflows/ci-coverage-build.yml +++ b/.github/workflows/ci-coverage-build.yml @@ -38,7 +38,7 @@ jobs: } colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml skip-tests: true - - uses: codecov/codecov-action@v1.0.14 + - uses: codecov/codecov-action@v3.1.1 with: file: ros_ws/lcov/total_coverage.info flags: unittests From b3939631e3586c3a14c53099065b9870afb1a3c4 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jan 2023 14:18:58 +0100 Subject: [PATCH 24/25] Update list of possible reviewers --- .github/reviewer-lottery.yml | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/.github/reviewer-lottery.yml b/.github/reviewer-lottery.yml index fbaf95434..a6a949fb7 100644 --- a/.github/reviewer-lottery.yml +++ b/.github/reviewer-lottery.yml @@ -14,28 +14,22 @@ groups: reviewers: 5 usernames: - rosterloh - - kellyprankin - progtologist - arne48 - DasRoteSkelett - - a10263790 - Serafadam - harderthan - jaron-l - malapatiravi - - erickisos - - ShawnSchaerer - - Briancbn - - TomoyaFujita2016 - homalozoa + - erickisos - anfemosa - - jackcenter - VX792 - mhubii - livanov93 - aprotyas - peterdavidfagan - - UsamaHamayun1 - duringhof - bijoua29 - - kasiceo + - lm2292 + - mcbed From 11e2c886e5bf3fe3d1c3ce42b8b21a2f9f4c0aad Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 5 Aug 2023 20:22:04 +0000 Subject: [PATCH 25/25] Bump example to latest ros2_control(ler) syntax --- README.md | 6 ++--- ...er_manager_forward_position_publisher.yaml | 5 ++-- ...er_manager_joint_trajectory_publisher.yaml | 24 ++++++++++++------- ..._namespace_forward_position_publisher.yaml | 3 ++- ..._namespace_joint_trajectory_publisher.yaml | 12 ++++++---- ...oller_manager_example_two_rrbots.launch.py | 4 ++-- .../launch/rrbot_namespace.launch.py | 2 +- ...test_forward_position_controller.launch.py | 2 +- ...test_joint_trajectory_controller.launch.py | 2 +- 9 files changed, 37 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 056eed017..b2eaaf571 100644 --- a/README.md +++ b/README.md @@ -699,7 +699,7 @@ ros2 launch ros2_control_demo_bringup test_forward_position_controller.launch.py Switch controller to use `position_trajectory_controller` (`JointTrajectoryController`): ``` -ros2 control switch_controllers -c /rrbot/controller_manager --stop forward_position_controller --start position_trajectory_controller +ros2 control switch_controllers -c /rrbot/controller_manager --deactivate forward_position_controller --activate position_trajectory_controller ``` Commanding the robot using `JointTrajectoryController` (name: `/rrbot/position_trajectory_controller`) @@ -750,8 +750,8 @@ ros2 launch ros2_control_demo_bringup test_multi_controller_manager_forward_posi Switch controller to use `position_trajectory_controller`s (`JointTrajectoryController`) - alternatively start main launch file with argument `robot_controller:=position_trajectory_controller`: ``` -ros2 control switch_controllers -c /rrbot_1/controller_manager --stop forward_position_controller --start position_trajectory_controller -ros2 control switch_controllers -c /rrbot_2/controller_manager --stop forward_position_controller --start position_trajectory_controller +ros2 control switch_controllers -c /rrbot_1/controller_manager --deactivate forward_position_controller --activate position_trajectory_controller +ros2 control switch_controllers -c /rrbot_2/controller_manager --deactivate forward_position_controller --activate position_trajectory_controller ``` Commanding the robot using `JointTrajectoryController` (`position_trajectory_controller`): diff --git a/ros2_control_demo_bringup/config/multi_controller_manager_forward_position_publisher.yaml b/ros2_control_demo_bringup/config/multi_controller_manager_forward_position_publisher.yaml index 2e853b690..0a3afff56 100644 --- a/ros2_control_demo_bringup/config/multi_controller_manager_forward_position_publisher.yaml +++ b/ros2_control_demo_bringup/config/multi_controller_manager_forward_position_publisher.yaml @@ -1,7 +1,8 @@ /rrbot_1/publisher_forward_position_controller: ros__parameters: - controller_name: "rrbot_1/forward_position_controller" + + publish_topic: "forward_position_controller/commands" wait_sec_between_publish: 5 goal_names: ["pos1", "pos2", "pos3", "pos4"] @@ -14,7 +15,7 @@ /rrbot_2/publisher_forward_position_controller: ros__parameters: - controller_name: "rrbot_2/forward_position_controller" + publish_topic: "forward_position_controller/commands" wait_sec_between_publish: 5 goal_names: ["pos1", "pos2", "pos3", "pos4"] diff --git a/ros2_control_demo_bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml b/ros2_control_demo_bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml index 34d13ccfd..3792fa61e 100644 --- a/ros2_control_demo_bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml +++ b/ros2_control_demo_bringup/config/multi_controller_manager_joint_trajectory_publisher.yaml @@ -5,10 +5,14 @@ wait_sec_between_publish: 6 goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: [0.785, 0.785] - pos2: [0.0, 0.0] - pos3: [-0.785, -0.785] - pos4: [0.0, 0.0] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] joints: - rrbot_1_joint1 @@ -27,10 +31,14 @@ wait_sec_between_publish: 6 goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: [-0.785, 0.0] - pos2: [0.0, -0.785] - pos3: [+0.785, -1.57] - pos4: [+1.57, -0.785] + pos1: + positions: [-0.785, 0.0] + pos2: + positions: [0.0, -0.785] + pos3: + positions: [+0.785, -1.57] + pos4: + positions: [+1.57, -0.785] joints: - rrbot_2_joint1 diff --git a/ros2_control_demo_bringup/config/rrbot_namespace_forward_position_publisher.yaml b/ros2_control_demo_bringup/config/rrbot_namespace_forward_position_publisher.yaml index 88b2f573d..c022d0873 100644 --- a/ros2_control_demo_bringup/config/rrbot_namespace_forward_position_publisher.yaml +++ b/ros2_control_demo_bringup/config/rrbot_namespace_forward_position_publisher.yaml @@ -1,9 +1,10 @@ publisher_forward_position_controller: ros__parameters: - controller_name: "rrbot/forward_position_controller" wait_sec_between_publish: 5 + publish_topic: /rrbot/forward_position_controller/commands + goal_names: ["pos1", "pos2", "pos3", "pos4"] pos1: [0.785, 0.785] pos2: [0, 0] diff --git a/ros2_control_demo_bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml b/ros2_control_demo_bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml index 6a85e4a10..8b357deec 100644 --- a/ros2_control_demo_bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml +++ b/ros2_control_demo_bringup/config/rrbot_namespace_joint_trajectory_publisher.yaml @@ -5,10 +5,14 @@ publisher_joint_trajectory_controller: wait_sec_between_publish: 6 goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: [0.785, 0.785] - pos2: [0, 0] - pos3: [-0.785, -0.785] - pos4: [0, 0] + pos1: + positions: [0.785, 0.785] + pos2: + positions: [0.0, 0.0] + pos3: + positions: [-0.785, -0.785] + pos4: + positions: [0.0, 0.0] joints: - joint1 diff --git a/ros2_control_demo_bringup/launch/multi_controller_manager_example_two_rrbots.launch.py b/ros2_control_demo_bringup/launch/multi_controller_manager_example_two_rrbots.launch.py index 8f1affb5f..8a48ec3a6 100644 --- a/ros2_control_demo_bringup/launch/multi_controller_manager_example_two_rrbots.launch.py +++ b/ros2_control_demo_bringup/launch/multi_controller_manager_example_two_rrbots.launch.py @@ -81,7 +81,7 @@ def generate_launch_description(): "position_trajectory_controller", "-c", "/rrbot_1/controller_manager", - "--stopped", + "--inactive", ], ) @@ -108,7 +108,7 @@ def generate_launch_description(): "position_trajectory_controller", "-c", "/rrbot_2/controller_manager", - "--stopped", + "--inactive", ], ) diff --git a/ros2_control_demo_bringup/launch/rrbot_namespace.launch.py b/ros2_control_demo_bringup/launch/rrbot_namespace.launch.py index b8716a6ae..91c4dad89 100644 --- a/ros2_control_demo_bringup/launch/rrbot_namespace.launch.py +++ b/ros2_control_demo_bringup/launch/rrbot_namespace.launch.py @@ -100,7 +100,7 @@ def generate_launch_description(): "position_trajectory_controller", "-c", "/rrbot/controller_manager", - "--stopped", + "--inactive", ], ) diff --git a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py b/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py index 226ba0772..03b9d5d79 100644 --- a/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py +++ b/ros2_control_demo_bringup/launch/test_forward_position_controller.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", parameters=[position_goals], diff --git a/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py b/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py index 8c66c88c0..cddcbcb59 100644 --- a/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py +++ b/ros2_control_demo_bringup/launch/test_joint_trajectory_controller.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", parameters=[position_goals],