added multi move-group capability

lbr_bringup/launch/move_group.launch.py

@@ -1,14 +1,14 @@
 from typing import List
 
-from launch import LaunchDescription, LaunchDescriptionEntity
+from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
 from launch.actions import OpaqueFunction
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 
 from lbr_bringup import LBRMoveGroupMixin
 from lbr_description import LBRDescriptionMixin, RVizMixin
 
 
-def launch_setup(context) -> List[LaunchDescriptionEntity]:
+def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     ld = LaunchDescription()
 
     ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
@@ -19,19 +19,45 @@ def launch_setup(context) -> List[LaunchDescriptionEntity]:
 
     model = LaunchConfiguration("model").perform(context)
     moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
-        model,
+        robot_name=model,
+        base_frame=LaunchConfiguration("base_frame"),
         package_name=f"{model}_moveit_config",
     )
     movegroup_params = LBRMoveGroupMixin.params_move_group()
 
-    # MoveGroup
+    # MoveGroup:
+    # - requires world frame
+    # - maps link robot_name/base_frame -> base_frame
+    # These two transform need publishing
+    robot_name = LaunchConfiguration("robot_name").perform(context)
+    ld.add_action(
+        LBRDescriptionMixin.node_static_tf(
+            tf=[0, 0, 0, 0, 0, 0],
+            parent="world",
+            child=LaunchConfiguration("base_frame"),
+        )
+    )
+    ld.add_action(
+        LBRDescriptionMixin.node_static_tf(
+            tf=[0, 0, 0, 0, 0, 0],  # keep zero
+            parent=LaunchConfiguration("base_frame"),
+            child=PathJoinSubstitution(
+                [
+                    LaunchConfiguration("robot_name"),
+                    LaunchConfiguration("base_frame"),
+                ]  # results in robot_name/base_frame
+            ),
+        )
+    )
+
     ld.add_action(
         LBRMoveGroupMixin.node_move_group(
             parameters=[
                 moveit_configs_builder.to_dict(),
                 movegroup_params,
                 {"use_sim_time": LaunchConfiguration("sim")},
             ],
+            namespace=robot_name,
         )
     )
 
@@ -42,6 +68,12 @@ def launch_setup(context) -> List[LaunchDescriptionEntity]:
         parameters=LBRMoveGroupMixin.params_rviz(
             moveit_configs=moveit_configs_builder.to_moveit_configs()
         ),
+        remappings=[
+            ("robot_description", robot_name + "/robot_description"),
+            ("robot_description_semantic", robot_name + "/robot_description_semantic"),
+            ("display_planned_path", robot_name + "/display_planned_path"),
+            ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
+        ],
     )
 
     ld.add_action(rviz)
@@ -54,6 +86,7 @@ def generate_launch_description() -> LaunchDescription:
 
     ld.add_action(LBRDescriptionMixin.arg_model())
     ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    ld.add_action(LBRDescriptionMixin.arg_base_frame())
     ld.add_action(LBRDescriptionMixin.arg_sim())
 
     ld.add_action(OpaqueFunction(function=launch_setup))

lbr_bringup/launch/real.launch.py

@@ -4,7 +4,12 @@
 from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
 from launch.conditions import IfCondition
 from launch.event_handlers import OnProcessExit, OnProcessStart
-from launch.substitutions import AndSubstitution, LaunchConfiguration, NotSubstitution
+from launch.substitutions import (
+    AndSubstitution,
+    LaunchConfiguration,
+    NotSubstitution,
+    PathJoinSubstitution,
+)
 
 from lbr_bringup import LBRMoveGroupMixin
 from lbr_description import LBRDescriptionMixin, RVizMixin
@@ -50,9 +55,39 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics())
     ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene())
 
+    robot_name = LaunchConfiguration("robot_name").perform(context)
+
+    # MoveGroup:
+    # - requires world frame
+    # - maps link robot_name/base_frame -> base_frame
+    # These two transform need publishing
+    robot_name = LaunchConfiguration("robot_name").perform(context)
+    ld.add_action(
+        LBRDescriptionMixin.node_static_tf(
+            tf=[0, 0, 0, 0, 0, 0],
+            parent="world",
+            child=LaunchConfiguration("base_frame"),
+            condition=IfCondition(LaunchConfiguration("moveit")),
+        ),
+    )
+    ld.add_action(
+        LBRDescriptionMixin.node_static_tf(
+            tf=[0, 0, 0, 0, 0, 0],  # keep zero
+            parent=LaunchConfiguration("base_frame"),
+            child=PathJoinSubstitution(
+                [
+                    LaunchConfiguration("robot_name"),
+                    LaunchConfiguration("base_frame"),
+                ]  # results in robot_name/base_frame
+            ),
+            condition=IfCondition(LaunchConfiguration("moveit")),
+        )
+    )
+
     model = LaunchConfiguration("model").perform(context)
     moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
