refactor(rbs_skill_servers): update action configuration, dependencies, and skills

- Removed `assembly_config_service.py` node from launch configuration.
- Added default `goal.duration` setting to `MoveToPose` and `MoveToPoseArray`.
- Replaced `timeout_seconds` with `duration` in action definitions for `MoveitSendJointStates` and `MoveitSendPose`.
- Removed dependencies on TinyXML2 and Gazebo/SDFormat, adding `controller_manager_msgs` and `control_msgs` to CMake configuration.
- Added new action servers `cartesian_move_to_pose` and `move_to_joint_states`, registering them in CMakeLists file.
- Introduced `SkillBase`, a template class for managing action-based skills, providing essential ROS 2 action server support and functionalities for handling goals, cancels, accepted actions, and controller management.
- Implemented methods to load, configure, and switch required controllers with conflict detection for active controllers, along with parameter checking and asynchronous handling for required parameters.
- Enhanced error handling for missing controllers, parameters, and resource conflicts.
- Updated `skills.launch.py` to utilize `ComposableNodeContainer` for skill nodes, incorporating `MoveToJointStateActionServer` and `CartesianMoveToPose` as composable nodes.
- Changed the executable name in `cartesian_move_to_pose_action_server` node configuration.
- Added `cartesian_move_to_pose.cpp`, implementing the `CartesianMoveToPose` action server, including trajectory interpolation, pose adjustment, and controller management.
- Updated `package.xml` to include `rclcpp_components` dependency.
- Refactored `MoveToJointStateActionServer` to extend `SkillBase`, leveraging `FollowJointTrajectory` for joint trajectory execution, while removing redundant code and dependencies.
- Implemented trajectory generation based on initial and target joint positions with parameterized interpolation for smoother execution, enhancing joint state handling to dynamically align current and target joint values.

rbs_bringup/launch/rbs_robot.launch.py

@@ -21,26 +21,18 @@ from launch_ros.substitutions import FindPackageShare
 
 
 def launch_setup(context, *args, **kwargs):
-    # Initialize Arguments
     robot_type = LaunchConfiguration("robot_type")
-    # General arguments
-    launch_rviz = LaunchConfiguration("launch_rviz")
     with_gripper_condition = LaunchConfiguration("with_gripper")
     controllers_file = LaunchConfiguration("controllers_file")
-    cartesian_controllers = LaunchConfiguration("cartesian_controllers")
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
     robot_name = LaunchConfiguration("robot_name")
-    start_joint_controller = LaunchConfiguration("start_joint_controller")
-    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
-    launch_moveit = LaunchConfiguration("launch_moveit")
-    launch_task_planner = LaunchConfiguration("launch_task_planner")
-    launch_perception = LaunchConfiguration("launch_perception")
+    use_moveit = LaunchConfiguration("use_moveit")
     moveit_config_package = LaunchConfiguration("moveit_config_package")
     moveit_config_file = LaunchConfiguration("moveit_config_file")
     use_sim_time = LaunchConfiguration("use_sim_time")
     hardware = LaunchConfiguration("hardware")
-    launch_controllers = LaunchConfiguration("launch_controllers")
+    use_controllers = LaunchConfiguration("use_controllers")
     gripper_name = LaunchConfiguration("gripper_name")
     x_pos = LaunchConfiguration("x")
     y_pos = LaunchConfiguration("y")
@@ -58,6 +50,9 @@ def launch_setup(context, *args, **kwargs):
     controllers_file = controllers_file.perform(context)
     multi_robot = multi_robot.perform(context)
     robot_description = LaunchConfiguration("robot_description")
+    robot_description_semantic = LaunchConfiguration("robot_description_semantic")
+    control_space = LaunchConfiguration("control_space")
+    control_strategy = LaunchConfiguration("control_strategy")
 
     remappings = []
     if multi_robot == "true":
@@ -93,45 +88,45 @@ def launch_setup(context, *args, **kwargs):
 
     robot_description = {"robot_description": robot_description_content}
 
