From daa2fda94473d119b29ef909817d54dffd568a94 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 13 Aug 2024 16:25:09 +0000 Subject: [PATCH 1/2] Add namespaced example --- .../cart_controller_position_namespaced.yaml | 19 ++++ ...cart_example_position_namespaced.launch.py | 91 +++++++++++++++++++ .../test_cart_position_namespaced.xacro.urdf | 72 +++++++++++++++ 3 files changed, 182 insertions(+) create mode 100644 gazebo_ros2_control_demos/config/cart_controller_position_namespaced.yaml create mode 100644 gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py create mode 100644 gazebo_ros2_control_demos/urdf/test_cart_position_namespaced.xacro.urdf diff --git a/gazebo_ros2_control_demos/config/cart_controller_position_namespaced.yaml b/gazebo_ros2_control_demos/config/cart_controller_position_namespaced.yaml new file mode 100644 index 00000000..029144d0 --- /dev/null +++ b/gazebo_ros2_control_demos/config/cart_controller_position_namespaced.yaml @@ -0,0 +1,19 @@ +/my_ns/controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +/my_ns/joint_trajectory_controller: + ros__parameters: + joints: + - slider_to_cart + command_interfaces: + - position + state_interfaces: + - position + - velocity diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py new file mode 100644 index 00000000..5d5d5a92 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py @@ -0,0 +1,91 @@ +# Copyright 2020 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']), + ) + + gazebo_ros2_control_demos_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos')) + + xacro_file = os.path.join(gazebo_ros2_control_demos_path, + 'urdf', + 'test_cart_position_namespaced.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace="my_ns", + output='screen', + parameters=[params] + ) + + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + namespace="my_ns", + arguments=['-topic', 'robot_description', + '-entity', 'cart'], + output='screen') + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '-c', '/my_ns/controller_manager', + 'joint_state_broadcaster'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '-c', '/my_ns/controller_manager', + 'joint_trajectory_controller'], + output='screen' + ) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_trajectory_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position_namespaced.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position_namespaced.xacro.urdf new file mode 100644 index 00000000..4aab59ac --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_cart_position_namespaced.xacro.urdf @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + -15 + 15 + + + 1.0 + + + + + + + + + + my_ns + + $(find gazebo_ros2_control_demos)/config/cart_controller_position_namespaced.yaml + + + From 0fb3bc14fdfabb2c1bd41e25d0f8d1a2e1bc31f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 19 Aug 2024 09:46:34 +0200 Subject: [PATCH 2/2] Update copyright year MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- .../launch/cart_example_position_namespaced.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py b/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py index 5d5d5a92..7204baad 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_position_namespaced.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 Open Source Robotics Foundation, Inc. +# Copyright 2024 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.