-        model,
+        robot_name=model,
+        base_frame=LaunchConfiguration("base_frame"),
         package_name=f"{model}_moveit_config",
     )
     movegroup_params = LBRMoveGroupMixin.params_move_group()
@@ -65,6 +100,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
                 {"use_sim_time": False},
             ],
             condition=IfCondition(LaunchConfiguration("moveit")),
+            namespace=robot_name,
         )
     )
 
@@ -78,6 +114,12 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
         condition=IfCondition(
             AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz"))
         ),
+        remappings=[
+            ("robot_description", robot_name + "/robot_description"),
+            ("robot_description_semantic", robot_name + "/robot_description_semantic"),
+            ("display_planned_path", robot_name + "/display_planned_path"),
+            ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
+        ],
     )
 
     # RViz no MoveIt
@@ -107,6 +149,7 @@ def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()
     ld.add_action(LBRDescriptionMixin.arg_model())
     ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    ld.add_action(LBRDescriptionMixin.arg_base_frame())
     ld.add_action(LBRDescriptionMixin.arg_port_id())
     ld.add_action(
         DeclareLaunchArgument(

lbr_bringup/launch/sim.launch.py

@@ -21,7 +21,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     joint_state_broadcaster = LBRSystemInterfaceMixin.node_joint_state_broadcaster()
     ld.add_action(joint_state_broadcaster)
     robot_state_publisher = LBRSystemInterfaceMixin.node_robot_state_publisher(
-        robot_description=robot_description, use_sim_time=True, frame_prefix=""
+        robot_description=robot_description, use_sim_time=True
     )
     ld.add_action(
         robot_state_publisher

lbr_bringup/lbr_bringup/launch_mixin.py

@@ -54,16 +54,21 @@ def arg_monitor_dynamics() -> DeclareLaunchArgument:
 
     @staticmethod
     def moveit_configs_builder(
-        robot_name: str, package_name: str
+        robot_name: str, base_frame: str, package_name: str
     ) -> MoveItConfigsBuilder:
-        return MoveItConfigsBuilder(
-            robot_name=robot_name,
-            package_name=package_name,
-        ).robot_description(
-            os.path.join(
-                get_package_share_directory("lbr_description"),
-                f"urdf/{robot_name}/{robot_name}.urdf.xacro",
+        return (
+            MoveItConfigsBuilder(
+                robot_name=robot_name,
+                package_name=package_name,
             )
+            .robot_description(
+                os.path.join(
+                    get_package_share_directory("lbr_description"),
+                    f"urdf/{robot_name}/{robot_name}.urdf.xacro",
+                ),
+                mappings={"base_frame": base_frame},
+            )
+            .planning_pipelines(default_planning_pipeline="ompl", pipelines=["ompl"])
         )
 
     @staticmethod

lbr_description/lbr_description/launch_mixin.py

@@ -1,4 +1,4 @@
-from typing import Dict, Optional, Union
+from typing import Dict, List, Optional, Union
 
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -25,7 +25,7 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription:
                     ]
                 )
             ),
-            **kwargs
+            **kwargs,
         )
 
     @staticmethod
@@ -44,7 +44,7 @@ def node_spawn_entity(
                 LaunchConfiguration("robot_name"),
             ],
             output="screen",
-            **kwargs
+            **kwargs,
         )
 
 
@@ -60,7 +60,7 @@ def param_robot_description(
         if model is None:
             model = LaunchConfiguration("model", default="iiwa7")
         if base_frame is None:
-            base_frame = LaunchConfiguration("base_frame", default="world")
+            base_frame = LaunchConfiguration("base_frame", default="base_frame")
         if robot_name is None:
             robot_name = LaunchConfiguration("robot_name", default="lbr")
         if port_id is None:
@@ -106,7 +106,7 @@ def arg_model(default_value: str = "iiwa7") -> DeclareLaunchArgument:
         )
 
     @staticmethod
