diff --git a/example_1/bringup/launch/test_forward_position_controller.launch.py b/example_1/bringup/launch/test_forward_position_controller.launch.py index 3605c3446..94870e697 100644 --- a/example_1/bringup/launch/test_forward_position_controller.launch.py +++ b/example_1/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_1"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_1")) + / "config" + / "rrbot_forward_position_publisher.yaml" + ], output="both", ) ] diff --git a/example_1/bringup/launch/test_joint_trajectory_controller.launch.py b/example_1/bringup/launch/test_joint_trajectory_controller.launch.py index 1ece95065..af1639c57 100644 --- a/example_1/bringup/launch/test_joint_trajectory_controller.launch.py +++ b/example_1/bringup/launch/test_joint_trajectory_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_1"), - "config", - "rrbot_joint_trajectory_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_1")) + / "config" + / "rrbot_joint_trajectory_publisher.yaml" + ], output="both", ) ] diff --git a/example_1/description/launch/view_robot.launch.py b/example_1/description/launch/view_robot.launch.py index bbb2ca2bf..b6a4793be 100644 --- a/example_1/description/launch/view_robot.launch.py +++ b/example_1/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_1"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_1")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_10/bringup/launch/rrbot.launch.py b/example_10/bringup/launch/rrbot.launch.py index b535123a1..50e63a622 100644 --- a/example_10/bringup/launch/rrbot.launch.py +++ b/example_10/bringup/launch/rrbot.launch.py @@ -14,99 +14,80 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - # Initialize Arguments - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_10"), - "urdf", - "rrbot.urdf.xacro", - ] + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_10")) + / "config" + / "rrbot_controllers.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_10")) + / "urdf" + / "rrbot.urdf.xacro", + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + ] + ) + } + ], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "gpio_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_10")) + / "config" + / "rrbot_controllers.yaml", + ], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_10")) + / "config" + / "rrbot_controllers.yaml", + ], ), - " ", - "use_mock_hardware:=", - use_mock_hardware, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_10"), - "config", - "rrbot_controllers.yaml", ] ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["forward_position_controller", "--param-file", robot_controllers], - ) - - gpio_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["gpio_controller", "--param-file", robot_controllers], - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - gpio_controller_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_10/bringup/launch/test_forward_position_controller.launch.py b/example_10/bringup/launch/test_forward_position_controller.launch.py index 3605c3446..99093f0c9 100644 --- a/example_10/bringup/launch/test_forward_position_controller.launch.py +++ b/example_10/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_1"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_1")) + / "config" + / "rrbot_forward_position_publisher.yaml", + ], output="both", ) ] diff --git a/example_10/description/launch/view_robot.launch.py b/example_10/description/launch/view_robot.launch.py index bc294c87f..8417de81a 100644 --- a/example_10/description/launch/view_robot.launch.py +++ b/example_10/description/launch/view_robot.launch.py @@ -14,86 +14,73 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_10"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_10")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py index b657bf7a0..7c0f11fad 100644 --- a/example_11/bringup/launch/carlikebot.launch.py +++ b/example_11/bringup/launch/carlikebot.launch.py @@ -13,134 +13,100 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition, UnlessCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "remap_odometry_tf", - default_value="false", - description="Remap odometry TF from the steering controller to the TF tree.", - ) - ) - - # Initialize Arguments - gui = LaunchConfiguration("gui") - remap_odometry_tf = LaunchConfiguration("remap_odometry_tf") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_11"), "urdf", "carlikebot.urdf.xacro"] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + DeclareLaunchArgument( + "remap_odometry_tf", + default_value="false", + description="Remap odometry TF from the steering controller to the TF tree.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_11")) + / "config" + / "carlikebot_controllers.yaml" + ], + output="both", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_11")) + / "urdf" + / "carlikebot.urdf.xacro", + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "carlikebot/rviz" + / "carlikebot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + # the steering controller libraries by default publish odometry on a separate topic than /tf + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "bicycle_steering_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_11")) + / "config" + / "carlikebot_controllers.yaml", + "--controller-ros-args", + "-r /bicycle_steering_controller/tf_odometry:=/tf", + ], + condition=IfCondition(LaunchConfiguration("remap_odometry_tf")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "bicycle_steering_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_11")) + / "config" + / "carlikebot_controllers.yaml", + ], + condition=UnlessCondition(LaunchConfiguration("remap_odometry_tf")), ), ] ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_11"), - "config", - "carlikebot_controllers.yaml", - ] - ) - rviz_config_file = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_description"), - "carlikebot/rviz", - "carlikebot.rviz", - ] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_bicycle_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - # the steering controller libraries by default publish odometry on a separate topic than /tf - robot_bicycle_controller_spawner_remapped = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "bicycle_steering_controller", - "--param-file", - robot_controllers, - "--controller-ros-args", - "-r /bicycle_steering_controller/tf_odometry:=/tf", - ], - condition=IfCondition(remap_odometry_tf), - ) - - robot_bicycle_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["bicycle_steering_controller", "--param-file", robot_controllers], - condition=UnlessCondition(remap_odometry_tf), - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_bicycle_controller_spawner_remapped, robot_bicycle_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_bicycle_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_11/description/launch/view_robot.launch.py b/example_11/description/launch/view_robot.launch.py index 50b1e9840..684e0f4ce 100644 --- a/example_11/description/launch/view_robot.launch.py +++ b/example_11/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="carlikebot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ), - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_11"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="carlikebot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_11")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "carlikebot/rviz" + / "carlikebot_view.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "carlikebot/rviz", "carlikebot_view.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_12/bringup/launch/launch_chained_controllers.launch.py b/example_12/bringup/launch/launch_chained_controllers.launch.py index 85de861ce..19e480822 100644 --- a/example_12/bringup/launch/launch_chained_controllers.launch.py +++ b/example_12/bringup/launch/launch_chained_controllers.launch.py @@ -14,48 +14,35 @@ from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - - robot_controllers = PathJoinSubstitution( + return LaunchDescription( [ - FindPackageShare("ros2_control_demo_example_12"), - "config", - "rrbot_chained_controllers.yaml", + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "config" + / "rrbot_chained_controllers.yaml", + ], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "config" + / "rrbot_chained_controllers.yaml", + ], + ), ] ) - - position_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["position_controller", "--param-file", robot_controllers], - ) - - forward_position_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["forward_position_controller", "--param-file", robot_controllers], - ) - - # Delay start of forward_position_controller_spawner after `position_controller_spawner` - delay_forward_position_controller_spawner_after_position_controller_spawner = ( - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=position_controller_spawner, - on_exit=[forward_position_controller_spawner], - ) - ) - ) - - nodes = [ - position_controller_spawner, - delay_forward_position_controller_spawner_after_position_controller_spawner, - ] - - return LaunchDescription(nodes) diff --git a/example_12/bringup/launch/rrbot.launch.py b/example_12/bringup/launch/rrbot.launch.py index b1524f963..12e6d5d50 100644 --- a/example_12/bringup/launch/rrbot.launch.py +++ b/example_12/bringup/launch/rrbot.launch.py @@ -14,117 +14,93 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_12"), - "urdf", - "rrbot.urdf.xacro", - ] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # controller manager node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "config" + / "rrbot_chained_controllers.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "urdf" + / "rrbot.urdf.xacro", + ] + ) + } + ], + ), + # RViz (conditional) + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + # Spawners + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint1_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "config" + / "rrbot_chained_controllers.yaml", + ], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint2_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "config" + / "rrbot_chained_controllers.yaml", + ], ), ] ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_12"), - "config", - "rrbot_chained_controllers.yaml", - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - j1_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint1_position_controller", "--param-file", robot_controllers], - ) - - j2_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint2_position_controller", "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[j1_controller_spawner, j2_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_12/description/launch/view_robot.launch.py b/example_12/description/launch/view_robot.launch.py index f61bf7335..dfce57b15 100644 --- a/example_12/description/launch/view_robot.launch.py +++ b/example_12/description/launch/view_robot.launch.py @@ -14,86 +14,73 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_12"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_12")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_13/bringup/launch/three_robots.launch.py b/example_13/bringup/launch/three_robots.launch.py index 39a3cfee3..e326ed04c 100644 --- a/example_13/bringup/launch/three_robots.launch.py +++ b/example_13/bringup/launch/three_robots.launch.py @@ -18,7 +18,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -26,156 +26,137 @@ def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", - default_value="50.0", - description="Slowdown factor of the RRbot.", - ) - ) - # Declare arguments - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - slowdown = LaunchConfiguration("slowdown") - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # controller manager node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_controllers.yaml", + ], + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "urdf" + / "three_robots.urdf.xacro", + " slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + # RViz (conditional) + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "rviz" + / "three_robots.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + # spawners: use helper that generates spawner launch descriptions + generate_controllers_spawner_launch_description( [ - FindPackageShare("ros2_control_demo_example_13"), - "urdf", - "three_robots.urdf.xacro", - ] + "joint_state_broadcaster", + "rrbot_joint_state_broadcaster", + "rrbot_position_controller", + "rrbot_external_fts_broadcaster", + ], + controller_params_files=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_controllers.yaml", + ], + ), + generate_controllers_spawner_launch_description( + ["rrbot_with_sensor_joint_state_broadcaster", "rrbot_with_sensor_fts_broadcaster"], + controller_params_files=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_controllers.yaml", + ], + ), + generate_controllers_spawner_launch_description( + ["rrbot_with_sensor_position_controller"], + controller_params_files=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_controllers.yaml", + ], + extra_spawner_args=["--inactive"], + ), + generate_controllers_spawner_launch_description( + [ + "threedofbot_joint_state_broadcaster", + "threedofbot_position_controller", + "threedofbot_pid_gain_controller", + ], + controller_params_files=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_controllers.yaml", + ], + extra_spawner_args=["--inactive"], + ), + # Command publishers + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_position_command_publisher", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_position_command_publishers.yaml", + ], + ), + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="rrbot_with_sensor_position_command_publisher", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_position_command_publishers.yaml", + ], + ), + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="threedofbot_position_command_publisher", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_13")) + / "config" + / "three_robots_position_command_publishers.yaml", + ], ), - " slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_13"), - "config", - "three_robots_controllers.yaml", - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_13"), "rviz", "three_robots.rviz"] - ) - - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_13"), - "config", - "three_robots_position_command_publishers.yaml", ] ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - # global broadcaster and initially active controllers from RRBot - general_ctrl_spawner = generate_controllers_spawner_launch_description( - [ - # Global joint state broadcaster - "joint_state_broadcaster", - # RRBot controllers - "rrbot_joint_state_broadcaster", - "rrbot_position_controller", - # External FTS broadcaster - "rrbot_external_fts_broadcaster", - ], - controller_params_files=[robot_controllers], - ) - - # RRBot with sensors controllers, initially active - rrbot_sensor_ctrl_spawner_active = generate_controllers_spawner_launch_description( - ["rrbot_with_sensor_joint_state_broadcaster", "rrbot_with_sensor_fts_broadcaster"], - controller_params_files=[robot_controllers], - ) - - # RRBot with sensors controllers, initially inactive - rrbot_sensor_ctrl_spawner_inactive = generate_controllers_spawner_launch_description( - [ - "rrbot_with_sensor_position_controller", - ], - controller_params_files=[robot_controllers], - extra_spawner_args=["--inactive"], - ) - - # ThreeDofBot controllers, initially inactive - threedofbot_ctrl_spawner = generate_controllers_spawner_launch_description( - [ - "threedofbot_joint_state_broadcaster", - "threedofbot_position_controller", - "threedofbot_pid_gain_controller", - ], - controller_params_files=[robot_controllers], - extra_spawner_args=["--inactive"], - ) - - # Command publishers - rrbot_position_command_publisher = Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - name="rrbot_position_command_publisher", - parameters=[position_goals], - ) - rrbot_with_sensor_position_command_publisher = Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - name="rrbot_with_sensor_position_command_publisher", - parameters=[position_goals], - ) - threedofbot_position_command_publisher = Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - name="threedofbot_position_command_publisher", - parameters=[position_goals], - ) - - nodes = [ - control_node, - robot_state_pub_node, - rviz_node, - general_ctrl_spawner, - rrbot_sensor_ctrl_spawner_active, - rrbot_sensor_ctrl_spawner_inactive, - threedofbot_ctrl_spawner, - rrbot_position_command_publisher, - rrbot_with_sensor_position_command_publisher, - threedofbot_position_command_publisher, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py index 09c168f08..66da1e4b5 100644 --- a/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py +++ b/example_14/bringup/launch/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.launch.py @@ -13,141 +13,100 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_velocity_controller", - description="Robot controller to start.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - prefix = LaunchConfiguration("prefix") - slowdown = LaunchConfiguration("slowdown") - robot_controller = LaunchConfiguration("robot_controller") - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_14"), - "urdf", - "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro", - ] + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for multi-robot setup. If changed than also joint names in the controllers' configuration have to be updated.", + ), + DeclareLaunchArgument( + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." + ), + DeclareLaunchArgument( + "robot_controller", + default_value="forward_velocity_controller", + description="Robot controller to start.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # controller manager node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_14")) + / "config" + / "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml", + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_14")) + / "urdf" + / "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro", + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + # Spawners + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + LaunchConfiguration("robot_controller"), + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_14")) + / "config" + / "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml", + ], ), - " ", - "prefix:=", - prefix, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_14"), - "config", - "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_14/description/launch/view_robot.launch.py b/example_14/description/launch/view_robot.launch.py index 4bcb91972..4e9e336c2 100644 --- a/example_14/description/launch/view_robot.launch.py +++ b/example_14/description/launch/view_robot.launch.py @@ -15,97 +15,85 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_14"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value=( + "rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.urdf.xacro" + ), + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_14")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py b/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py index 82c26ca24..aae49f347 100644 --- a/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py +++ b/example_15/bringup/launch/multi_controller_manager_example_two_rrbots.launch.py @@ -16,153 +16,113 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, ThisLaunchFileDir +from launch.substitutions import LaunchConfiguration, PathSubstitution, ThisLaunchFileDir from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with fake hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable fake command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_position_controller", - description="Robot controller to start.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "start_rviz_multi", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - start_rviz_lc = LaunchConfiguration("start_rviz_multi") - - # Initialize Arguments - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - robot_controller = LaunchConfiguration("robot_controller") - - rrbot_1_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "namespace": "rrbot_1", - "description_package": "ros2_control_demo_example_1", - "description_file": "rrbot.urdf.xacro", - "runtime_config_package": "ros2_control_demo_example_15", - "controllers_file": "multi_controller_manager_rrbot_generic_controllers.yaml", - "prefix": "rrbot_1_", - "use_mock_hardware": use_mock_hardware, - "mock_sensor_commands": mock_sensor_commands, - "slowdown": slowdown, - "controller_manager_name": "/rrbot_1/controller_manager", - "robot_controller": robot_controller, - "start_rviz": "false", - }.items(), - ) - - rrbot_1_position_trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - namespace="rrbot_1", - arguments=[ - "position_trajectory_controller", - "--inactive", - "--param-file", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - "multi_controller_manager_rrbot_generic_controllers.yaml", - ] + return LaunchDescription( + [ + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with fake hardware mirroring command to its states.", ), - ], - ) - - rrbot_2_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "namespace": "rrbot_2", - "description_package": "ros2_control_demo_example_5", - "description_file": "rrbot_system_with_external_sensor.urdf.xacro", - "runtime_config_package": "ros2_control_demo_example_15", - "controllers_file": "multi_controller_manager_rrbot_generic_controllers.yaml", - "prefix": "rrbot_2_", - "use_mock_hardware": use_mock_hardware, - "mock_sensor_commands": mock_sensor_commands, - "slowdown": slowdown, - "controller_manager_name": "/rrbot_2/controller_manager", - "robot_controller": robot_controller, - "start_rviz": "false", - }.items(), - ) - - rrbot_2_position_trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - namespace="rrbot_2", - arguments=[ - "position_trajectory_controller", - "--inactive", - "--param-file", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - "multi_controller_manager_rrbot_generic_controllers.yaml", - ] + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description=( + "Enable fake command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true." + ), ), - ], - ) - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_15"), "rviz", "multi_controller_manager.rviz"] - ) - - rviz_node_multi = Node( - package="rviz2", - executable="rviz2", - name="rviz2_multi", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(start_rviz_lc), + DeclareLaunchArgument( + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." + ), + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ), + DeclareLaunchArgument( + "start_rviz_multi", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "namespace": "rrbot_1", + "description_package": "ros2_control_demo_example_1", + "description_file": "rrbot.urdf.xacro", + "runtime_config_package": "ros2_control_demo_example_15", + "controllers_file": "multi_controller_manager_rrbot_generic_controllers.yaml", + "prefix": "rrbot_1_", + "use_mock_hardware": LaunchConfiguration("use_mock_hardware"), + "mock_sensor_commands": LaunchConfiguration("mock_sensor_commands"), + "slowdown": LaunchConfiguration("slowdown"), + "controller_manager_name": "/rrbot_1/controller_manager", + "robot_controller": LaunchConfiguration("robot_controller"), + "start_rviz": "false", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), + launch_arguments={ + "namespace": "rrbot_2", + "description_package": "ros2_control_demo_example_5", + "description_file": "rrbot_system_with_external_sensor.urdf.xacro", + "runtime_config_package": "ros2_control_demo_example_15", + "controllers_file": "multi_controller_manager_rrbot_generic_controllers.yaml", + "prefix": "rrbot_2_", + "use_mock_hardware": LaunchConfiguration("use_mock_hardware"), + "mock_sensor_commands": LaunchConfiguration("mock_sensor_commands"), + "slowdown": LaunchConfiguration("slowdown"), + "controller_manager_name": "/rrbot_2/controller_manager", + "robot_controller": LaunchConfiguration("robot_controller"), + "start_rviz": "false", + }.items(), + ), + Node( + package="controller_manager", + executable="spawner", + namespace="rrbot_1", + arguments=[ + "position_trajectory_controller", + "--inactive", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "multi_controller_manager_rrbot_generic_controllers.yaml", + ], + ), + Node( + package="controller_manager", + executable="spawner", + namespace="rrbot_2", + arguments=[ + "position_trajectory_controller", + "--inactive", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "multi_controller_manager_rrbot_generic_controllers.yaml", + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2_multi", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "rviz" + / "multi_controller_manager.rviz", + ], + condition=IfCondition(LaunchConfiguration("start_rviz_multi")), + ), + ] ) - - included_launch_files = [ - rrbot_1_launch, - rrbot_2_launch, - ] - - nodes_to_start = [ - rrbot_1_position_trajectory_controller_spawner, - rrbot_2_position_trajectory_controller_spawner, - rviz_node_multi, - ] - - return LaunchDescription(declared_arguments + included_launch_files + nodes_to_start) diff --git a/example_15/bringup/launch/rrbot_base.launch.py b/example_15/bringup/launch/rrbot_base.launch.py index 1e75578bb..de15ad8ee 100644 --- a/example_15/bringup/launch/rrbot_base.launch.py +++ b/example_15/bringup/launch/rrbot_base.launch.py @@ -13,225 +13,196 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "namespace", - default_value="/", - description="Namespace of controller manager and controllers. This is useful for \ - multi-robot scenarios.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ros2_control_demo_bringup", - description='Package with the controller\'s configuration in "config" folder. \ - Usually the argument is not set, it enables use of a custom setup.', - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "controllers_file", - default_value="rrbot_controllers.yaml", - description="YAML file with the controllers configuration.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_gazebo", - default_value="false", - description="Start robot in Gazebo simulation.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="true", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable mock command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="3.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "controller_manager_name", - default_value="/controller_manager", - description="Full name of the controller manager. This values should be set if \ - controller manager is used under a namespace.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_position_controller", - description="Robot controller to start.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "start_rviz", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - namespace = LaunchConfiguration("namespace") - runtime_config_package = LaunchConfiguration("runtime_config_package") - controllers_file = LaunchConfiguration("controllers_file") - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - prefix = LaunchConfiguration("prefix") - use_gazebo = LaunchConfiguration("use_gazebo") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - controller_manager_name = LaunchConfiguration("controller_manager_name") - robot_controller = LaunchConfiguration("robot_controller") - start_rviz = LaunchConfiguration("start_rviz") - - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] - ), - " ", - "prefix:=", - prefix, - " ", - "use_gazebo:=", - use_gazebo, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( + return LaunchDescription( [ - FindPackageShare(runtime_config_package), - "config", - controllers_file, + DeclareLaunchArgument( + "namespace", + default_value="/", + description="Namespace of controller manager and controllers. This is useful for multi-robot scenarios.", + ), + DeclareLaunchArgument( + "runtime_config_package", + default_value="ros2_control_demo_bringup", + description='Package with the controller\'s configuration in "config" folder. Usually the argument is not set, it enables use of a custom setup.', + ), + DeclareLaunchArgument( + "controllers_file", + default_value="rrbot_controllers.yaml", + description="YAML file with the controllers configuration.", + ), + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description="Description package with robot URDF/xacro files. Usually the argument is not set, it enables use of a custom description.", + ), + DeclareLaunchArgument( + "description_file", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for multi-robot setup. If changed than also joint names in the controllers' configuration have to be updated.", + ), + DeclareLaunchArgument( + "use_gazebo", + default_value="false", + description="Start robot in Gazebo simulation.", + ), + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with mock hardware mirroring command to its states.", + ), + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. Used only if 'use_mock_hardware' parameter is true.", + ), + DeclareLaunchArgument( + "slowdown", default_value="3.0", description="Slowdown factor of the RRbot." + ), + DeclareLaunchArgument( + "controller_manager_name", + default_value="/controller_manager", + description="Full name of the controller manager. This values should be set if controller manager is used under a namespace.", + ), + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ), + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # robot description from xacro + Node( + package="controller_manager", + executable="ros2_control_node", + namespace=LaunchConfiguration("namespace"), + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution( + FindPackageShare(LaunchConfiguration("description_package")) + ) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_gazebo:=", + LaunchConfiguration("use_gazebo"), + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + " ", + "mock_sensor_commands:=", + LaunchConfiguration("mock_sensor_commands"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + }, + PathSubstitution( + FindPackageShare(LaunchConfiguration("runtime_config_package")) + ) + / "config" + / LaunchConfiguration("controllers_file"), + ], + output="both", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + namespace=LaunchConfiguration("namespace"), + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution( + FindPackageShare(LaunchConfiguration("description_package")) + ) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_gazebo:=", + LaunchConfiguration("use_gazebo"), + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + " ", + "mock_sensor_commands:=", + LaunchConfiguration("mock_sensor_commands"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + namespace=LaunchConfiguration("namespace"), + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "config" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("start_rviz")), + ), + Node( + package="controller_manager", + executable="spawner", + namespace=LaunchConfiguration("namespace"), + arguments=[ + "joint_state_broadcaster", + "-c", + LaunchConfiguration("controller_manager_name"), + ], + ), + Node( + package="controller_manager", + executable="spawner", + namespace=LaunchConfiguration("namespace"), + arguments=[ + LaunchConfiguration("robot_controller"), + "-c", + LaunchConfiguration("controller_manager_name"), + "--param-file", + PathSubstitution( + FindPackageShare(LaunchConfiguration("runtime_config_package")) + ) + / "config" + / LaunchConfiguration("controllers_file"), + ], + ), ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "config", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - namespace=namespace, - parameters=[robot_description, robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - namespace=namespace, - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - namespace=namespace, - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - namespace=namespace, - arguments=["joint_state_broadcaster", "-c", controller_manager_name], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - namespace=namespace, - arguments=[ - robot_controller, - "-c", - controller_manager_name, - "--param-file", - robot_controllers, - ], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - robot_controller_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_15/bringup/launch/rrbot_namespace.launch.py b/example_15/bringup/launch/rrbot_namespace.launch.py index a2b18d819..940125e09 100644 --- a/example_15/bringup/launch/rrbot_namespace.launch.py +++ b/example_15/bringup/launch/rrbot_namespace.launch.py @@ -14,130 +14,108 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "start_rviz", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - start_rviz = LaunchConfiguration("start_rviz") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_1"), - "urdf", - "rrbot.urdf.xacro", - ] + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + Node( + package="controller_manager", + executable="ros2_control_node", + namespace="/rrbot", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_1")) + / "urdf" + / "rrbot.urdf.xacro", + ] + ) + }, + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "rrbot_namespace_controllers.yaml", + ], + output={"stdout": "screen", "stderr": "screen"}, + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + namespace="/rrbot", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_1")) + / "urdf" + / "rrbot.urdf.xacro", + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "rviz" + / "rrbot_namespace.rviz", + ], + condition=IfCondition(LaunchConfiguration("start_rviz")), + ), + Node( + package="controller_manager", + executable="spawner", + namespace="rrbot", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + namespace="rrbot", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "rrbot_namespace_controllers.yaml", + "--controller-ros-args", + "-r forward_position_controller/commands:=/position_commands", + ], + ), + Node( + package="controller_manager", + executable="spawner", + namespace="rrbot", + arguments=[ + "position_trajectory_controller", + "--inactive", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "rrbot_namespace_controllers.yaml", + ], ), ] ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - "rrbot_namespace_controllers.yaml", - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_15"), "rviz", "rrbot_namespace.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - namespace="/rrbot", - parameters=[robot_description, robot_controllers], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - namespace="/rrbot", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - namespace="rrbot", - arguments=["joint_state_broadcaster"], - ) - - robot_forward_position_controller_spawner = Node( - package="controller_manager", - executable="spawner", - namespace="rrbot", - arguments=[ - "forward_position_controller", - "--param-file", - robot_controllers, - # we use the remapping from a relative name to FQN /position_commands - "--controller-ros-args", - "-r forward_position_controller/commands:=/position_commands", - ], - ) - - robot_position_trajectory_controller_spawner = Node( - package="controller_manager", - executable="spawner", - namespace="rrbot", - arguments=[ - "position_trajectory_controller", - "--inactive", - "--param-file", - robot_controllers, - ], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - robot_forward_position_controller_spawner, - robot_position_trajectory_controller_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_15/bringup/launch/test_forward_position_controller.launch.py b/example_15/bringup/launch/test_forward_position_controller.launch.py index d539e6a87..b092a9a2a 100644 --- a/example_15/bringup/launch/test_forward_position_controller.launch.py +++ b/example_15/bringup/launch/test_forward_position_controller.launch.py @@ -14,43 +14,33 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "publisher_config", - default_value="rrbot_forward_position_publisher.yaml", - description="Name of the publisher config file stored inside \ - ros2_control_demo_bringup/config/", - ) - ) - - # Initialize Arguments - publisher_config = LaunchConfiguration("publisher_config") - - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - publisher_config, - ] - ) - return LaunchDescription( [ + DeclareLaunchArgument( + "publisher_config", + default_value="rrbot_forward_position_publisher.yaml", + description=( + "Name of the publisher config file stored inside " + "ros2_control_demo_bringup/config/" + ), + ), Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / LaunchConfiguration("publisher_config") + ], output="both", - ) + ), ] ) diff --git a/example_15/bringup/launch/test_joint_trajectory_controller.launch.py b/example_15/bringup/launch/test_joint_trajectory_controller.launch.py index 5aac0004f..1d35fb644 100644 --- a/example_15/bringup/launch/test_joint_trajectory_controller.launch.py +++ b/example_15/bringup/launch/test_joint_trajectory_controller.launch.py @@ -14,43 +14,31 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "publisher_config", - default_value="rrbot_joint_trajectory_publisher.yaml", - description="Name of the publisher config file stored inside \ - ros2_control_demo_bringup/config/", - ) - ) - - # Initialize Arguments - publisher_config = LaunchConfiguration("publisher_config") - - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - publisher_config, - ] - ) - return LaunchDescription( [ + DeclareLaunchArgument( + "publisher_config", + default_value="rrbot_joint_trajectory_publisher.yaml", + description="Name of the publisher config file stored inside \ + ros2_control_demo_bringup/config/", + ), Node( package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / LaunchConfiguration("publisher_config") + ], output="both", - ) + ), ] ) diff --git a/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py b/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py index df17e30d5..640258042 100644 --- a/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py +++ b/example_15/bringup/launch/test_multi_controller_manager_forward_position_controller.launch.py @@ -13,49 +13,44 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # For example: the parameters for different node may be placed into the same yaml file - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - "multi_controller_manager_forward_position_publisher.yaml", - ] - ) - - rrbot_1_publisher_forward_position_controller_node = Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - namespace="rrbot_1", - name=["publisher_forward_position_controller"], - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - - rrbot_2_publisher_forward_position_controller_node = Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - namespace="rrbot_2", - name=["publisher_forward_position_controller"], - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - return LaunchDescription( [ - rrbot_1_publisher_forward_position_controller_node, - rrbot_2_publisher_forward_position_controller_node, + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + namespace="rrbot_1", + name=["publisher_forward_position_controller"], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "multi_controller_manager_forward_position_publisher.yaml", + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ), + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + namespace="rrbot_2", + name=["publisher_forward_position_controller"], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "multi_controller_manager_forward_position_publisher.yaml", + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ), ] ) diff --git a/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py b/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py index 00e73c529..e8a8f72b3 100644 --- a/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py +++ b/example_15/bringup/launch/test_multi_controller_manager_joint_trajectory_controller.launch.py @@ -13,49 +13,44 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # For example: the parameters for different node may be placed into the same yaml file - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_15"), - "config", - "multi_controller_manager_joint_trajectory_publisher.yaml", - ] - ) - - rrbot_1_publisher_joint_trajectory_controller_node = Node( - package="ros2_controllers_test_nodes", - executable="publisher_joint_trajectory_controller", - namespace="rrbot_1", - name="publisher_joint_trajectory_controller", - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - - rrbot_2_publisher_joint_trajectory_controller_node = Node( - package="ros2_controllers_test_nodes", - executable="publisher_joint_trajectory_controller", - namespace="rrbot_2", - name="publisher_joint_trajectory_controller", - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - return LaunchDescription( [ - rrbot_1_publisher_joint_trajectory_controller_node, - rrbot_2_publisher_joint_trajectory_controller_node, + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + namespace="rrbot_1", + name="publisher_joint_trajectory_controller", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "multi_controller_manager_joint_trajectory_publisher.yaml", + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ), + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + namespace="rrbot_2", + name="publisher_joint_trajectory_controller", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_15")) + / "config" + / "multi_controller_manager_joint_trajectory_publisher.yaml", + ], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ), ] ) diff --git a/example_16/bringup/launch/diffbot.launch.py b/example_16/bringup/launch/diffbot.launch.py index 8552eb55a..a27e28494 100644 --- a/example_16/bringup/launch/diffbot.launch.py +++ b/example_16/bringup/launch/diffbot.launch.py @@ -14,158 +14,97 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - Command, - FindExecutable, - PathJoinSubstitution, - LaunchConfiguration, -) +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "fixed_frame_id", - default_value="odom", - description="Fixed frame id of the robot.", - ) - ) - - # Initialize Arguments - gui = LaunchConfiguration("gui") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - fixed_frame_id = LaunchConfiguration("fixed_frame_id") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_16"), "urdf", "diffbot.urdf.xacro"] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ), + DeclareLaunchArgument( + "fixed_frame_id", + default_value="odom", + description="Fixed frame id of the robot.", + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_16")) + / "config" + / "diffbot_chained_controllers.yaml" + ], + output="both", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_16")) + / "urdf" + / "diffbot.urdf.xacro", + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "diffbot/rviz" + / "diffbot.rviz", + "-f", + LaunchConfiguration("fixed_frame_id"), + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "pid_controller_left_wheel_joint", + "pid_controller_right_wheel_joint", + "diffbot_base_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_16")) + / "config" + / "diffbot_chained_controllers.yaml", + "--controller-ros-args", + "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", + ], ), - " ", - "use_mock_hardware:=", - use_mock_hardware, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_16"), - "config", - "diffbot_chained_controllers.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file, "-f", fixed_frame_id], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - pid_controllers_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "pid_controller_left_wheel_joint", - "pid_controller_right_wheel_joint", - "--param-file", - robot_controllers, - ], - ) - - robot_base_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "diffbot_base_controller", - "--param-file", - robot_controllers, - "--controller-ros-args", - "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", - ], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - delay_robot_base_after_pid_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=pid_controllers_spawner, - on_exit=[robot_base_controller_spawner], - ) - ) - - # Delay start of joint_state_broadcaster after `robot_base_controller` - # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. - delay_joint_state_broadcaster_after_robot_base_controller_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=robot_base_controller_spawner, - on_exit=[joint_state_broadcaster_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - pid_controllers_spawner, - delay_robot_base_after_pid_controller_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_joint_state_broadcaster_after_robot_base_controller_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_16/description/launch/view_robot.launch.py b/example_16/description/launch/view_robot.launch.py index a48efbec7..57ebc80a3 100644 --- a/example_16/description/launch/view_robot.launch.py +++ b/example_16/description/launch/view_robot.launch.py @@ -16,99 +16,85 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="diffbot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_16"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="diffbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_16")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "diffbot/rviz" + / "diffbot_view.rviz", + "-f", + "base_link", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - # start rviz2 with initial fixed frame id as base_link - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file, "-f", "base_link"], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_2/bringup/launch/diffbot.launch.py b/example_2/bringup/launch/diffbot.launch.py index 8c3f2b3b7..a825a788d 100644 --- a/example_2/bringup/launch/diffbot.launch.py +++ b/example_2/bringup/launch/diffbot.launch.py @@ -13,124 +13,90 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - - # Initialize Arguments - gui = LaunchConfiguration("gui") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_2"), "urdf", "diffbot.urdf.xacro"] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_2")) + / "config" + / "diffbot_controllers.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_2")) + / "urdf" + / "diffbot.urdf.xacro", + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "diffbot/rviz" + / "diffbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "diffbot_base_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_2")) + / "config" + / "diffbot_controllers.yaml", + "--controller-ros-args", + "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", + ], ), - " ", - "use_mock_hardware:=", - use_mock_hardware, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_2"), - "config", - "diffbot_controllers.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "diffbot/rviz", "diffbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "diffbot_base_controller", - "--param-file", - robot_controllers, - "--controller-ros-args", - "-r /diffbot_base_controller/cmd_vel:=/cmd_vel", - ], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_2/description/launch/view_robot.launch.py b/example_2/description/launch/view_robot.launch.py index 8c030d3aa..5893ff7f7 100644 --- a/example_2/description/launch/view_robot.launch.py +++ b/example_2/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="diffbot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_2"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="diffbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_2")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "diffbot/rviz" + / "diffbot_view.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "diffbot/rviz", "diffbot_view.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_3/bringup/launch/test_forward_position_controller.launch.py b/example_3/bringup/launch/test_forward_position_controller.launch.py index bef29f996..01b30de2c 100644 --- a/example_3/bringup/launch/test_forward_position_controller.launch.py +++ b/example_3/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_3"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_3")) + / "config" + / "rrbot_forward_position_publisher.yaml" + ], output="both", ) ] diff --git a/example_3/description/launch/view_robot.launch.py b/example_3/description/launch/view_robot.launch.py index be7e715a8..edf90286b 100644 --- a/example_3/description/launch/view_robot.launch.py +++ b/example_3/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot_system_multi_interface.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_3"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot_system_multi_interface.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_3")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py index 770d627dc..5073272ed 100644 --- a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py +++ b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py @@ -13,164 +13,131 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable mocked command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - prefix = LaunchConfiguration("prefix") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_4"), - "urdf", - "rrbot_system_with_sensor.urdf.xacro", - ] + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ), + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description=( + "Enable mocked command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true." + ), + ), + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_4")) + / "config" + / "rrbot_with_sensor_controllers.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_4")) + / "urdf" + / "rrbot_system_with_sensor.urdf.xacro", + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + " ", + "mock_sensor_commands:=", + LaunchConfiguration("mock_sensor_commands"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_example_4")) + / "rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_4")) + / "config" + / "rrbot_with_sensor_controllers.yaml", + ], + ), + # add the spawner node for the fts_broadcaster + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "fts_broadcaster", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_4")) + / "config" + / "rrbot_with_sensor_controllers.yaml", + ], ), - " ", - "prefix:=", - prefix, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_4"), - "config", - "rrbot_with_sensor_controllers.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_4"), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["forward_position_controller", "--param-file", robot_controllers], - ) - - # add the spawner node for the fts_broadcaster - fts_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["fts_broadcaster", "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - fts_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_4/bringup/launch/test_forward_position_controller.launch.py b/example_4/bringup/launch/test_forward_position_controller.launch.py index c7bb9bd51..99d3cea58 100644 --- a/example_4/bringup/launch/test_forward_position_controller.launch.py +++ b/example_4/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_4"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_4")) + / "config" + / "rrbot_forward_position_publisher.yaml", + ], output="both", ) ] diff --git a/example_4/description/launch/view_robot.launch.py b/example_4/description/launch/view_robot.launch.py index c72b1579b..06f133b8b 100644 --- a/example_4/description/launch/view_robot.launch.py +++ b/example_4/description/launch/view_robot.launch.py @@ -15,97 +15,85 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_example_4", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot_system_with_sensor.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_example_4", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot_system_with_sensor.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution( + FindPackageShare(LaunchConfiguration("description_package")) + ) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py index cc2cf26f3..5e66e4268 100755 --- a/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py +++ b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py @@ -13,164 +13,131 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable mocked command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - prefix = LaunchConfiguration("prefix") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_5"), - "urdf", - "rrbot_system_with_external_sensor.urdf.xacro", - ] + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ), + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description=( + "Enable mocked command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true." + ), + ), + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) + / "config" + / "rrbot_with_external_sensor_controllers.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) + / "urdf" + / "rrbot_system_with_external_sensor.urdf.xacro", + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + " ", + "mock_sensor_commands:=", + LaunchConfiguration("mock_sensor_commands"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) + / "rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) + / "config" + / "rrbot_with_external_sensor_controllers.yaml", + ], + ), + # add the spawner node for the fts_broadcaster + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "fts_broadcaster", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) + / "config" + / "rrbot_with_external_sensor_controllers.yaml", + ], ), - " ", - "prefix:=", - prefix, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_5"), - "config", - "rrbot_with_external_sensor_controllers.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_5"), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["forward_position_controller", "--param-file", robot_controllers], - ) - - # add the spawner node for the fts_broadcaster - fts_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["fts_broadcaster", "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - fts_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_5/bringup/launch/test_forward_position_controller.launch.py b/example_5/bringup/launch/test_forward_position_controller.launch.py index 72e514570..106da17f9 100644 --- a/example_5/bringup/launch/test_forward_position_controller.launch.py +++ b/example_5/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_5"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_5")) + / "config" + / "rrbot_forward_position_publisher.yaml", + ], output="both", ) ] diff --git a/example_5/description/launch/view_robot.launch.py b/example_5/description/launch/view_robot.launch.py index df4cef17b..07e5659b2 100644 --- a/example_5/description/launch/view_robot.launch.py +++ b/example_5/description/launch/view_robot.launch.py @@ -15,97 +15,85 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_example_5", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot_system_with_external_sensor.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_example_5", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot_system_with_external_sensor.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution( + FindPackageShare(LaunchConfiguration("description_package")) + ) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_6/bringup/launch/rrbot_modular_actuators.launch.py b/example_6/bringup/launch/rrbot_modular_actuators.launch.py index 9bfbf56c9..6b144e00d 100644 --- a/example_6/bringup/launch/rrbot_modular_actuators.launch.py +++ b/example_6/bringup/launch/rrbot_modular_actuators.launch.py @@ -13,164 +13,124 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "use_mock_hardware", - default_value="false", - description="Start robot with mock hardware mirroring command to its states.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "mock_sensor_commands", - default_value="false", - description="Enable mocked command interfaces for sensors used for simple simulations. \ - Used only if 'use_mock_hardware' parameter is true.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_position_controller", - description="Robot controller to start.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - prefix = LaunchConfiguration("prefix") - use_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - robot_controller = LaunchConfiguration("robot_controller") - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_6"), - "urdf", - "rrbot_modular_actuators.urdf.xacro", - ] + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + DeclareLaunchArgument( + "use_mock_hardware", + default_value="false", + description="Start robot with mock hardware mirroring command to its states.", + ), + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description=( + "Enable mocked command interfaces for sensors used for simple simulations. " + "Used only if 'use_mock_hardware' parameter is true." + ), + ), + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ), + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_6")) + / "config" + / "rrbot_modular_actuators.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_6")) + / "urdf" + / "rrbot_modular_actuators.urdf.xacro", + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "use_mock_hardware:=", + LaunchConfiguration("use_mock_hardware"), + " ", + "mock_sensor_commands:=", + LaunchConfiguration("mock_sensor_commands"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + LaunchConfiguration("robot_controller"), + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_6")) + / "config" + / "rrbot_modular_actuators.yaml", + ], ), - " ", - "prefix:=", - prefix, - " ", - "use_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_6"), - "config", - "rrbot_modular_actuators.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_6/bringup/launch/test_forward_position_controller.launch.py b/example_6/bringup/launch/test_forward_position_controller.launch.py index e9e6d8d85..a29f55154 100644 --- a/example_6/bringup/launch/test_forward_position_controller.launch.py +++ b/example_6/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_6"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_6")) + / "config" + / "rrbot_forward_position_publisher.yaml", + ], output="both", ) ] diff --git a/example_6/description/launch/view_robot.launch.py b/example_6/description/launch/view_robot.launch.py index d5ce1ca4f..497fb090d 100644 --- a/example_6/description/launch/view_robot.launch.py +++ b/example_6/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot_modular_actuators.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_6"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot_modular_actuators.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_6")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_7/bringup/launch/r6bot_controller.launch.py b/example_7/bringup/launch/r6bot_controller.launch.py index 119b2d5e4..a8516d11e 100644 --- a/example_7/bringup/launch/r6bot_controller.launch.py +++ b/example_7/bringup/launch/r6bot_controller.launch.py @@ -13,109 +13,80 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import RegisterEventHandler, DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_7"), - "urdf", - "r6bot.urdf.xacro", - ] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_7")) + / "config" + / "r6bot_controller.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_7")) + / "urdf" + / "r6bot.urdf.xacro", + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "r6bot/rviz" + / "view_robot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "r6bot_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_7")) + / "config" + / "r6bot_controller.yaml", + ], ), ] ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_7"), - "config", - "r6bot_controller.yaml", - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "r6bot/rviz", "view_robot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - gui = LaunchConfiguration("gui") - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["r6bot_controller", "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_7/bringup/launch/send_trajectory.launch.py b/example_7/bringup/launch/send_trajectory.launch.py index 3d37ec8a0..4cda7e533 100644 --- a/example_7/bringup/launch/send_trajectory.launch.py +++ b/example_7/bringup/launch/send_trajectory.launch.py @@ -19,28 +19,30 @@ def generate_launch_description(): - # Get URDF via xacro - robot_description_content = Command( + + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_7"), - "urdf", - "r6bot.urdf.xacro", - ] - ), + Node( + package="ros2_control_demo_example_7", + executable="send_trajectory", + name="send_trajectory_node", + parameters=[ + { + "robot_description": Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_7"), + "urdf", + "r6bot.urdf.xacro", + ] + ), + ] + ) + } + ], + ) ] ) - robot_description = {"robot_description": robot_description_content} - - send_trajectory_node = Node( - package="ros2_control_demo_example_7", - executable="send_trajectory", - name="send_trajectory_node", - parameters=[robot_description], - ) - - nodes_to_start = [send_trajectory_node] - return LaunchDescription(nodes_to_start) diff --git a/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py index 10b24fe9d..a444ac7d5 100644 --- a/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py +++ b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py @@ -13,141 +13,103 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "robot_controller", - default_value="forward_position_controller", - description="Robot controller to start.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - prefix = LaunchConfiguration("prefix") - slowdown = LaunchConfiguration("slowdown") - robot_controller = LaunchConfiguration("robot_controller") - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_8"), - "urdf", - "rrbot_transmissions_system_position_only.urdf.xacro", - ] + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + DeclareLaunchArgument( + "slowdown", + default_value="50.0", + description="Slowdown factor of the RRbot.", + ), + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_8")) + / "config" + / "rrbot_controllers.yaml" + ], + output="both", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_8")) + / "urdf" + / "rrbot_transmissions_system_position_only.urdf.xacro", + " ", + "prefix:=", + LaunchConfiguration("prefix"), + " ", + "slowdown:=", + LaunchConfiguration("slowdown"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + LaunchConfiguration("robot_controller"), + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_8")) + / "config" + / "rrbot_controllers.yaml", + ], ), - " ", - "prefix:=", - prefix, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_8"), - "config", - "rrbot_controllers.yaml", ] ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_8/bringup/launch/test_forward_position_controller.launch.py b/example_8/bringup/launch/test_forward_position_controller.launch.py index bbeb5b2f2..b9dd6847c 100644 --- a/example_8/bringup/launch/test_forward_position_controller.launch.py +++ b/example_8/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_8"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_8")) + / "config" + / "rrbot_forward_position_publisher.yaml", + ], output="both", ) ] diff --git a/example_8/description/launch/view_robot.launch.py b/example_8/description/launch/view_robot.launch.py index ddc47aab7..da1343074 100644 --- a/example_8/description/launch/view_robot.launch.py +++ b/example_8/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot_transmissions_system_position_only.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_8"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot_transmissions_system_position_only.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_8")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_9/bringup/launch/rrbot.launch.py b/example_9/bringup/launch/rrbot.launch.py index e619c03bf..cce995e63 100644 --- a/example_9/bringup/launch/rrbot.launch.py +++ b/example_9/bringup/launch/rrbot.launch.py @@ -14,112 +14,80 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - gui = LaunchConfiguration("gui") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_9"), - "urdf", - "rrbot.urdf.xacro", - ] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # Control node + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "config" + / "rrbot_controllers.yaml" + ], + output="both", + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "urdf" + / "rrbot.urdf.xacro", + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "config" + / "rrbot_controllers.yaml", + ], ), ] ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_9"), - "config", - "rrbot_controllers.yaml", - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["forward_position_controller", "--param-file", robot_controllers], - ) - - # Delay rviz start after `joint_state_broadcaster` - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - - # Delay start of robot_controller after `joint_state_broadcaster` - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[robot_controller_spawner], - ) - ) - - nodes = [ - control_node, - robot_state_pub_node, - joint_state_broadcaster_spawner, - delay_rviz_after_joint_state_broadcaster_spawner, - delay_robot_controller_spawner_after_joint_state_broadcaster_spawner, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_9/bringup/launch/rrbot_gazebo.launch.py b/example_9/bringup/launch/rrbot_gazebo.launch.py index 90af2f3ac..9bee18291 100644 --- a/example_9/bringup/launch/rrbot_gazebo.launch.py +++ b/example_9/bringup/launch/rrbot_gazebo.launch.py @@ -16,126 +16,111 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - gui = LaunchConfiguration("gui") - - # gazebo - gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"] - ), - launch_arguments=[("gz_args", " -r -v 3 empty.sdf")], - condition=IfCondition(gui), - ) - gazebo_headless = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"] - ), - launch_arguments=[("gz_args", ["--headless-rendering -s -r -v 3 empty.sdf"])], - condition=UnlessCondition(gui), - ) - - # Gazebo bridge - gazebo_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], - output="screen", - ) - - gz_spawn_entity = Node( - package="ros_gz_sim", - executable="create", - output="screen", - arguments=[ - "-topic", - "/robot_description", - "-name", - "rrbot_system_position", - "-allow_renaming", - "true", - ], - ) - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_9"), "urdf", "rrbot.urdf.xacro"] + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ), + # gazebo + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathSubstitution(FindPackageShare("ros_gz_sim")) + / "launch" + / "gz_sim.launch.py" + ] + ), + launch_arguments=[("gz_args", " -r -v 3 empty.sdf")], + condition=IfCondition(LaunchConfiguration("gui")), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathSubstitution(FindPackageShare("ros_gz_sim")) + / "launch" + / "gz_sim.launch.py" + ] + ), + launch_arguments=[("gz_args", ["--headless-rendering -s -r -v 3 empty.sdf"])], + condition=UnlessCondition(LaunchConfiguration("gui")), + ), + # Gazebo bridge + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ), + Node( + package="ros_gz_sim", + executable="create", + output="screen", + arguments=[ + "-topic", + "/robot_description", + "-name", + "rrbot_system_position", + "-allow_renaming", + "true", + ], + ), + # robot_state_publisher with robot_description from xacro + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "urdf" + / "rrbot.urdf.xacro", + " ", + "use_gazebo:=true", + ] + ) + } + ], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + ), + Node( + package="controller_manager", + executable="spawner", + arguments=[ + "forward_position_controller", + "--param-file", + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "config" + / "rrbot_controllers.yaml", + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare("ros2_control_demo_description")) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "use_gazebo:=true", - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_9"), - "config", - "rrbot_controllers.yaml", ] ) - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] - ) - - node_robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[robot_description], - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["forward_position_controller", "--param-file", robot_controllers], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - gazebo, - gazebo_headless, - gazebo_bridge, - node_robot_state_publisher, - gz_spawn_entity, - joint_state_broadcaster_spawner, - robot_controller_spawner, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes) diff --git a/example_9/bringup/launch/test_forward_position_controller.launch.py b/example_9/bringup/launch/test_forward_position_controller.launch.py index 5cbc7ca68..c66e34e17 100644 --- a/example_9/bringup/launch/test_forward_position_controller.launch.py +++ b/example_9/bringup/launch/test_forward_position_controller.launch.py @@ -13,28 +13,24 @@ # limitations under the License. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - position_goals = PathJoinSubstitution( - [ - FindPackageShare("ros2_control_demo_example_9"), - "config", - "rrbot_forward_position_publisher.yaml", - ] - ) - return LaunchDescription( [ Node( package="ros2_controllers_test_nodes", executable="publisher_forward_position_controller", name="publisher_forward_position_controller", - parameters=[position_goals], + parameters=[ + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "config" + / "rrbot_forward_position_publisher.yaml", + ], output="both", ) ] diff --git a/example_9/description/launch/view_robot.launch.py b/example_9/description/launch/view_robot.launch.py index 3f7da6b91..3f7347187 100644 --- a/example_9/description/launch/view_robot.launch.py +++ b/example_9/description/launch/view_robot.launch.py @@ -15,97 +15,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import Command, LaunchConfiguration, PathSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="ros2_control_demo_description", - description="Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "description_file", - default_value="rrbot.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "gui", - default_value="true", - description="Start Rviz2 and Joint State Publisher gui automatically \ - with this launch file.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - description="Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers' configuration \ - have to be updated.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - gui = LaunchConfiguration("gui") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( + return LaunchDescription( [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare("ros2_control_demo_example_9"), "urdf", description_file] + DeclareLaunchArgument( + "description_package", + default_value="ros2_control_demo_description", + description=( + "Description package with robot URDF/xacro files. Usually the argument " + "is not set, it enables use of a custom description." + ), + ), + DeclareLaunchArgument( + "description_file", + default_value="rrbot.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ), + DeclareLaunchArgument( + "gui", + default_value="true", + description=( + "Start Rviz2 and Joint State Publisher gui automatically " + "with this launch file." + ), + ), + DeclareLaunchArgument( + "prefix", + default_value='""', + description=( + "Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated." + ), + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(LaunchConfiguration("gui")), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + { + "robot_description": Command( + [ + "xacro", + " ", + PathSubstitution(FindPackageShare("ros2_control_demo_example_9")) + / "urdf" + / LaunchConfiguration("description_file"), + " ", + "prefix:=", + LaunchConfiguration("prefix"), + ] + ) + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=[ + "-d", + PathSubstitution(FindPackageShare(LaunchConfiguration("description_package"))) + / "rrbot/rviz" + / "rrbot.rviz", + ], + condition=IfCondition(LaunchConfiguration("gui")), ), - " ", - "prefix:=", - prefix, ] ) - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rrbot/rviz", "rrbot.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - condition=IfCondition(gui), - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(gui), - ) - - nodes = [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - - return LaunchDescription(declared_arguments + nodes)