-    robot_description_semantic_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare(moveit_config_package),
-                    "config/moveit",
-                    "rbs_arm.srdf.xacro",
-                ]
-            ),
-            " ",
-            "name:=",
-            robot_type,
-            " ",
-            "with_gripper:=",
-            with_gripper_condition,
-            " ",
-            "gripper_name:=",
-            gripper_name,
-            " ",
-        ]
-    )
+    if not robot_description_semantic.perform(context):
+        robot_description_semantic_content = Command(
+            [
+                PathJoinSubstitution([FindExecutable(name="xacro")]),
+                " ",
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare(moveit_config_package),
+                        "config/moveit",
+                        "rbs_arm.srdf.xacro",
+                    ]
+                ),
+                " ",
+                "name:=",
+                robot_type,
+                " ",
+                "with_gripper:=",
+                with_gripper_condition,
+                " ",
+                "gripper_name:=",
+                gripper_name,
+                " ",
+            ]
+        )
+    else:
+        robot_description_semantic_content = robot_description_semantic
 
     robot_description_semantic = {
         "robot_description_semantic": robot_description_semantic_content
     }
+
     robot_description_kinematics = PathJoinSubstitution(
         [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
     )
 
-    # kinematics_yaml = load_yaml("rbs_arm", "config/kinematics.yaml")
-
     robot_description_kinematics = {
         "robot_description_kinematics": robot_description_kinematics
     }
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
-    )
+
     robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
@@ -141,26 +136,6 @@ def launch_setup(context, *args, **kwargs):
         parameters=[{"use_sim_time": use_sim_time}, robot_description],
     )
 
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
-    )
-
-    rviz = Node(
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        output="log",
-        namespace=namespace,
-        arguments=["-d", rviz_config_file],
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ],
-        condition=IfCondition(launch_rviz),
-    )
-
     control = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
@@ -174,12 +149,12 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments={
-            "control_space": "task",
-            "control_strategy": "position",
-            "has_gripper": "true",
+            "control_space": control_space,
+            "control_strategy": control_strategy,
+            "has_gripper": with_gripper_condition,
             "namespace": namespace,
         }.items(),
-        condition=IfCondition(launch_controllers),
+        condition=IfCondition(use_controllers),
     )
 
     moveit = IncludeLaunchDescription(
@@ -204,7 +179,7 @@ def launch_setup(context, *args, **kwargs):
             "robot_description_semantic": robot_description_semantic_content,
             "namespace": namespace,
         }.items(),
-        condition=IfCondition(launch_moveit),
+        condition=IfCondition(use_moveit),
     )
 
     skills = IncludeLaunchDescription(
@@ -226,141 +201,77 @@ def launch_setup(context, *args, **kwargs):
             "use_sim_time": use_sim_time,
             "with_gripper_condition": with_gripper_condition,
             "namespace": namespace,
+            "use_moveit": use_moveit,
         }.items(),
     )
 
-    task_planner = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("rbs_task_planner"),
-                        "launch",
-                        "task_planner.launch.py",
-                    ]
-                )
-            ]
-        ),
-        launch_arguments={
-            # TBD
-        }.items(),
-        condition=IfCondition(launch_task_planner),
-    )
-
-    perception = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("rbs_perception"),
-                        "launch",
-                        "perception.launch.py",
-                    ]
-                )
-            ]
-        ),
-        launch_arguments={
-            # TBD
-        }.items(),
-        condition=IfCondition(launch_perception),
-    )
-
-    asm_config = Node(
-        package="rbs_utils",
-        namespace=namespace,
-        executable="assembly_config_service.py",
-    )
-
     nodes_to_start = [
         robot_state_publisher,
         control,
         moveit,
         skills,
-        asm_config,
-        # task_planner,
-        # perception,
-        rviz,
     ]
     return nodes_to_start
 
 
 def generate_launch_description():
     declared_arguments = []
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_type",
-            description="Type of robot by name",
+            description="Type of robot to launch, specified by name.",
             choices=["rbs_arm", "ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
             default_value="rbs_arm",
         )
     )
