Merge pull request #121 from lbr-stack/dev-humble-ros2-control

Major ROS 2 control integration re-work

.github/workflows/build.yml

@@ -22,4 +22,4 @@ jobs:
         with:
           package-name: lbr_fri_ros2_stack
           target-ros2-distro: humble
-          vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yml
+          vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml

.gitignore

@@ -1,2 +1,3 @@
 .vscode
 **/__pycache__
+COLCON_IGNORE

README.md

@@ -28,11 +28,11 @@ Full documentation available [here](https://lbr-fri-ros2-stack-doc.readthedocs.i
 Install [colcon](https://docs.ros.org/en/humble/Tutorials/Colcon-Tutorial.html#install-colcon), [rosdep](https://docs.ros.org/en/crystal/Installation/Linux-Install-Binary.html#installing-and-initializing-rosdep) and [vcstool](https://github.com/dirk-thomas/vcstool#how-to-install-vcstool). Build this repository
 
 ```shell
-mkdir -p lbr_fri_ros2_stack_ws/src && cd lbr_fri_ros2_stack_ws
-wget https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yml -P src
-vcs import src < src/repos.yml
+mkdir -p lbr-stack/src && cd lbr-stack
+wget https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml -P src
+vcs import src < src/repos.yaml
 rosdep install --from-paths src --ignore-src -r -y
-colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
+colcon build --symlink-install
 ```
 Next, launch the simulation via
 ```shell

lbr_bringup/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.16.3)
+cmake_minimum_required(VERSION 3.22)
 project(lbr_bringup)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

lbr_bringup/config/config.rviz

@@ -9,7 +9,7 @@ Panels:
         - /RobotModel1
         - /RobotModel1/Description Topic1
       Splitter Ratio: 0.5
-    Tree Height: 549
+    Tree Height: 712
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -59,7 +59,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /robot_description
+        Value: /lbr/robot_description
       Enabled: true
       Links:
         All Links Enabled: true
@@ -119,14 +119,14 @@ Visualization Manager:
         Inertia: false
         Mass: false
       Name: RobotModel
-      TF Prefix: ""
+      TF Prefix: lbr
       Update Interval: 0
       Value: true
       Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 21; 21; 26
-    Fixed Frame: world
+    Fixed Frame: lbr/world
     Frame Rate: 30
   Name: root
   Tools:
@@ -169,33 +169,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 3.063441276550293
+      Distance: 3.38482666015625
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.2026004195213318
-        Y: -0.19916969537734985
-        Z: 0.42787155508995056
-      Focal Shape Fixed Size: true
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6053980588912964
+      Pitch: 0.7853981852531433
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.6253981590270996
+      Yaw: 0.7853981852531433
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015b00000356fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f00000356000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000356fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f00000356000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002ed00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c40000035600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -204,6 +204,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
-  X: 60
-  Y: 60
+  Width: 1850
+  X: 70
+  Y: 27

lbr_bringup/doc/lbr_bringup.rst

@@ -55,7 +55,7 @@ and select:
 - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` (will put the robot into a compliant mode)
 - ``FRI client command mode``: ``POSITION``
 
-Make sure that the ``update_rate`` in `lbr_controllers.yml <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_hardware_interface/config/lbr_controllers.yml>`_ is greater or equal ``100`` (``FRI send period``).
+Make sure that the ``update_rate`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_ is greater or equal ``100`` (``FRI send period``).
 
 For using other ``FRI send period``, also change the ``sample_time`` in the `lbr.ros2_control.xacro <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_description/ros2_control/lbr.ros2_control.xacro>`_ (automated in the future).
 
@@ -80,6 +80,6 @@ Noisy Execution
 ~~~~~~~~~~~~~~~
 Three main causes:
 
-- Frequency: Make sure the ``ros2_control_node`` runs at the same or a higher rate of the ``FRI send period``, change ``update_rate`` in `lbr_controllers.yml <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_hardware_interface/config/lbr_controllers.yml>`_. 
+- Frequency: Make sure the ``ros2_control_node`` runs at the same or a higher rate of the ``FRI send period``, change ``update_rate`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_. 
 - Standalone noise: Smoothing might be required, see :ref:`LBR Demos FRI ROS 2`.
 - Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username.

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,25 +4,30 @@
 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
-from lbr_hardware_interface import LBRHardwareInterfaceMixin
+from lbr_ros2_control import LBRSystemInterfaceMixin
 
 
 def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     ld = LaunchDescription()
 
     robot_description = LBRDescriptionMixin.param_robot_description(sim=False)
-    ros2_control_node = LBRHardwareInterfaceMixin.node_ros2_control(
+    ros2_control_node = LBRSystemInterfaceMixin.node_ros2_control(
         robot_description=robot_description
     )
     ld.add_action(ros2_control_node)
 
     # joint state broad caster and controller on ros2 control node start
-    joint_state_broadcaster = LBRHardwareInterfaceMixin.node_joint_state_broadcaster()
-    controller = LBRHardwareInterfaceMixin.node_controller()
+    joint_state_broadcaster = LBRSystemInterfaceMixin.node_joint_state_broadcaster()
+    controller = LBRSystemInterfaceMixin.node_controller()
 
     controller_event_handler = RegisterEventHandler(
         OnProcessStart(
@@ -33,8 +38,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     ld.add_action(controller_event_handler)
 
     # robot state publisher on joint state broadcaster spawn exit
-    robot_state_publisher = LBRHardwareInterfaceMixin.node_robot_state_publisher(
-        robot_description=robot_description, use_sim_time=False, frame_prefix=""
+    robot_state_publisher = LBRSystemInterfaceMixin.node_robot_state_publisher(
+        robot_description=robot_description, use_sim_time=False
     )
     robot_state_publisher_event_handler = RegisterEventHandler(
         OnProcessExit(
@@ -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,18 +114,24 @@ 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
-    ld.add_action(RVizMixin.arg_rviz_config_pkg())
-    ld.add_action(RVizMixin.arg_rviz_config())
     rviz = RVizMixin.node_rviz(
+        rviz_config_pkg="lbr_bringup",
+        rviz_config="config/config.rviz",
         condition=IfCondition(
             AndSubstitution(
                 LaunchConfiguration("rviz"),
                 NotSubstitution(LaunchConfiguration("moveit")),
             )
-        )
+        ),
     )
 
     # RViz event handler
@@ -107,6 +149,8 @@ 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(
             name="moveit",
@@ -119,8 +163,8 @@ def generate_launch_description() -> LaunchDescription:
             name="rviz", default_value="true", description="Whether to launch RViz."
         )
     )
-    ld.add_action(LBRHardwareInterfaceMixin.arg_ctrl_cfg_pkg())
-    ld.add_action(LBRHardwareInterfaceMixin.arg_ctrl_cfg())
-    ld.add_action(LBRHardwareInterfaceMixin.arg_ctrl())
+    ld.add_action(LBRSystemInterfaceMixin.arg_ctrl_cfg_pkg())
+    ld.add_action(LBRSystemInterfaceMixin.arg_ctrl_cfg())
+    ld.add_action(LBRSystemInterfaceMixin.arg_ctrl())
     ld.add_action(OpaqueFunction(function=launch_setup))
     return ld