-    def arg_base_frame(default_value: str = "world") -> DeclareLaunchArgument:
+    def arg_base_frame(default_value: str = "base_frame") -> DeclareLaunchArgument:
         return DeclareLaunchArgument(
             name="base_frame",
             default_value=default_value,
@@ -140,7 +140,7 @@ def arg_sim(default_value: str = "true") -> DeclareLaunchArgument:
 
     @staticmethod
     def param_base_frame() -> Dict[str, LaunchConfiguration]:
-        return {"base_frame": LaunchConfiguration("base_frame", default="world")}
+        return {"base_frame": LaunchConfiguration("base_frame", default="base_frame")}
 
     @staticmethod
     def param_robot_name() -> Dict[str, LaunchConfiguration]:
@@ -154,6 +154,29 @@ def param_port_id() -> Dict[str, LaunchConfiguration]:
     def param_sim() -> Dict[str, LaunchConfiguration]:
         return {"sim": LaunchConfiguration("sim", default="true")}
 
+    @staticmethod
+    def node_static_tf(
+        tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+        parent: Optional[Union[LaunchConfiguration, str]] = None,
+        child: Optional[Union[LaunchConfiguration, str]] = None,
+        **kwargs,
+    ) -> Node:
+        label = ["--x", "--y", "--z", "--roll", "--pitch", "--yaw"]
+        tf = [str(x) for x in tf]
+        return Node(
+            package="tf2_ros",
+            executable="static_transform_publisher",
+            output="screen",
+            arguments=[item for pair in zip(label, tf) for item in pair]
+            + [
+                "--frame-id",
+                parent,
+                "--child-frame-id",
+                child,
+            ],
+            **kwargs,
+        )
+
 
 class RVizMixin:
     @staticmethod
@@ -180,7 +203,7 @@ def arg_rviz_config(
     def node_rviz(
         rviz_config_pkg: Optional[Union[LaunchConfiguration, str]] = None,
         rviz_config: Optional[Union[LaunchConfiguration, str]] = None,
-        **kwargs
+        **kwargs,
     ) -> Node:
         if rviz_config_pkg is None:
             rviz_config_pkg = LaunchConfiguration(
@@ -203,5 +226,5 @@ def node_rviz(
                     ]
                 ),
             ],
-            **kwargs
+            **kwargs,
         )

lbr_description/urdf/iiwa14/iiwa14.urdf.xacro

@@ -6,7 +6,7 @@
     <!-- include the lbr iiwa macro -->
     <xacro:include filename="$(find lbr_description)/urdf/iiwa14/iiwa14_description.urdf.xacro" />
 
-    <xacro:arg name="base_frame" default="world" />
+    <xacro:arg name="base_frame" default="base_frame" />
     <xacro:arg name="robot_name" default="lbr" />
     <xacro:arg name="port_id" default="30200" />
     <xacro:arg name="sim" default="true" />

lbr_description/urdf/iiwa7/iiwa7.urdf.xacro

@@ -6,7 +6,7 @@
     <!-- include the lbr iiwa macro -->
     <xacro:include filename="$(find lbr_description)/urdf/iiwa7/iiwa7_description.urdf.xacro" />
 
-    <xacro:arg name="base_frame" default="world" />
+    <xacro:arg name="base_frame" default="base_frame" />
     <xacro:arg name="robot_name" default="lbr" />
     <xacro:arg name="port_id" default="30200" />
     <xacro:arg name="sim" default="true" />

lbr_description/urdf/med14/med14.urdf.xacro

@@ -6,7 +6,7 @@
     <!-- include the lbr med macro -->
     <xacro:include filename="$(find lbr_description)/urdf/med14/med14_description.urdf.xacro" />
 
-    <xacro:arg name="base_frame" default="world" />
+    <xacro:arg name="base_frame" default="base_frame" />
     <xacro:arg name="robot_name" default="lbr" />
     <xacro:arg name="port_id" default="30200" />
     <xacro:arg name="sim" default="true" />

lbr_description/urdf/med7/med7.urdf.xacro

@@ -6,7 +6,7 @@
     <!-- include the lbr med macro -->
     <xacro:include filename="$(find lbr_description)/urdf/med7/med7_description.urdf.xacro" />
 
-    <xacro:arg name="base_frame" default="world" />
+    <xacro:arg name="base_frame" default="base_frame" />
     <xacro:arg name="robot_name" default="lbr" />
     <xacro:arg name="port_id" default="30200" />
     <xacro:arg name="sim" default="true" />

lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz

@@ -38,7 +38,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      Move Group Namespace: ""
+      Move Group Namespace: lbr
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false

lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz

@@ -38,7 +38,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      Move Group Namespace: ""
+      Move Group Namespace: lbr
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false

lbr_moveit_config/med14_moveit_config/config/moveit.rviz

@@ -38,7 +38,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      Move Group Namespace: ""
+      Move Group Namespace: lbr
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false

lbr_moveit_config/med7_moveit_config/config/moveit.rviz

@@ -38,7 +38,7 @@ Visualization Manager:
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
       Enabled: true
-      Move Group Namespace: ""
+      Move Group Namespace: lbr
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
       MoveIt_Allow_Replanning: false