From 5377b7da9c06bb5e3baa2e8335fab23a0b48f0a6 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Mon, 8 Apr 2024 20:59:49 -0400 Subject: [PATCH 01/14] started adding FTS info --- gz_ros2_control/src/gz_system.cpp | 77 +++++++++++++++++++++++++++++++ 1 file changed, 77 insertions(+) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index b76be6bb..52695f93 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -52,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -122,6 +124,25 @@ struct MimicJoint std::vector interfaces_to_mimic; }; +class ForceTorqueData +{ +public: + /// \brief imu's name. + std::string name{}; + + /// \brief imu's topic name. + std::string topicName{}; + + /// \brief handles to the force torque from within Gazebo + sim::Entity sim_ft_sensors_ = sim::kNullEntity; + + /// \brief An array per FT + std::array ft_sensor_data_; + + /// \brief callback to get the Force Torque topic values + //void OnForceTorque(const GZ_MSGS_NAMESPACE IMU & _msg); // TODO +}; + class ImuData { public: @@ -173,6 +194,8 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief vector with the imus . std::vector> imus_; + std::vector> ft_sensors_; + /// \brief state interfaces that will be exported to the Resource Manager std::vector state_interfaces_; @@ -461,8 +484,12 @@ void GazeboSimSystem::registerSensors( size_t n_sensors = hardware_info.sensors.size(); std::vector sensor_components_; + RCLCPP_WARN(this->nh_->get_logger(), "======================> RegisterSensors"); + RCLCPP_WARN(this->nh_->get_logger(), "======================> n_sensors: %lu", n_sensors); + for (unsigned int j = 0; j < n_sensors; j++) { hardware_interface::ComponentInfo component = hardware_info.sensors[j]; + RCLCPP_WARN(this->nh_->get_logger(), "======================> Sensor name: %s", hardware_info.sensors[j].name.c_str()); sensor_components_.push_back(component); } // This is split in two steps: Count the number and type of sensor and associate the interfaces @@ -521,6 +548,56 @@ void GazeboSimSystem::registerSensors( this->dataPtr->imus_.push_back(imuData); return true; }); + + this->dataPtr->ecm->Each( + [&](const sim::Entity & _entity, + const sim::components::ForceTorque *, + const sim::components::Name * _name) -> bool + { + auto ftData = std::make_shared(); + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(_entity); + if (sensorTopicComp) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\tState:"); + ftData->name = _name->Data(); + ftData->sim_ft_sensors_ = _entity; + + hardware_interface::ComponentInfo component; + for (auto & comp : sensor_components_) { + if (comp.name == _name->Data()) { + component = comp; + } + } + + static const std::map interface_name_map = { + {"force.x", 0}, + {"force.y", 1}, + {"force.z", 2}, + {"torque.x", 3}, + {"torque.y", 4}, + {"torque.z", 5}, + }; + + for (const auto & state_interface : component.state_interfaces) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); + + size_t data_index = interface_name_map.at(state_interface.name); + this->dataPtr->state_interfaces_.emplace_back( + ftData->name, + state_interface.name, + &ftData->ft_sensor_data_[data_index]); + } + this->dataPtr->ft_sensors_.push_back(ftData); + return true; + }); + } CallbackReturn From 7bcf0efa79f1a2de43c3bc8131410ac30292a231 Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 09:44:06 -0400 Subject: [PATCH 02/14] Providing force torque sensor data to controller_manager --- gz_ros2_control/src/gz_system.cpp | 37 ++++++++++++++++++++++++++----- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 52695f93..fd5e9682 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -23,6 +23,7 @@ #ifdef GZ_HEADERS #include +#include #include #include @@ -49,6 +50,7 @@ #define GZ_VECTOR_DOT dot #else #include +#include #include #include @@ -140,9 +142,19 @@ class ForceTorqueData std::array ft_sensor_data_; /// \brief callback to get the Force Torque topic values - //void OnForceTorque(const GZ_MSGS_NAMESPACE IMU & _msg); // TODO + void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); // TODO }; +void ForceTorqueData::OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg) +{ + this->ft_sensor_data_[0] = _msg.force().x(); + this->ft_sensor_data_[1] = _msg.force().y(); + this->ft_sensor_data_[2] = _msg.force().z(); + this->ft_sensor_data_[3] = _msg.torque().x(); + this->ft_sensor_data_[4] = _msg.torque().y(); + this->ft_sensor_data_[5] = _msg.torque().z(); +} + class ImuData { public: @@ -484,12 +496,8 @@ void GazeboSimSystem::registerSensors( size_t n_sensors = hardware_info.sensors.size(); std::vector sensor_components_; - RCLCPP_WARN(this->nh_->get_logger(), "======================> RegisterSensors"); - RCLCPP_WARN(this->nh_->get_logger(), "======================> n_sensors: %lu", n_sensors); - for (unsigned int j = 0; j < n_sensors; j++) { hardware_interface::ComponentInfo component = hardware_info.sensors[j]; - RCLCPP_WARN(this->nh_->get_logger(), "======================> Sensor name: %s", hardware_info.sensors[j].name.c_str()); sensor_components_.push_back(component); } // This is split in two steps: Count the number and type of sensor and associate the interfaces @@ -597,7 +605,6 @@ void GazeboSimSystem::registerSensors( this->dataPtr->ft_sensors_.push_back(ftData); return true; }); - } CallbackReturn @@ -716,6 +723,24 @@ hardware_interface::return_type GazeboSimSystem::read( } } } + + for (unsigned int i = 0; i < this->dataPtr->ft_sensors_.size(); ++i) { + if (this->dataPtr->ft_sensors_[i]->topicName.empty()) { + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(this->dataPtr->ft_sensors_[i]->sim_ft_sensors_); + if (sensorTopicComp) { + this->dataPtr->ft_sensors_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + + this->dataPtr->node.Subscribe( + this->dataPtr->ft_sensors_[i]->topicName, &ForceTorqueData::OnForceTorque, + this->dataPtr->ft_sensors_[i].get()); + } + } + } + return hardware_interface::return_type::OK; } From 934d8b7dc06dd7fafb53849e11b034efa94eedcc Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 16:45:30 -0400 Subject: [PATCH 03/14] Removed old TODO that was completed --- gz_ros2_control/src/gz_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index fd5e9682..94ee9d48 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -142,7 +142,7 @@ class ForceTorqueData std::array ft_sensor_data_; /// \brief callback to get the Force Torque topic values - void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); // TODO + void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg); }; void ForceTorqueData::OnForceTorque(const GZ_MSGS_NAMESPACE Wrench & _msg) From ae6740e47fd9b33e33985a48d4f6537b26a092dd Mon Sep 17 00:00:00 2001 From: Kevin DeMarco Date: Tue, 9 Apr 2024 16:54:32 -0400 Subject: [PATCH 04/14] Indent spaces fix --- gz_ros2_control/src/gz_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 94ee9d48..63152e1f 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -573,7 +573,7 @@ void GazeboSimSystem::registerSensors( } RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "\tState:"); + this->nh_->get_logger(), "\tState:"); ftData->name = _name->Data(); ftData->sim_ft_sensors_ = _entity; @@ -731,7 +731,7 @@ hardware_interface::return_type GazeboSimSystem::read( if (sensorTopicComp) { this->dataPtr->ft_sensors_[i]->topicName = sensorTopicComp->Data(); RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << + this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << " has a topic name: " << sensorTopicComp->Data()); this->dataPtr->node.Subscribe( From 0eb49dfa9fae564a85094a0373ddcf2c08e9a033 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 14:09:09 +0200 Subject: [PATCH 05/14] Added demo, test and docs for FT sensor for Humble --- doc/index.rst | 17 +++ gz_ros2_control/src/gz_system.cpp | 9 +- gz_ros2_control_demos/CMakeLists.txt | 1 + .../config/cart_controller_ft_sensor.yaml | 23 +++ .../launch/cart_example_ft_sensor.launch.py | 133 ++++++++++++++++++ .../urdf/test_cart_ft_sensor.xacro.urdf | 114 +++++++++++++++ .../worlds/empty_ft_sensor.sdf | 131 +++++++++++++++++ gz_ros2_control_tests/tests/CMakeLists.txt | 4 + gz_ros2_control_tests/tests/ft_sensor_test.py | 117 +++++++++++++++ 9 files changed, 546 insertions(+), 3 deletions(-) create mode 100644 gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml create mode 100644 gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py create mode 100644 gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf create mode 100644 gz_ros2_control_demos/worlds/empty_ft_sensor.sdf create mode 100644 gz_ros2_control_tests/tests/ft_sensor_test.py diff --git a/doc/index.rst b/doc/index.rst index 0e992c1f..3fcae99d 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -162,6 +162,23 @@ We should include: +Using force-torque sensors in simulation +----------------------------------------------------------- + +To use ``force-torque`` sensors in *gz_ros2_control* you should define its parameters in your URDF or SDF (see the `SDF specification `__) + +.. code-block:: xml + + + 10.0 + true + true + force_torque_sensor + + +It is important to add this as ``reference`` sensor in the ```` tag in your URDF file. + + Add the gz_ros2_control plugin ========================================== diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 63152e1f..2caea666 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -14,6 +14,8 @@ #include "gz_ros2_control/gz_system.hpp" +#include +#include #include #include #include @@ -129,10 +131,10 @@ struct MimicJoint class ForceTorqueData { public: - /// \brief imu's name. + /// \brief force torque sensor's name. std::string name{}; - /// \brief imu's topic name. + /// \brief force torque sensor's topic name. std::string topicName{}; /// \brief handles to the force torque from within Gazebo @@ -203,9 +205,10 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief vector with the joint's names. std::vector joints_; - /// \brief vector with the imus . + /// \brief vector with the imus. std::vector> imus_; + /// \brief vector with the force torque sensors. std::vector> ft_sensors_; /// \brief state interfaces that will be exported to the Resource Manager diff --git a/gz_ros2_control_demos/CMakeLists.txt b/gz_ros2_control_demos/CMakeLists.txt index f73d883e..19377ad8 100644 --- a/gz_ros2_control_demos/CMakeLists.txt +++ b/gz_ros2_control_demos/CMakeLists.txt @@ -26,6 +26,7 @@ install(DIRECTORY launch config urdf + worlds DESTINATION share/${PROJECT_NAME}/ ) diff --git a/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml b/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml new file mode 100644 index 00000000..b98e6fed --- /dev/null +++ b/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml @@ -0,0 +1,23 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +joint_trajectory_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - slider_to_cart + command_interfaces: + - position + state_interfaces: + - position + - velocity + +force_torque_sensor_broadcaster: + ros__parameters: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + sensor_name: "force_torque_sensor" + frame_id: "slider_to_cart" \ No newline at end of file diff --git a/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py new file mode 100644 index 00000000..c122f8f9 --- /dev/null +++ b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py @@ -0,0 +1,133 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + gz_args = LaunchConfiguration('gz_args', default='') + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution( + [FindPackageShare('gz_ros2_control_demos'), + 'urdf', 'test_cart_ft_sensor.xacro.urdf'] + ), + ] + ) + robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'cart_controller_ft_sensor.yaml', + ] + ) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', + '-name', 'cart', '-allow_renaming', 'true'], + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster', + ], + ) + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_controller', + '--param-file', + robot_controllers, + ], + ) + force_torque_sensor_broadcaster = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'force_torque_sensor_broadcaster', + '--param-file', + robot_controllers, + ], + ) + + # Bridge + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], + output='screen' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py'])]), + launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawn_entity, + on_exit=[joint_state_broadcaster_spawner], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_trajectory_controller_spawner, + on_exit=[force_torque_sensor_broadcaster], + ) + ), + bridge, + node_robot_state_publisher, + gz_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf b/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf new file mode 100644 index 00000000..18e49a50 --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -15 + 15 + + + 1.0 + + + + + + + + + + + + + + + + + + + 0 0.8 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + + + + + 0 0 0.8 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + + + + 10.0 + true + true + force_torque_sensor + + + + + + $(find gz_ros2_control_demos)/config/cart_controller_position.yaml + + + \ No newline at end of file diff --git a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf new file mode 100644 index 00000000..90397c42 --- /dev/null +++ b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf @@ -0,0 +1,131 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + \ No newline at end of file diff --git a/gz_ros2_control_tests/tests/CMakeLists.txt b/gz_ros2_control_tests/tests/CMakeLists.txt index 58b2fa3a..476047fe 100644 --- a/gz_ros2_control_tests/tests/CMakeLists.txt +++ b/gz_ros2_control_tests/tests/CMakeLists.txt @@ -15,3 +15,7 @@ add_launch_test(velocity_test.py add_launch_test(effort_test.py TIMEOUT 50 ) + +add_launch_test(ft_sensor_test.py + TIMEOUT 50 +) diff --git a/gz_ros2_control_tests/tests/ft_sensor_test.py b/gz_ros2_control_tests/tests/ft_sensor_test.py new file mode 100644 index 00000000..18452936 --- /dev/null +++ b/gz_ros2_control_tests/tests/ft_sensor_test.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python3 +# Copyright 2025 ros2_control Maintainers +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python.packages import get_package_share_directory +from controller_manager.test_utils import ( + check_controllers_running, + check_if_js_published, + check_node_running +) +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +import launch_testing +from launch_testing.actions import ReadyToTest +from launch_testing.util import KeepAliveProc +from launch_testing_ros import WaitForTopics +import psutil +import pytest +import rclpy +from rosgraph_msgs.msg import Clock + + +# This function specifies the processes to be run for our test +@pytest.mark.rostest +def generate_test_description(): + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env['PYTHONUNBUFFERED'] = '1' + launch_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory('gz_ros2_control_demos'), + 'launch/cart_example_ft_sensor.launch.py', + ) + ), + launch_arguments={'gz_args': '--headless-rendering -s'}.items(), + ) + + return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) + + +class TestFixture(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + for proc in psutil.process_iter(): + # check whether the process name matches + if proc.name() == 'ruby': + proc.kill() + if 'gz sim' in proc.name(): + proc.kill() + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_node') + + def tearDown(self): + self.node.destroy_node() + + def test_node_start(self, proc_output): + check_node_running(self.node, 'robot_state_publisher') + + def test_clock(self): + topic_list = [('/clock', Clock)] + with WaitForTopics(topic_list, timeout=10.0): + print('/clock is receiving messages!') + + def test_check_if_msgs_published(self): + check_if_js_published( + '/joint_states', + [ + 'slider_to_cart', + ], + ) + + def test_arm(self, launch_service, proc_info, proc_output): + + # Check if the controllers are running + cnames = [ + 'joint_trajectory_controller', + 'joint_state_broadcaster', + 'force_torque_sensor_broadcaster' + ] + check_controllers_running(self.node, cnames) + + proc_action = Node( + package='gz_ros2_control_demos', + executable='example_position', + output='screen', + ) + + with launch_testing.tools.launch_process( + launch_service, proc_action, proc_info, proc_output + ): + proc_info.assertWaitForShutdown(process=proc_action, timeout=300) + launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, + allowable_exit_codes=[0]) From ba69fd1cc66ff088a47c7500efc74aa77b8fa447 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 14:59:35 +0200 Subject: [PATCH 06/14] Fixed pre-commit issues --- gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml | 2 +- gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf | 2 +- gz_ros2_control_demos/worlds/empty_ft_sensor.sdf | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml b/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml index b98e6fed..283322d5 100644 --- a/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml +++ b/gz_ros2_control_demos/config/cart_controller_ft_sensor.yaml @@ -20,4 +20,4 @@ force_torque_sensor_broadcaster: ros__parameters: type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster sensor_name: "force_torque_sensor" - frame_id: "slider_to_cart" \ No newline at end of file + frame_id: "slider_to_cart" diff --git a/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf b/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf index 18e49a50..1b4bdf67 100644 --- a/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_cart_ft_sensor.xacro.urdf @@ -111,4 +111,4 @@ $(find gz_ros2_control_demos)/config/cart_controller_position.yaml - \ No newline at end of file + diff --git a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf index 90397c42..33b75d3c 100644 --- a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf +++ b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf @@ -128,4 +128,4 @@ ign service -s /world/empty/create \ - \ No newline at end of file + From 81e07abf5b14253958edc1533f051fa48e524c0c Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 16:31:12 +0200 Subject: [PATCH 07/14] Changed world in launch file --- .../launch/cart_example_ft_sensor.launch.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py index c122f8f9..ecebf018 100644 --- a/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py @@ -48,6 +48,14 @@ def generate_launch_description(): ] ) + gazebo_world = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'worlds', + 'empty_ft_sensor.sdf', + ] + ) + node_robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -103,7 +111,7 @@ def generate_launch_description(): [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])]), - launch_arguments=[('gz_args', [gz_args, ' -r -v 1 empty.sdf'])]), + launch_arguments=[('gz_args', [gz_args, ' -r -v 1 ', gazebo_world])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, From 7e0c7fda218e00f6e0201a9affa8ab495128f60b Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 17:17:20 +0200 Subject: [PATCH 08/14] Fixed error in ft world --- gz_ros2_control_demos/worlds/empty_ft_sensor.sdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf index 33b75d3c..aa83053a 100644 --- a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf +++ b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf @@ -81,9 +81,9 @@ ign service -s /world/empty/create \ filename="ignition-gazebo-contact-system" name="gz::sim::systems::Contact"> - + name="ignition::gazebo::systems::ForceTorque"> From 5a82f069009d22d6fd63509c7f5b6734a38a1ab1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 17:27:05 +0200 Subject: [PATCH 09/14] Added build for force-torque sensor broadcaster to CI --- .github/workflows/ci-humble.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci-humble.yaml b/.github/workflows/ci-humble.yaml index c9d9eacf..02a6bc45 100644 --- a/.github/workflows/ci-humble.yaml +++ b/.github/workflows/ci-humble.yaml @@ -72,6 +72,7 @@ jobs: id: build run: | . /opt/ros/humble/local_setup.sh + colcon build --packages-select force_torque_sensor_broadcaster colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests - name: Run tests id: test From b73f7a0681ab087e26365a909c7820ec945bf973 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 17:31:47 +0200 Subject: [PATCH 10/14] Changed order of building packages --- .github/workflows/ci-humble.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci-humble.yaml b/.github/workflows/ci-humble.yaml index 02a6bc45..28858897 100644 --- a/.github/workflows/ci-humble.yaml +++ b/.github/workflows/ci-humble.yaml @@ -72,8 +72,8 @@ jobs: id: build run: | . /opt/ros/humble/local_setup.sh - colcon build --packages-select force_torque_sensor_broadcaster colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests + colcon build --packages-select force_torque_sensor_broadcaster - name: Run tests id: test run: | From 68df047f7c70fcff8f18f16e9a6b69b15469ac3d Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 18:32:34 +0200 Subject: [PATCH 11/14] Addef force-torque sensor to package.xml --- gz_ros2_control_demos/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index 1d346857..f52981a1 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -40,6 +40,7 @@ control_msgs diff_drive_controller effort_controllers + force_torque_sensor_broadcaster gz_ros2_control hardware_interface imu_sensor_broadcaster From 290a0039daba0501be01ff637e8e16b31884ea4e Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 17 Jul 2025 18:36:57 +0200 Subject: [PATCH 12/14] Fixed pre-commit in sdf world --- gz_ros2_control_demos/worlds/empty_ft_sensor.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf index aa83053a..6e981b28 100644 --- a/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf +++ b/gz_ros2_control_demos/worlds/empty_ft_sensor.sdf @@ -81,7 +81,7 @@ ign service -s /world/empty/create \ filename="ignition-gazebo-contact-system" name="gz::sim::systems::Contact"> - From e64266c175f88c3efe3bf8d84194b91deb3f1364 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 18 Jul 2025 12:06:38 +0200 Subject: [PATCH 13/14] Removed force-torque sensor build from CI --- .github/workflows/ci-humble.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ci-humble.yaml b/.github/workflows/ci-humble.yaml index 28858897..c9d9eacf 100644 --- a/.github/workflows/ci-humble.yaml +++ b/.github/workflows/ci-humble.yaml @@ -73,7 +73,6 @@ jobs: run: | . /opt/ros/humble/local_setup.sh colcon build --packages-up-to gz_ros2_control_demos gz_ros2_control_tests - colcon build --packages-select force_torque_sensor_broadcaster - name: Run tests id: test run: | From 073cbc74533e011c64138b38ee4af4051c8eaa2c Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 30 Jul 2025 19:36:57 +0200 Subject: [PATCH 14/14] Fixed couple issues --- doc/index.rst | 3 +-- .../launch/cart_example_ft_sensor.launch.py | 7 +++---- gz_ros2_control_tests/tests/ft_sensor_test.py | 1 - 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 3fcae99d..ea6a3278 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -163,7 +163,7 @@ We should include: Using force-torque sensors in simulation ------------------------------------------------------------ +---------------------------------------- To use ``force-torque`` sensors in *gz_ros2_control* you should define its parameters in your URDF or SDF (see the `SDF specification `__) @@ -178,7 +178,6 @@ To use ``force-torque`` sensors in *gz_ros2_control* you should define its param It is important to add this as ``reference`` sensor in the ```` tag in your URDF file. - Add the gz_ros2_control plugin ========================================== diff --git a/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py index ecebf018..e46c5052 100644 --- a/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py @@ -1,4 +1,4 @@ -# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2025 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -74,8 +74,7 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package='controller_manager', executable='spawner', - arguments=['joint_state_broadcaster', - ], + arguments=['joint_state_broadcaster'], ) joint_trajectory_controller_spawner = Node( package='controller_manager', @@ -111,7 +110,7 @@ def generate_launch_description(): [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), 'launch', 'gz_sim.launch.py'])]), - launch_arguments=[('gz_args', [gz_args, ' -r -v 1 ', gazebo_world])]), + launch_arguments=[('gz_args', [gz_args, ' -r -v 4 ', gazebo_world])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, diff --git a/gz_ros2_control_tests/tests/ft_sensor_test.py b/gz_ros2_control_tests/tests/ft_sensor_test.py index 18452936..8e3ae5a9 100644 --- a/gz_ros2_control_tests/tests/ft_sensor_test.py +++ b/gz_ros2_control_tests/tests/ft_sensor_test.py @@ -94,7 +94,6 @@ def test_check_if_msgs_published(self): ) def test_arm(self, launch_service, proc_info, proc_output): - # Check if the controllers are running cnames = [ 'joint_trajectory_controller',