diff --git a/doc/index.rst b/doc/index.rst index 0a8cc1b8..8c586115 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -158,7 +158,7 @@ URDF: .. code-block:: xml - + $(find gz_ros2_control_demos)/config/cart_controller.yaml @@ -167,7 +167,7 @@ SDF: .. code-block:: xml - + $(find gz_ros2_control_demos)/config/cart_controller.yaml @@ -197,7 +197,7 @@ URDF: .. code-block:: xml - + ... my_namespace @@ -210,7 +210,7 @@ SDF: .. code-block:: xml - + ... my_namespace @@ -251,7 +251,7 @@ URDF: ... - + ... @@ -266,7 +266,7 @@ SDF: ... - + ... diff --git a/gz_ros2_control_demos/config/diff_drive_controller.yaml b/gz_ros2_control_demos/config/diff_drive_controller.yaml index 02446dff..ebb160bb 100644 --- a/gz_ros2_control_demos/config/diff_drive_controller.yaml +++ b/gz_ros2_control_demos/config/diff_drive_controller.yaml @@ -5,14 +5,13 @@ joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster -/**/diff_drive_base_controller: +/**/diff_drive_controller: ros__parameters: type: diff_drive_controller/DiffDriveController left_wheel_names: ["left_wheel_joint"] right_wheel_names: ["right_wheel_joint"] wheel_separation: 1.25 - #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal wheel_radius: 0.3 wheel_separation_multiplier: 1.0 @@ -29,8 +28,8 @@ enable_odom_tf: true cmd_vel_timeout: 0.5 - #publish_limited_velocity: true - #velocity_rolling_window_size: 10 + publish_limited_velocity: true + velocity_rolling_window_size: 10 # Velocity and acceleration limits # Whenever a min_* is unspecified, default to -max_* diff --git a/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml new file mode 100644 index 00000000..e48c37fb --- /dev/null +++ b/gz_ros2_control_demos/config/ros_gz_bridge_config.yaml @@ -0,0 +1,6 @@ +--- +- ros_topic_name: "/clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py index 942ce286..9a847f39 100644 --- a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py @@ -12,59 +12,48 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare +from ros_gz_bridge.actions import RosGzBridge +from ros_gz_sim.actions import GzServer def generate_launch_description(): + pkg_share = get_package_share_directory('gz_ros2_control_demos') + # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) def robot_state_publisher(context): - performed_description_format = LaunchConfiguration('description_format').perform(context) + description_format = LaunchConfiguration('description_format').perform(context) # Get URDF or SDF via xacro - robot_description_content = Command( + xacro_processed = Command( [ PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', PathJoinSubstitution([ - FindPackageShare('gz_ros2_control_demos'), - performed_description_format, - f'test_ackermann_drive.xacro.{performed_description_format}' + pkg_share, + description_format, + f'test_ackermann_drive.xacro.{description_format}' ]), ] ) - robot_description = {'robot_description': robot_description_content} node_robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', - parameters=[robot_description] + parameters=[{'robot_description': xacro_processed}] ) return [node_robot_state_publisher] - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare('gz_ros2_control_demos'), - 'config', - 'ackermann_drive_controller.yaml', - ] - ) - - gz_spawn_entity = Node( - package='ros_gz_sim', - executable='create', - output='screen', - arguments=['-topic', 'robot_description', '-name', - 'ackermann', '-allow_renaming', 'true'], - ) + robot_controllers = PathJoinSubstitution([ + pkg_share, + 'config', + 'ackermann_drive_controller.yaml' + ]) joint_state_broadcaster_spawner = Node( package='controller_manager', @@ -74,46 +63,51 @@ def robot_state_publisher(context): ackermann_steering_controller_spawner = Node( package='controller_manager', executable='spawner', - arguments=['ackermann_steering_controller', - '--param-file', - robot_controllers, - '--controller-ros-args', - '-r /ackermann_steering_controller/tf_odometry:=/tf', - '--controller-ros-args', - '-r /ackermann_steering_controller/reference:=/cmd_vel' - ], + arguments=[ + 'ackermann_steering_controller', + '--param-file', robot_controllers, + '--controller-ros-args', + '-r /ackermann_steering_controller/tf_odometry:=/tf', + '--controller-ros-args', + '-r /ackermann_steering_controller/reference:=/cmd_vel' + ], + ) + + # Launch just the Gazebo server as a composable node. + gz_server = GzServer( + world_sdf_file='empty.sdf', + container_name='ros_gz_container', + create_own_container='True', + use_composition='True', + ) + + gz_gui = ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen') + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', '-name', + 'diff_drive', '-allow_renaming', 'true'], ) - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], - output='screen' + # Setup ros_gz_bridge to bridge topics between ROS and Gazebo. + # It is launched as a composable node in the container created by the Gazebo server. + ros_gz_bridge = RosGzBridge( + bridge_name='ros_gz_bridge', + config_file=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']), + container_name='ros_gz_container', + create_own_container='False', + use_composition='True', ) ld = LaunchDescription([ - bridge, - # Launch gazebo environment - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), - 'launch', - 'gz_sim.launch.py'])]), - launch_arguments=[('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=[ackermann_steering_controller_spawner], - ) - ), + gz_server, + gz_gui, gz_spawn_entity, + ros_gz_bridge, + joint_state_broadcaster_spawner, + ackermann_steering_controller_spawner, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index a0b21269..b5d75ed4 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -12,66 +12,55 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare +from ros_gz_bridge.actions import RosGzBridge +from ros_gz_sim.actions import GzServer def generate_launch_description(): + pkg_share = get_package_share_directory('gz_ros2_control_demos') + # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) def robot_state_publisher(context): - performed_description_format = LaunchConfiguration('description_format').perform(context) + description_format = LaunchConfiguration('description_format').perform(context) # Get URDF or SDF via xacro - robot_description_content = Command( + xacro_processed = Command( [ PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', PathJoinSubstitution([ - FindPackageShare('gz_ros2_control_demos'), - performed_description_format, - f'test_diff_drive.xacro.{performed_description_format}' + pkg_share, + description_format, + f'test_diff_drive.xacro.{description_format}' ]), ] ) - robot_description = {'robot_description': robot_description_content} node_robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', output='screen', - parameters=[robot_description] + parameters=[{'robot_description': xacro_processed}] ) return [node_robot_state_publisher] - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare('gz_ros2_control_demos'), - 'config', - 'diff_drive_controller.yaml', - ] - ) - - gz_spawn_entity = Node( - package='ros_gz_sim', - executable='create', - output='screen', - arguments=['-topic', 'robot_description', '-name', - 'diff_drive', '-allow_renaming', 'true'], - ) + robot_controllers = PathJoinSubstitution([ + pkg_share, + 'config', + 'diff_drive_controller.yaml' + ]) joint_state_broadcaster_spawner = Node( package='controller_manager', executable='spawner', arguments=['joint_state_broadcaster'], ) - diff_drive_base_controller_spawner = Node( + diff_drive_controller_spawner = Node( package='controller_manager', executable='spawner', arguments=[ @@ -83,36 +72,41 @@ def robot_state_publisher(context): ], ) - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], - output='screen' + # Launch just the Gazebo server as a composable node. + gz_server = GzServer( + world_sdf_file='empty.sdf', + container_name='ros_gz_container', + create_own_container='True', + use_composition='True', + ) + + gz_gui = ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen') + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', '-name', + 'diff_drive', '-allow_renaming', 'true'], + ) + + # Setup ros_gz_bridge to bridge topics between ROS and Gazebo. + # It is launched as a composable node in the container created by the Gazebo server. + ros_gz_bridge = RosGzBridge( + bridge_name='ros_gz_bridge', + config_file=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']), + container_name='ros_gz_container', + create_own_container='False', + use_composition='True', ) ld = LaunchDescription([ - # Launch gazebo environment - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), - 'launch', - 'gz_sim.launch.py'])]), - launch_arguments=[('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=[diff_drive_base_controller_spawner], - ) - ), - bridge, + gz_server, + gz_gui, gz_spawn_entity, + ros_gz_bridge, + joint_state_broadcaster_spawner, + diff_drive_controller_spawner, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', @@ -120,7 +114,7 @@ def robot_state_publisher(context): description='If true, use simulated clock'), DeclareLaunchArgument( 'description_format', - default_value='urdf', + default_value='sdf', description='Robot description format to use, urdf or sdf'), ]) ld.add_action(OpaqueFunction(function=robot_state_publisher)) diff --git a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py index 7ce742be..6d1058df 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example_namespaced.launch.py @@ -12,59 +12,51 @@ # See the License for the specific language governing permissions and # limitations under the License. +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.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare +from ros_gz_bridge.actions import RosGzBridge +from ros_gz_sim.actions import GzServer def generate_launch_description(): + pkg_share = get_package_share_directory('gz_ros2_control_demos') + # 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('gz_ros2_control_demos'), - 'urdf', 'test_diff_drive.xacro.urdf'] - ), - ' ', - 'namespace:=r1', - ] - ) - robot_description = {'robot_description': robot_description_content} - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare('gz_ros2_control_demos'), - 'config', - 'diff_drive_controller.yaml', - ] - ) - - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - namespace='r1', - output='screen', - parameters=[robot_description] - ) + def robot_state_publisher(context): + description_format = LaunchConfiguration('description_format').perform(context) + # Get URDF or SDF via xacro + xacro_processed = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + pkg_share, + description_format, + f'test_diff_drive.xacro.{description_format}' + ]), + ' ', + 'namespace:=r1', + ] + ) + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + namespace='r1', + output='screen', + parameters=[{'robot_description': xacro_processed}] + ) + return [node_robot_state_publisher] - gz_spawn_entity = Node( - package='ros_gz_sim', - executable='create', - namespace='r1', - output='screen', - arguments=['-topic', 'robot_description', '-name', - 'diff_drive', '-allow_renaming', 'true'], - ) + robot_controllers = PathJoinSubstitution([ + pkg_share, + 'config', + 'diff_drive_controller.yaml' + ]) joint_state_broadcaster_spawner = Node( package='controller_manager', @@ -76,51 +68,62 @@ def generate_launch_description(): '-r /r1/diff_drive_controller/cmd_vel:=/r1/cmd_vel', ], ) - diff_drive_base_controller_spawner = Node( + diff_drive_controller_spawner = Node( package='controller_manager', executable='spawner', arguments=[ - 'diff_drive_base_controller', + 'diff_drive_controller', '--param-file', robot_controllers, '-c', '/r1/controller_manager' - ], + ], ) - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'], - output='screen' + # Launch just the Gazebo server as a composable node. + gz_server = GzServer( + world_sdf_file='empty.sdf', + container_name='ros_gz_container', + create_own_container='True', + use_composition='True', ) - return LaunchDescription([ - # Launch gazebo environment - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), - 'launch', - 'gz_sim.launch.py'])]), - launch_arguments=[('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=[diff_drive_base_controller_spawner], - ) - ), - bridge, - node_robot_state_publisher, + gz_gui = ExecuteProcess(cmd=['gz', 'sim', '-g'], output='screen') + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + namespace='r1', + output='screen', + arguments=['-topic', 'robot_description', '-name', + 'diff_drive', '-allow_renaming', 'true'], + ) + + # Setup ros_gz_bridge to bridge topics between ROS and Gazebo. + # It is launched as a composable node in the container created by the Gazebo server. + ros_gz_bridge = RosGzBridge( + bridge_name='ros_gz_bridge', + config_file=PathJoinSubstitution([pkg_share, 'config', 'ros_gz_bridge_config.yaml']), + container_name='ros_gz_container', + create_own_container='False', + use_composition='True', + ) + + ld = LaunchDescription([ + gz_server, + gz_gui, gz_spawn_entity, + ros_gz_bridge, + joint_state_broadcaster_spawner, + diff_drive_controller_spawner, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', default_value=use_sim_time, description='If true, use simulated clock'), + DeclareLaunchArgument( + 'description_format', + default_value='sdf', + description='Robot description format to use, urdf or sdf'), ]) + ld.add_action(OpaqueFunction(function=robot_state_publisher)) + return ld diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index d1eb44f7..ce64be28 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -20,6 +20,8 @@ geometry_msgs rclcpp rclcpp_action + ros_gz_bridge + ros_gz_sim std_msgs ament_index_python diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf index 0e7f2bad..f836b88a 100644 --- a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -1,6 +1,9 @@ + + + true @@ -212,7 +215,10 @@ - $(find gz_ros2_control_demos)/config/diff_drive_controller_velocity.yaml + + $(arg namespace) + + $(find gz_ros2_control_demos)/config/diff_drive_controller.yaml