re-termed mixin

lbr-stack · Aug 1, 2023 · 65c6888 · 65c6888
1 parent 23dbe6d
commit 65c6888
Show file tree

Hide file tree

Showing 17 changed files with 222 additions and 132 deletions.
diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py
@@ -1,14 +1,14 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
-from lbr_description import LBRDescriptionLaunch
+from lbr_description import LBRDescriptionMixin
 
 
 def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()
-    ld.add_action(LBRDescriptionLaunch.arg_model())
-    ld.add_action(LBRDescriptionLaunch.arg_robot_name())
-    robot_description = LBRDescriptionLaunch.description(sim=False)
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    robot_description = LBRDescriptionMixin.description(sim=False)
     ld.add_action(
         Node(
             package="lbr_demos_fri_ros2_advanced_cpp",

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py
@@ -1,14 +1,14 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
-from lbr_description import LBRDescriptionLaunch
+from lbr_description import LBRDescriptionMixin
 
 
 def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()
-    ld.add_action(LBRDescriptionLaunch.arg_model())
-    ld.add_action(LBRDescriptionLaunch.arg_robot_name())
-    robot_description = LBRDescriptionLaunch.description(sim=False)
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    robot_description = LBRDescriptionMixin.description(sim=False)
     ld.add_action(
         Node(
             package="lbr_demos_fri_ros2_advanced_python",

diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt
@@ -18,7 +18,7 @@ install(
 
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.dsv")
 
-# launch files
+# Launch mixins
 ament_python_install_package(${PROJECT_NAME})
 
 if(BUILD_TESTING)

diff --git a/lbr_description/gazebo/iiwa.gazebo.xacro b/lbr_description/gazebo/iiwa.gazebo.xacro
@@ -5,7 +5,7 @@
         <!-- ros_control-plugin -->
         <gazebo>
             <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-                <parameters>$(find lbr_bringup)/config/lbr_controllers.yml</parameters>
+                <parameters>$(find lbr_hardware_interface)/config/lbr_controllers.yml</parameters>
                 <robotNamespace>/${robot_name}</robotNamespace>
             </plugin>
         </gazebo>

diff --git a/lbr_description/launch/view_robot.launch.py b/lbr_description/launch/view_robot.launch.py
@@ -1,17 +1,14 @@
-import os
-
-from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
-from lbr_description import LBRDescriptionLaunch
+from lbr_description import LBRDescriptionMixin, RVizMixin
 
 
 def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()
-    ld.add_action(LBRDescriptionLaunch.arg_model())
-    ld.add_action(LBRDescriptionLaunch.arg_robot_name())
-    robot_description = LBRDescriptionLaunch.description(sim=True)
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    robot_description = LBRDescriptionMixin.description(sim=True)
     ld.add_action(
         Node(
             package="joint_state_publisher_gui",
@@ -27,15 +24,9 @@ def generate_launch_description() -> LaunchDescription:
         )
     )
     ld.add_action(
-        Node(
-            package="rviz2",
-            executable="rviz2",
-            arguments=[
-                "-d",
-                os.path.join(
-                    get_package_share_directory("lbr_description"), "config/config.rviz"
-                ),
-            ],
+        RVizMixin.node_rviz(
+            rviz_config_pkg="lbr_description",
+            rviz_config="config/config.rviz",
         )
     )
     return ld
diff --git a/lbr_description/lbr_description/__init__.py b/lbr_description/lbr_description/__init__.py
@@ -1 +1 @@
-from .launch_util import LBRDescriptionLaunch
+from .launch_mixin import GazeboMixin, LBRDescriptionMixin, RVizMixin
diff --git a/lbr_description/lbr_description/launch_mixin.py b/lbr_description/lbr_description/launch_mixin.py
@@ -0,0 +1,156 @@
+from typing import Dict, Optional, Union
+
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+class GazeboMixin:
+    @staticmethod
+    def include_gazebo(**kwargs) -> IncludeLaunchDescription:
+        return IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("gazebo_ros"),
+                        "launch",
+                        "gazebo.launch.py",
+                    ]
+                )
+            ),
+            **kwargs
+        )
+
+    @staticmethod
+    def node_spawn_entity(
+        robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs
+    ) -> Node:
+        if robot_name is None:
+            robot_name = LaunchConfiguration("robot_name")
+        return Node(
+            package="gazebo_ros",
+            executable="spawn_entity.py",
+            arguments=[
+                "-topic",
+                "robot_description",
+                "-entity",
+                LaunchConfiguration("robot_name"),
+            ],
+            output="screen",
+            **kwargs
+        )
+
+
+class LBRDescriptionMixin:
+    @staticmethod
+    def description(
+        model: Optional[Union[LaunchConfiguration, str]] = None,
+        robot_name: Optional[Union[LaunchConfiguration, str]] = None,
+        sim: Optional[Union[LaunchConfiguration, bool]] = None,
+    ) -> Dict[str, str]:
+        if model is None:
+            model = LaunchConfiguration("model")
+        if robot_name is None:
+            robot_name = LaunchConfiguration("robot_name")
+        if sim is None:
+            sim = LaunchConfiguration("sim")
+        if type(sim) is bool:
+            sim = "true" if sim else "false"
+        robot_description = {
+            "robot_description": Command(
+                [
+                    FindExecutable(name="xacro"),
+                    " ",
+                    PathJoinSubstitution(
+                        [
+                            FindPackageShare("lbr_description"),
+                            "urdf",
+                            model,
+                            model,
+                        ]
+                    ),
+                    ".urdf.xacro",
+                    " robot_name:=",
+                    robot_name,
+                    " sim:=",
+                    sim,
+                ]
+            )
+        }
+        return robot_description
+
+    @staticmethod
+    def arg_model() -> DeclareLaunchArgument:
+        return DeclareLaunchArgument(
+            name="model",
+            default_value="iiwa7",
+            description="The LBR model in use.",
+            choices=["iiwa7", "iiwa14", "med7", "med14"],
+        )
+
+    @staticmethod
+    def arg_robot_name() -> DeclareLaunchArgument:
+        return DeclareLaunchArgument(
+            name="robot_name",
+            default_value="lbr",
+            description="The robot's name.",
+        )
+
+    @staticmethod
+    def arg_sim() -> DeclareLaunchArgument:
+        return DeclareLaunchArgument(
+            name="sim",
+            default_value="true",
+            description="Whether to use the simulation or not.",
+        )
+
+
+class RVizMixin:
+    @staticmethod
+    def arg_rviz_config_pkg() -> DeclareLaunchArgument:
+        return DeclareLaunchArgument(
+            name="rviz_config_pkg",
+            default_value="lbr_description",
+            description="The RViz configuration file.",
+        )
+
+    @staticmethod
+    def arg_rviz_config() -> DeclareLaunchArgument:
+        return DeclareLaunchArgument(
+            name="rviz_config",
+            default_value="config/config.rviz",
+            description="The RViz configuration file.",
+        )
+
+    @staticmethod
+    def node_rviz(
+        rviz_config_pkg: Optional[Union[LaunchConfiguration, str]] = None,
+        rviz_config: Optional[Union[LaunchConfiguration, str]] = None,
+        **kwargs
+    ) -> Node:
+        if rviz_config_pkg is None:
+            rviz_config_pkg = LaunchConfiguration("rviz_config_pkg")
+        if rviz_config is None:
+            rviz_config = LaunchConfiguration("rviz_config")
+        return Node(
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            arguments=[
+                "-d",
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare(rviz_config_pkg),
+                        rviz_config,
+                    ]
+                ),
+            ],
+            **kwargs
+        )
diff --git a/lbr_description/lbr_description/launch_util.py b/lbr_description/lbr_description/launch_util.py
diff --git a/lbr_fri_ros2/CMakeLists.txt b/lbr_fri_ros2/CMakeLists.txt
@@ -7,6 +7,7 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 find_package(control_toolbox REQUIRED)
 find_package(fri REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
@@ -96,4 +97,7 @@ install(
   DESTINATION share/lbr_fri_ros2
 )
 
+# Launch mixins
+ament_python_install_package(${PROJECT_NAME})
+
 ament_package()
diff --git a/lbr_fri_ros2/launch/lbr_app.launch.py b/lbr_fri_ros2/launch/lbr_app.launch.py
@@ -1,21 +1,13 @@
 from launch import LaunchDescription
-from launch_ros.actions import Node
 
-from lbr_description import LBRDescriptionLaunch
+from lbr_fri_ros2 import LBRFRIROS2Mixin
+from lbr_description import LBRDescriptionMixin
 
 
 def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()
-    ld.add_action(LBRDescriptionLaunch.model_arg())
-    ld.add_action(LBRDescriptionLaunch.robot_name_arg())
-    robot_description = LBRDescriptionLaunch.description(sim=False)
-    ld.add_action(
-        Node(
-            package="lbr_fri_ros2",
-            executable="lbr_app",
-            emulate_tty=True,
-            output="screen",
-            parameters=[robot_description],
-        )
-    )
+    ld.add_action(LBRDescriptionMixin.arg_model())
+    ld.add_action(LBRDescriptionMixin.arg_robot_name())
+    robot_description = LBRDescriptionMixin.description(sim=False)
+    ld.add_action(LBRFRIROS2Mixin.node_lbr_app(robot_description=robot_description))
     return ld
diff --git a/lbr_fri_ros2/lbr_fri_ros2/__init__.py b/lbr_fri_ros2/lbr_fri_ros2/__init__.py
@@ -0,0 +1 @@
+from .launch_mixin import LBRFRIROS2Mixin
diff --git a/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py b/lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py
@@ -0,0 +1,16 @@
+from typing import Dict
+
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+
+
+class LBRFRIROS2Mixin:
+    @staticmethod
+    def node_lbr_app(robot_description: Dict[str, str]) -> DeclareLaunchArgument:
+        return Node(
+            package="lbr_fri_ros2",
+            executable="lbr_app",
+            emulate_tty=True,
+            output="screen",
+            parameters=[robot_description],
+        )