diff --git a/example_3/bringup/launch/rrbot_base.launch.py b/example_3/bringup/launch/rrbot_base.launch.py deleted file mode 100644 index 671443fd5..000000000 --- a/example_3/bringup/launch/rrbot_base.launch.py +++ /dev/null @@ -1,205 +0,0 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ros2_control_demo_example_3", - 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_example_3", - 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_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 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="3.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", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - 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_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - 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_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(runtime_config_package), - "config", - controllers_file, - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, 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(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--controller-manager", "/controller_manager"], - ) - - # 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_3/bringup/launch/rrbot_system_multi_interface.launch.py b/example_3/bringup/launch/rrbot_system_multi_interface.launch.py index 670d87030..4ae3d116d 100644 --- a/example_3/bringup/launch/rrbot_system_multi_interface.launch.py +++ b/example_3/bringup/launch/rrbot_system_multi_interface.launch.py @@ -1,4 +1,4 @@ -# Copyright 2021 Department of Engineering Cybernetics, NTNU. +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,9 +13,13 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -30,6 +34,21 @@ def generate_launch_description(): 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 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." @@ -42,23 +61,116 @@ def generate_launch_description(): description="Robot controller to start.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + 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") + start_rviz = LaunchConfiguration("start_rviz") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_3"), + "urdf", + "rrbot_system_multi_interface.urdf.xacro", + ] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + "slowdown:=", + slowdown, + ] + ) + robot_description = {"robot_description": robot_description_content} - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "controllers_file": "rrbot_multi_interface_forward_controllers.yaml", - "description_file": "rrbot_system_multi_interface.urdf.xacro", - "prefix": prefix, - "use_mock_hardware": "false", - "mock_sensor_commands": "false", - "slowdown": slowdown, - "robot_controller": robot_controller, - }.items(), + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_3"), + "config", + "rrbot_multi_interface_forward_controllers.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_3"), "rviz", "rrbot.rviz"] ) - return LaunchDescription(declared_arguments + [base_launch]) + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, 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(start_rviz), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "--controller-manager", "/controller_manager"], + ) + + # 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_4/bringup/launch/rrbot_base.launch.py b/example_4/bringup/launch/rrbot_base.launch.py deleted file mode 100644 index cf6c6d258..000000000 --- a/example_4/bringup/launch/rrbot_base.launch.py +++ /dev/null @@ -1,205 +0,0 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ros2_control_demo_example_4", - 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_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", - 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_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 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="3.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", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - 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_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - 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_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(runtime_config_package), - "config", - controllers_file, - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, 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(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--controller-manager", "/controller_manager"], - ) - - # 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_4/bringup/launch/rrbot_system_with_sensor.launch.py b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py old mode 100755 new mode 100644 index b4d587463..80db70574 --- a/example_4/bringup/launch/rrbot_system_with_sensor.launch.py +++ b/example_4/bringup/launch/rrbot_system_with_sensor.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -11,15 +11,15 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -# -# -# Authors: Subhas Das, Denis Stogl from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -29,8 +29,9 @@ def generate_launch_description(): 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.", + 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( @@ -50,9 +51,14 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "slowdown", - default_value="50.0", - description="Slowdown factor of the RRbot.", + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", ) ) @@ -61,17 +67,78 @@ def generate_launch_description(): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") + start_rviz = LaunchConfiguration("start_rviz") - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "controllers_file": "rrbot_with_sensor_controllers.yaml", - "description_file": "rrbot_system_with_sensor.urdf.xacro", - "prefix": prefix, - "use_mock_hardware": use_mock_hardware, - "mock_sensor_commands": mock_sensor_commands, - "slowdown": slowdown, - }.items(), + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_4"), + "urdf", + "rrbot_system_with_sensor.urdf.xacro", + ] + ), + " ", + "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_description, 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(start_rviz), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], ) # add the spawner node for the fts_broadcaster @@ -81,9 +148,29 @@ def generate_launch_description(): arguments=["fts_broadcaster", "--controller-manager", "/controller_manager"], ) - # collect all the nodes here + # 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 + [base_launch] + nodes) + return LaunchDescription(declared_arguments + nodes) diff --git a/example_5/bringup/launch/rrbot_base.launch.py b/example_5/bringup/launch/rrbot_base.launch.py deleted file mode 100644 index 27835bab7..000000000 --- a/example_5/bringup/launch/rrbot_base.launch.py +++ /dev/null @@ -1,205 +0,0 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ros2_control_demo_example_5", - 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_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", - 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_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 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="3.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", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - 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_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - 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_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(runtime_config_package), - "config", - controllers_file, - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, 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(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--controller-manager", "/controller_manager"], - ) - - # 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_5/bringup/launch/rrbot_system_with_external_sensor.launch.py b/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py index 566b1f501..4c963ee8e 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 @@ -1,4 +1,4 @@ -# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -11,15 +11,15 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -# -# -# Authors: Subhas Das, Denis Stogl from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -29,8 +29,9 @@ def generate_launch_description(): 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.", + 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( @@ -48,12 +49,16 @@ def generate_launch_description(): Used only if 'use_mock_hardware' parameter is true.", ) ) - declared_arguments.append( DeclareLaunchArgument( - "slowdown", - default_value="50.0", - description="Slowdown factor of the RRbot.", + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + default_value="true", + description="Start RViz2 automatically with this launch file.", ) ) @@ -62,17 +67,78 @@ def generate_launch_description(): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") slowdown = LaunchConfiguration("slowdown") + start_rviz = LaunchConfiguration("start_rviz") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_5"), + "urdf", + "rrbot_system_with_external_sensor.urdf.xacro", + ] + ), + " ", + "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_description, 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(start_rviz), + ) - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "controllers_file": "rrbot_with_external_sensor_controllers.yaml", - "description_file": "rrbot_system_with_external_sensor.urdf.xacro", - "prefix": prefix, - "use_mock_hardware": use_mock_hardware, - "mock_sensor_commands": mock_sensor_commands, - "slowdown": slowdown, - }.items(), + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["forward_position_controller", "--controller-manager", "/controller_manager"], ) # add the spawner node for the fts_broadcaster @@ -82,9 +148,29 @@ def generate_launch_description(): arguments=["fts_broadcaster", "--controller-manager", "/controller_manager"], ) - # collect all the nodes here + # 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 + [base_launch] + nodes) + return LaunchDescription(declared_arguments + nodes) diff --git a/example_6/bringup/config/rrbot_modular_actuators.yaml b/example_6/bringup/config/rrbot_modular_actuators.yaml index 410212de9..15846b724 100644 --- a/example_6/bringup/config/rrbot_modular_actuators.yaml +++ b/example_6/bringup/config/rrbot_modular_actuators.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 2 # Hz + update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster diff --git a/example_6/bringup/launch/rrbot_base.launch.py b/example_6/bringup/launch/rrbot_base.launch.py deleted file mode 100644 index 7fc4e395b..000000000 --- a/example_6/bringup/launch/rrbot_base.launch.py +++ /dev/null @@ -1,205 +0,0 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ros2_control_demo_example_6", - 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_example_6", - 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_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 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="3.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", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - 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_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - 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_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(runtime_config_package), - "config", - controllers_file, - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, 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(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--controller-manager", "/controller_manager"], - ) - - # 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/rrbot_modular_actuators.launch.py b/example_6/bringup/launch/rrbot_modular_actuators.launch.py index 0e17447ec..384df6230 100644 --- a/example_6/bringup/launch/rrbot_modular_actuators.launch.py +++ b/example_6/bringup/launch/rrbot_modular_actuators.launch.py @@ -1,4 +1,4 @@ -# Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -13,9 +13,13 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -26,13 +30,28 @@ def generate_launch_description(): "prefix", default_value='""', description="Prefix of the joint names, useful for \ - multi-robot setup. If changed, joint names in the controllers' configuration \ - also have to be updated.", + 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 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="3.0", description="Slowdown factor of the RRbot." + "slowdown", default_value="50.0", description="Slowdown factor of the RRbot." ) ) declared_arguments.append( @@ -42,22 +61,116 @@ def generate_launch_description(): description="Robot controller to start.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "start_rviz", + 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") + start_rviz = LaunchConfiguration("start_rviz") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_6"), + "urdf", + "rrbot_modular_actuators.urdf.xacro", + ] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + "slowdown:=", + slowdown, + ] + ) + robot_description = {"robot_description": robot_description_content} - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "controllers_file": "rrbot_modular_actuators.yaml", - "description_file": "rrbot_modular_actuators.urdf.xacro", - "prefix": prefix, - "use_mock_hardware": "false", - "slowdown": slowdown, - "robot_controller": robot_controller, - }.items(), + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_6"), + "config", + "rrbot_modular_actuators.yaml", + ] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("ros2_control_demo_example_6"), "rviz", "rrbot.rviz"] ) - return LaunchDescription(declared_arguments + [base_launch]) + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, 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(start_rviz), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "--controller-manager", "/controller_manager"], + ) + + # 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/rrbot_base.launch.py b/example_8/bringup/launch/rrbot_base.launch.py deleted file mode 100644 index 1720fcfd6..000000000 --- a/example_8/bringup/launch/rrbot_base.launch.py +++ /dev/null @@ -1,205 +0,0 @@ -# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "runtime_config_package", - default_value="ros2_control_demo_example_8", - 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_example_8", - 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_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 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="3.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", - default_value="true", - description="Start RViz2 automatically with this launch file.", - ) - ) - - # Initialize Arguments - 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_mock_hardware = LaunchConfiguration("use_mock_hardware") - mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") - slowdown = LaunchConfiguration("slowdown") - 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_mock_hardware:=", - use_mock_hardware, - " ", - "mock_sensor_commands:=", - mock_sensor_commands, - " ", - "slowdown:=", - slowdown, - ] - ) - robot_description = {"robot_description": robot_description_content} - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(runtime_config_package), - "config", - controllers_file, - ] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "rrbot.rviz"] - ) - - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, 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(start_rviz), - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "--controller-manager", "/controller_manager"], - ) - - # 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/rrbot_transmissions_system_position_only.launch.py b/example_8/bringup/launch/rrbot_transmissions_system_position_only.launch.py index 6d3d0dca3..6209560bf 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,9 +13,13 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -56,15 +60,94 @@ def generate_launch_description(): robot_controller = LaunchConfiguration("robot_controller") start_rviz = LaunchConfiguration("start_rviz") - base_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot_base.launch.py"]), - launch_arguments={ - "description_file": "rrbot_transmissions_system_position_only.urdf.xacro", - "prefix": prefix, - "slowdown": slowdown, - "robot_controller": robot_controller, - "start_rviz": start_rviz, - }.items(), + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("ros2_control_demo_example_8"), + "urdf", + "rrbot_transmissions_system_position_only.urdf.xacro", + ] + ), + " ", + "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_example_8"), "rviz", "rrbot.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, 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(start_rviz), + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - return LaunchDescription(declared_arguments + [base_launch]) + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "--controller-manager", "/controller_manager"], + ) + + # 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)