diff --git a/doc/index.rst b/doc/index.rst index b1f1e61d..5c3c48f1 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -276,6 +276,18 @@ When the Gazebo world is launched you can run some of the following commands to ros2 run ign_ros2_control_demos example_tricycle_drive ros2 run ign_ros2_control_demos example_ackermann_drive +To demonstrate the setup of a namespaced robot, run + +.. code-block:: shell + + ros2 launch ign_ros2_control_demos diff_drive_example_namespaced.launch.py + +which will launch a diff drive robot within the namespace ``r1``. + +.. note:: + + The ros2_control settings for the controller_manager and the controller defined in ``diff_drive_controller.yaml`` use wildcards to match all namespaces. + Gripper ----------------------------------------------------------- diff --git a/ign_ros2_control_demos/config/diff_drive_controller_velocity.yaml b/ign_ros2_control_demos/config/diff_drive_controller.yaml similarity index 96% rename from ign_ros2_control_demos/config/diff_drive_controller_velocity.yaml rename to ign_ros2_control_demos/config/diff_drive_controller.yaml index 6f3ddab1..1e40e771 100644 --- a/ign_ros2_control_demos/config/diff_drive_controller_velocity.yaml +++ b/ign_ros2_control_demos/config/diff_drive_controller.yaml @@ -1,4 +1,4 @@ -controller_manager: +/**/controller_manager: ros__parameters: update_rate: 100 # Hz @@ -8,7 +8,7 @@ controller_manager: diff_drive_base_controller: type: diff_drive_controller/DiffDriveController -diff_drive_base_controller: +/**/diff_drive_base_controller: ros__parameters: left_wheel_names: ["left_wheel_joint"] right_wheel_names: ["right_wheel_joint"] diff --git a/ign_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py b/ign_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py new file mode 100644 index 00000000..036e0f91 --- /dev/null +++ b/ign_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py @@ -0,0 +1,116 @@ +# 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. +# 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 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) + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution( + [FindPackageShare('ign_ros2_control_demos'), + 'urdf', 'test_diff_drive.xacro.urdf'] + ), + ' ', + 'namespace:=r1', + ] + ) + robot_description = {'robot_description': robot_description_content} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace='r1', + output='screen', + parameters=[robot_description, {'use_sim_time': use_sim_time}], + ) + + ignition_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + namespace='r1', + output='screen', + arguments=['-topic', 'robot_description', + '-name', 'diff_drive', + '-allow_renaming', 'true'], + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_state_broadcaster', + '-c', '/r1/controller_manager' + ], + ) + diff_drive_base_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'diff_drive_base_controller', + '-c', '/r1/controller_manager' + ], + ) + + # Bridge + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + output='screen' + ) + + return LaunchDescription([ + bridge, + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_ign_gazebo'), + 'launch', 'ign_gazebo.launch.py')]), + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[ + joint_state_broadcaster_spawner, + diff_drive_base_controller_spawner + ], + ) + ), + node_robot_state_publisher, + ignition_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index 57b8169d..d7f44fef 100644 --- a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -1,5 +1,8 @@ + + + @@ -167,7 +170,10 @@ - $(find ign_ros2_control_demos)/config/diff_drive_controller_velocity.yaml + + $(arg namespace) + + $(find ign_ros2_control_demos)/config/diff_drive_controller.yaml