diff --git a/doc/index.rst b/doc/index.rst index 0e992c1f..ea6a3278 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -162,6 +162,22 @@ 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 b76be6bb..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 @@ -23,10 +25,12 @@ #ifdef GZ_HEADERS #include +#include #include #include #include +#include #include #include #include @@ -48,10 +52,12 @@ #define GZ_VECTOR_DOT dot #else #include +#include #include #include #include +#include #include #include #include @@ -122,6 +128,35 @@ struct MimicJoint std::vector interfaces_to_mimic; }; +class ForceTorqueData +{ +public: + /// \brief force torque sensor's name. + std::string name{}; + + /// \brief force torque sensor'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 Wrench & _msg); +}; + +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: @@ -170,9 +205,12 @@ 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 std::vector state_interfaces_; @@ -521,6 +559,55 @@ 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 @@ -639,6 +726,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; } 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..283322d5 --- /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" 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..e46c5052 --- /dev/null +++ b/gz_ros2_control_demos/launch/cart_example_ft_sensor.launch.py @@ -0,0 +1,140 @@ +# 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. +# 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', + ] + ) + + 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', + 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 4 ', gazebo_world])]), + 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/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 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..1b4bdf67 --- /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 + + + 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..6e981b28 --- /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 + + + + + + + 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..8e3ae5a9 --- /dev/null +++ b/gz_ros2_control_tests/tests/ft_sensor_test.py @@ -0,0 +1,116 @@ +#!/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])