-    # General arguments
     declared_arguments.append(
         DeclareLaunchArgument(
             "controllers_file",
             default_value="rbs_arm0_controllers.yaml",
-            description="YAML file with the controllers configuration.",
+            description="YAML file containing configuration settings for the controllers.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_package",
             default_value="rbs_arm",
-            description="Description package with robot URDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom description.",
+            description="Package containing the robot's URDF/XACRO description files. Can be set to use a custom description.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "description_file",
             default_value="rbs_arm_modular.xacro",
-            description="URDF/XACRO description file with the robot.",
+            description="URDF/XACRO file describing the robot's model.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_name",
             default_value="arm0",
-            description="tf_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(
-            "start_joint_controller",
-            default_value="true",
-            description="Enable headless mode for robot control",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "initial_joint_controller",
-            default_value="joint_trajectory_controller",
-            description="Robot controller to start.",
+            description="Prefix for joint names to support multi-robot setups. If changed, ensure joint names in controllers configuration are updated accordingly.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_package",
             default_value="rbs_arm",
-            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom moveit config.",
+            description="Package containing the MoveIt configuration files (SRDF/XACRO) for the robot. Can be customized.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_file",
             default_value="rbs_arm.srdf.xacro",
-            description="MoveIt SRDF/XACRO description file with the robot.",
+            description="MoveIt SRDF/XACRO file for robot configuration.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_sim_time",
             default_value="true",
-            description="Make MoveIt to use simulation time.\
-            This is needed for the trajectory planing in simulation.",
+            description="Enables simulation time in MoveIt, needed for trajectory planning in simulation.",
         )
     )
     declared_arguments.append(
@@ -368,55 +279,21 @@ def generate_launch_description():
             "gripper_name",
             default_value="",
             choices=["rbs_gripper", ""],
-            description="choose gripper by name (leave empty if hasn't)",
+            description="Specify gripper name (leave empty if none).",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "with_gripper", default_value="true", description="With gripper or not?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_rviz", default_value="false", description="Launch RViz?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "sim_gazebo", default_value="true", description="Gazebo Simulation"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_sim",
+            "with_gripper",
             default_value="true",
-            description="Launch simulator (Gazebo)?\
-                              Most general arg",
+            description="Specify if the robot includes a gripper.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_moveit", default_value="true", description="Launch moveit?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_perception", default_value="false", description="Launch perception?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_task_planner",
-            default_value="false",
-            description="Launch task_planner?",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "cartesian_controllers",
-            default_value="false",
-            description="Load cartesian\
-                              controllers?",
+            "use_moveit",
+            default_value="true",
+            description="Specify if MoveIt should be launched.",
         )
     )
     declared_arguments.append(
@@ -424,85 +301,100 @@ def generate_launch_description():
             "hardware",
             choices=["gazebo", "mock"],
             default_value="gazebo",
-            description="Choose your harware_interface",
+            description="Specify the hardware interface to use (e.g., Gazebo or mock).",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "launch_controllers",
+            "use_controllers",
             default_value="true",
-            description="Launch controllers?",
+            description="Specify if controllers should be launched.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "gazebo_gui", default_value="false", description="Launch gazebo with gui?"
+            "namespace",
+            default_value="",
+            description="ROS 2 namespace for the robot.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "namespace", default_value="", description="The ROS2 namespace of a robot"
+            "x",
+            default_value="0.0",
+            description="X-coordinate for the robot's position in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "x", default_value="0.0", description="Position of robot in world by X"
+            "y",
+            default_value="0.0",
+            description="Y-coordinate for the robot's position in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "y", default_value="0.0", description="Position of robot in world by Y"
+            "z",
+            default_value="0.0",
+            description="Z-coordinate for the robot's position in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "z", default_value="0.0", description="Position of robot in world by Z"
+            "roll",
+            default_value="0.0",
+            description="Roll orientation of the robot in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "roll", default_value="0.0", description="Position of robot in world by Z"
+            "pitch",
+            default_value="0.0",
+            description="Pitch orientation of the robot in the world.",
         )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "pitch", default_value="0.0", description="Position of robot in world by Z"
+            "yaw",
+            default_value="0.0",
+            description="Yaw orientation of the robot in the world.",
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "yaw", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "roll", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "pitch", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "yaw", default_value="0.0", description="Position of robot in world by Z"
-        )
-    )
-
     declared_arguments.append(
         DeclareLaunchArgument(
             "multi_robot",
             default_value="false",
-            description="Flag if you use multi robot setup",
+            description="Specify if the setup is for multiple robots.",
         )
     )
-
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_description",
             default_value="",
-            description="Robot description that override internal robot desctioption",
+            description="Custom robot description that overrides the internal default.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_space",
+            default_value="task",
+            choices=["task", "joint"],
+            description="Specify the control space for the robot (e.g., task space).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "control_strategy",
+            default_value="position",
+            choices=["position", "velocity", "effort"],
+            description="Specify the control strategy (e.g., position control).",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description_semantic",
+            default_value="",
+            description="Custom semantic robot description (SRDF) to override the default.",
         )
     )