Merge branch 'main' into feat/add-realsense-gazebo-plugin

rcdt_utilities/launch/moveit.launch.py

@@ -5,35 +5,32 @@
 from launch import LaunchDescription, LaunchContext, LaunchDescriptionEntity
 from launch.actions import OpaqueFunction, ExecuteProcess
 from launch_ros.actions import Node
-from rcdt_utilities.launch_utils import LaunchArgument, get_yaml, get_file_path
-from moveit_configs_utils import MoveItConfigsBuilder
+from rcdt_utilities.launch_utils import (
+    LaunchArgument,
+    get_yaml,
+    get_file_path,
+    get_moveit_parameters,
+)
 
 SKIP = LaunchDescriptionEntity()
 
 use_sim_arg = LaunchArgument("simulation", True, [True, False])
 moveit_mode_arg = LaunchArgument("moveit", "off", ["node", "rviz", "servo", "off"])
-moveit_config_package_arg = LaunchArgument(
-    "moveit_config_package", "rcdt_franka_moveit_config"
-)
+moveit_package_name_arg = LaunchArgument("moveit_package_name", "")
 servo_params_package_arg = LaunchArgument("servo_params_package", "rcdt_franka")
 
 
 def launch_setup(context: LaunchContext) -> None:
     use_sim = use_sim_arg.value(context)
     moveit_mode = moveit_mode_arg.value(context)
-    config_package = moveit_config_package_arg.value(context)
+    package_name = moveit_package_name_arg.value(context)
     servo_params_package = servo_params_package_arg.value(context)
 
-    # Create moveit config dictionary:
-    moveit_config = MoveItConfigsBuilder(robot_name="fr3", package_name=config_package)
-    if moveit_mode == "node":
-        moveit_config.trajectory_execution(
-            get_file_path(config_package, ["config"], "moveit_controllers.yaml")
-        )
-        moveit_config.moveit_cpp(
-            get_file_path(config_package, ["config"], "planning_pipeline.yaml")
-        )
-    moveit_config = moveit_config.to_dict()
+    moveit_config = get_moveit_parameters(
+        robot_name="fr3",
+        package_name=package_name,
+        mode=moveit_mode,
+    )
 
     # Moveit as node:
     moveit_node = Node(
@@ -49,14 +46,6 @@ def launch_setup(context: LaunchContext) -> None:
         parameters=[moveit_config],
     )
 
-    display_config_moveit = get_file_path("rcdt_utilities", ["rviz"], "moveit.rviz")
-    rviz = Node(
-        package="rviz2",
-        executable="rviz2",
-        arguments=["--display-config", display_config_moveit],
-        parameters=[moveit_config],
-    )
-
     # Moveit servo:
     file = get_file_path(servo_params_package, ["config"], "servo_params.yaml")
     servo_config = get_yaml(file)
@@ -93,16 +82,16 @@ def launch_setup(context: LaunchContext) -> None:
 
     return [
         moveit,
-        rviz if moveit_mode == "rviz" else SKIP,
         set_servo_command_type if moveit_mode == "servo" else SKIP,
     ]
 
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
+            use_sim_arg.declaration,
             moveit_mode_arg.declaration,
-            moveit_config_package_arg.declaration,
+            moveit_package_name_arg.declaration,
             servo_params_package_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]

rcdt_utilities/launch/rviz.launch.py

@@ -5,35 +5,66 @@
 from launch import LaunchDescription, LaunchContext
 from launch.actions import OpaqueFunction
 from launch_ros.actions import Node
-from rcdt_utilities.launch_utils import LaunchArgument, get_file_path
-
-display_config_general = get_file_path("rcdt_utilities", ["rviz"], "general.rviz")
+from rcdt_utilities.launch_utils import (
+    LaunchArgument,
+    get_file_path,
+    get_moveit_parameters,
+)
 
 rviz_frame_arg = LaunchArgument("rviz_frame", "world")
-rviz_display_config = LaunchArgument("rviz_display_config", display_config_general)
+moveit_mode_arg = LaunchArgument("moveit", "off", ["node", "rviz", "servo", "off"])
+moveit_package_name_arg = LaunchArgument("moveit_package_name", "")
 
 
 def launch_setup(context: LaunchContext) -> None:
-    arguments = ["--display-config", rviz_display_config.value(context)]
-
     rviz_frame = rviz_frame_arg.value(context)
+    moveit_mode = moveit_mode_arg.value(context)
+    moveit_package_name = moveit_package_name_arg.value(context)
+
+    arguments = []
     if rviz_frame != "":
         arguments.extend(["-f", rviz_frame])
+    display_config = get_file_path("rcdt_utilities", ["rviz"], "general.rviz")
+    match moveit_mode:
+        case "rviz":
+            display_config = get_file_path("rcdt_utilities", ["rviz"], "moveit.rviz")
+        case "node":
+            display_config = get_file_path("rcdt_utilities", ["rviz"], "planning.rviz")
+    arguments.extend(["--display-config", display_config])
+
+    parameters = []
+    if moveit_mode != "off":
+        moveit_config = get_moveit_parameters(
+            robot_name="fr3",
+            package_name=moveit_package_name,
+            mode=moveit_mode,
+        )
+        parameters.append(moveit_config)
 
     rviz = Node(
         package="rviz2",
         executable="rviz2",
         arguments=arguments,
+        parameters=parameters,
+    )
+
+    rviz_controller = Node(
+        package="rcdt_utilities",
+        executable="rviz_controller_node",
     )
 
-    return [rviz]
+    return [
+        rviz,
+        rviz_controller,
+    ]
 
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [
             rviz_frame_arg.declaration,
-            rviz_display_config.declaration,
+            moveit_mode_arg.declaration,
+            moveit_package_name_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]
     )

rcdt_utilities/rcdt_utilities/launch_utils.py

@@ -2,7 +2,7 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-from typing import List
+from typing import List, Literal
 import os
 import yaml
 import xacro
@@ -61,5 +61,18 @@ def get_robot_description(xacro_path: str, xacro_arguments: dict = None) -> str:
     return {"robot_description": robot_description_config.toxml()}
 
 
-def get_moveit_parameters(robot_name: str, package_name: str) -> dict:
-    return MoveItConfigsBuilder(robot_name, package_name=package_name).to_dict()
+def get_moveit_parameters(
+    robot_name: str,
+    package_name: str,
+    mode: Literal["off", "rviz", "sevo", "node"] = "off",
+) -> dict:
+    moveit_config = MoveItConfigsBuilder(robot_name, package_name=package_name)
+    match mode:
+        case "node":
+            moveit_config.trajectory_execution(
+                get_file_path(package_name, ["config"], "moveit_controllers.yaml")
+            )
+            moveit_config.moveit_cpp(
+                get_file_path(package_name, ["config"], "planning_pipeline.yaml")
+            )
+    return moveit_config.to_dict()