Add param names in bringup launch file

flexivrobotics · May 8, 2024 · 883602f · 883602f
1 parent 9118543
commit 883602f
Show file tree

Hide file tree

Showing 3 changed files with 83 additions and 44 deletions.
diff --git a/flexiv_bringup/README.md b/flexiv_bringup/README.md
@@ -2,7 +2,7 @@
 
 This package contains launch files: the main driver launcher, the MoveIt launch file and demo examples:
 
-- `rizon.launch.py` - the main launcher: starts ros2_control node including hardware interface, runs joint states, force torque sensor, external TCP wrench and TCP pose broadcasters, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller.
+- `rizon.launch.py` - the main launcher: starts *ros2_control* node including hardware interface, runs joint states, force torque sensor, external TCP wrench and TCP pose broadcasters, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller.
 - `rizon_moveit.launch.py` - runs MoveIt together with the driver. The controller for robot joints started in this launch file is *rizon_arm_controller*.
 - `test_joint_trajectory_controller.launch` - sends joint trajectory goals to the *rizon_arm_controller*.
 - `sine_sweep_position.launch.py` - gets current joint states and then performs a sine-sweep motion with *forward_position_controller*.

diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py
@@ -13,12 +13,20 @@
 
 
 def generate_launch_description():
+    rizon_type_param_name = "rizon_type"
+    robot_ip_param_name = "robot_ip"
+    local_ip_param_name = "local_ip"
+    start_rviz_param_name = "start_rviz"
+    use_fake_hardware_param_name = "use_fake_hardware"
+    fake_sensor_commands_param_name = "fake_sensor_commands"
+    robot_controller_param_name = "robot_controller"
+
     # Declare arguments
     declared_arguments = []
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "rizon_type",
+            rizon_type_param_name,
             description="Type of the Flexiv Rizon robot.",
             default_value="rizon4",
             choices=["rizon4", "rizon4s", "rizon10"],
@@ -27,37 +35,37 @@ def generate_launch_description():
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "robot_ip",
+            robot_ip_param_name,
             description="IP address of the robot server (remote).",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "local_ip",
+            local_ip_param_name,
             description="IP address of the workstation PC (local).",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "start_rviz",
+            start_rviz_param_name,
             default_value="true",
             description="start RViz automatically with the launch file",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "use_fake_hardware",
+            use_fake_hardware_param_name,
             default_value="false",
             description="Start robot with fake hardware mirroring command to its states.",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "fake_sensor_commands",
+            fake_sensor_commands_param_name,
             default_value="false",
             description="Enable fake command interfaces for sensors used for simple simulations. \
             Used only if 'use_fake_hardware' parameter is true.",
@@ -66,29 +74,20 @@ def generate_launch_description():
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "controllers_file",
-            default_value="rizon_controllers.yaml",
-            description="YAML file with the controllers configuration.",
-        )
-    )
-
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "robot_controller",
+            robot_controller_param_name,
             default_value="rizon_arm_controller",
             description="Robot controller to start. Available: forward_position_controller, rizon_arm_controller, joint_impedance_controller.",
         )
     )
 
     # Initialize Arguments
-    rizon_type = LaunchConfiguration("rizon_type")
-    robot_ip = LaunchConfiguration("robot_ip")
-    local_ip = LaunchConfiguration("local_ip")
-    start_rviz = LaunchConfiguration("start_rviz")
-    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
-    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
-    controllers_file = LaunchConfiguration("controllers_file")
-    robot_controller = LaunchConfiguration("robot_controller")
+    rizon_type = LaunchConfiguration(rizon_type_param_name)
+    robot_ip = LaunchConfiguration(robot_ip_param_name)
+    local_ip = LaunchConfiguration(local_ip_param_name)
+    start_rviz = LaunchConfiguration(start_rviz_param_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
+    robot_controller = LaunchConfiguration(robot_controller_param_name)
 
     # Get URDF via xacro
     flexiv_urdf_xacro = PathJoinSubstitution(
@@ -139,7 +138,7 @@ def generate_launch_description():
 
     # Robot controllers
     robot_controllers = PathJoinSubstitution(
-        [FindPackageShare("flexiv_bringup"), "config", controllers_file]
+        [FindPackageShare("flexiv_bringup"), "config", "rizon_controllers.yaml"]
     )
 
     # Controller Manager

diff --git a/flexiv_bringup/launch/rizon_moveit.launch.py b/flexiv_bringup/launch/rizon_moveit.launch.py
@@ -3,8 +3,9 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
 from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
 from launch.substitutions import (
     Command,
     FindExecutable,
@@ -29,12 +30,21 @@ def load_yaml(package_name, file_path):
 
 
 def generate_launch_description():
+    rizon_type_param_name = "rizon_type"
+    robot_ip_param_name = "robot_ip"
+    local_ip_param_name = "local_ip"
+    start_rviz_param_name = "start_rviz"
+    use_fake_hardware_param_name = "use_fake_hardware"
+    fake_sensor_commands_param_name = "fake_sensor_commands"
+    warehouse_sqlite_path_param_name = "warehouse_sqlite_path"
+    start_servo_param_name = "start_servo"
+
     # Declare command-line arguments
     declared_arguments = []
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "rizon_type",
+            rizon_type_param_name,
             description="Type of the Flexiv Rizon robot.",
             default_value="rizon4",
             choices=["rizon4", "rizon4s", "rizon10"],
@@ -43,37 +53,37 @@ def generate_launch_description():
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "robot_ip",
+            robot_ip_param_name,
             description="IP address of the robot server (remote).",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "local_ip",
+            local_ip_param_name,
             description="IP address of the workstation PC (local).",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "start_rviz",
+            start_rviz_param_name,
             default_value="true",
             description="start RViz automatically with the launch file",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "use_fake_hardware",
+            use_fake_hardware_param_name,
             default_value="false",
             description="Start robot with fake hardware mirroring command to its states.",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "fake_sensor_commands",
+            fake_sensor_commands_param_name,
             default_value="false",
             description="Enable fake command interfaces for sensors used for simple simulations. \
             Used only if 'use_fake_hardware' parameter is true.",
@@ -82,28 +92,28 @@ def generate_launch_description():
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "warehouse_sqlite_path",
+            warehouse_sqlite_path_param_name,
             default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
             description="Path to the sqlite database used by the warehouse_ros package.",
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            "start_servo",
+            start_servo_param_name,
             default_value="false",
             description="Start the MoveIt servo node?",
         )
     )
 
-    rizon_type = LaunchConfiguration("rizon_type")
-    robot_ip = LaunchConfiguration("robot_ip")
-    local_ip = LaunchConfiguration("local_ip")
-    start_rviz = LaunchConfiguration("start_rviz")
-    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
-    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
-    warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
-    start_servo = LaunchConfiguration("start_servo")
+    rizon_type = LaunchConfiguration(rizon_type_param_name)
+    robot_ip = LaunchConfiguration(robot_ip_param_name)
+    local_ip = LaunchConfiguration(local_ip_param_name)
+    start_rviz = LaunchConfiguration(start_rviz_param_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
+    warehouse_sqlite_path = LaunchConfiguration(warehouse_sqlite_path_param_name)
+    start_servo = LaunchConfiguration(start_servo_param_name)
 
     # Get URDF via xacro
     flexiv_urdf_xacro = PathJoinSubstitution(
@@ -295,6 +305,17 @@ def generate_launch_description():
         ],
     )
 
+    # Run tcp pose state broadcaster
+    tcp_pose_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "tcp_pose_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
     # Servo node for realtime control
     servo_yaml = load_yaml(
         "flexiv_moveit_config", "config/rizon_moveit_servo_config.yaml"
@@ -313,14 +334,33 @@ def generate_launch_description():
         output="screen",
     )
 
+    # 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 = [
         move_group_node,
         robot_state_publisher_node,
         ros2_control_node,
         joint_state_broadcaster_spawner,
-        robot_controller_spawner,
-        rviz_node,
+        tcp_pose_state_broadcaster_spawner,
         servo_node,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
     ]
 
     return LaunchDescription(declared_arguments + nodes)