lbr-stack · mhubii · May 13, 2024 · Dec 29, 2023 · Dec 29, 2023 · Dec 29, 2023
diff --git a/.github/workflows/black.yml b/.github/workflows/black.yml
@@ -0,0 +1,10 @@
+name: Lint
+
+on: [push, pull_request]
+
+jobs:
+  lint:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - uses: psf/black@stable
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -5,7 +5,7 @@ on:
     branches:
       - humble
   schedule:
-    - cron: '0 0 * * 0'
+    - cron: "0 0 * * 0"
 
 # ros 2 ci: https://github.com/marketplace/actions/ros-2-ci-action
 # doc: https://ubuntu.com/blog/ros-2-ci-with-github-actions
@@ -15,10 +15,11 @@ jobs:
     strategy:
       matrix:
         os: [ubuntu-latest]
+        fri_version: [1.11, 1.14, 1.15, 1.16, 2.5]
     steps:
       - uses: ros-tooling/[email protected]
       - uses: ros-tooling/[email protected]
         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.yaml
+          vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/dev-humble-launch/lbr_fri_ros2_stack/repos-fri-${{ matrix.fri_version }}.yaml
diff --git a/README.md b/README.md
@@ -1,29 +1,31 @@
 # lbr_fri_ros2_stack
-![Build status](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build.yml/badge.svg?branch=humble) ![GitHub](https://img.shields.io/github/license/lbr-stack/lbr_fri_ros2_stack) 
+[![Build status](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions/workflows/build.yml/badge.svg?branch=humble)](https://github.com/lbr-stack/lbr_fri_ros2_stack/actions)
+[![License](https://img.shields.io/github/license/lbr-stack/lbr_fri_ros2_stack)](https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble?tab=Apache-2.0-1-ov-file#readme) 
 [![Documentation Status](https://readthedocs.org/projects/lbr-stack/badge/?version=latest)](https://lbr-stack.readthedocs.io/en/latest/?badge=latest)
-[![status](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef/status.svg)](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef)
+[![JOSS](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef/status.svg)](https://joss.theoj.org/papers/c43c82bed833c02503dd47f2637192ef) 
+[![Code Style: Black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
 
 ROS 2 packages for the KUKA LBR, including communication to the real robot via the Fast Robot Interface ([FRI](https://github.com/lbr-stack/fri)) and [Gazebo](http://gazebosim.org/) simulation support. Included are the `iiwa7`, `iiwa14`, `med7`, and `med14`.
 
 <body>
     <table>
         <tr>
-            <th align="left" width="25%">LBR IIWA7 R800</th>
-            <th align="left" width="25%">LBR IIWA14 R820</th>
-            <th align="left" width="25%">LBR Med7 R800</th>
-            <th align="left" width="25%">LBR Med14 R820</th>
+            <th align="left" width="25%">LBR IIWA 7 R800</th>
+            <th align="left" width="25%">LBR IIWA 14 R820</th>
+            <th align="left" width="25%">LBR Med 7 R800</th>
+            <th align="left" width="25%">LBR Med 14 R820</th>
         </tr>
         <tr>
-            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/iiwa7_r800.png" alt="LBR IIWA7 R800"></td>
-            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/iiwa14_r820.png" alt="LBR IIWA14 R820"></td>
-            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/med7_r800.png" alt="LBR Med7 R800"></td>
-            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/med14_r820.png" alt="LBR Med14 R820"></td>
+            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/iiwa7_r800.png" alt="LBR IIWA 7 R800"></td>
+            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/iiwa14_r820.png" alt="LBR IIWA 14 R820"></td>
+            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/med7_r800.png" alt="LBR Med 7 R800"></td>
+            <td align="center"><img src="https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/doc/img/foxglove/med14_r820.png" alt="LBR Med 14 R820"></td>
         </tr>
     </table>
 </body>
 
 ## Documentation
-Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest).
+Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io/en/latest).
 
 ## Quick Start
 1. Install ROS 2 development tools
@@ -35,20 +37,22 @@ Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest).
 2. Create a workspace, clone, and install dependencies
 
     ```shell
+    source /opt/ros/humble/setup.bash
+    export FRI_CLIENT_VERSION=1.15
     mkdir -p lbr-stack/src && cd lbr-stack
-    vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml
+    vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml
     rosdep install --from-paths src -i -r -y
     ```
 
+> [!NOTE]
+> FRI client is cloned from [fri](https://github.com/lbr-stack/fri) and must be available as branch, refer [README](https://github.com/lbr-stack/fri?tab=readme-ov-file#contributing).
+
 3. Build
 
     ```shell
-    colcon build --symlink-install --cmake-args -DFRI_CLIENT_VERSION=1.15 --no-warn-unused-cli # replace by your FRI client version
+    colcon build --symlink-install
     ```
 
-> [!NOTE]
-> FRI client is added as external CMake project via [fri_vendor](https://github.com/lbr-stack/fri_vendor) and must be available as branch, refer [README](https://github.com/lbr-stack/fri?tab=readme-ov-file#contributing).
-
 4. Launch the simulation via
 
     ```shell
@@ -63,7 +67,7 @@ Full documentation available [here](https://lbr-stack.readthedocs.io/en/latest).
 > [!TIP]
 > List all arguments for the launch file via `ros2 launch lbr_bringup bringup.launch.py -s`
 
-Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-stack.readthedocs.io/en/latest) above.
+Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html).
 
 ## Citation
 If you enjoyed using this repository for your work, we would really appreciate ❤️ if you could cite it, as it helps us to continue offering support.

diff --git a/docker/Dockerfile b/docker/Dockerfile
@@ -3,9 +3,6 @@ FROM ros:humble-ros-base-jammy
 # change default shell to bash
 SHELL ["/bin/bash", "-c"]
 
-# set environment variables
-ARG FRI_CLIENT_VERSION=1.15
-
 # upgrade packages
 RUN apt-get update
 RUN apt-get upgrade -y
@@ -19,7 +16,7 @@ RUN rosdep install -i --from-paths src --rosdistro ${ROS_DISTRO} -y
 
 # "--symlink-install" allows the code in the locally mounted volume ./src to be adjusted without rebuilding
 RUN source /opt/ros/${ROS_DISTRO}/setup.bash && \
-    colcon build --symlink-install --cmake-args -DFRI_CLIENT_VERSION=${FRI_CLIENT_VERSION} --no-warn-unused-cli
+    colcon build --symlink-install
 
 # source the workspace
 RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc

diff --git a/docker/container_build.sh b/docker/container_build.sh
@@ -3,7 +3,7 @@ xhost +
 
 docker rm lbr_stack_container
 
-docker build --build-arg FRI_CLIENT_VERSION=$1 -t lbr_stack_container .
+docker build -t lbr_stack_container .
 
 docker run -it \
 	--network host \

diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst
@@ -1,4 +1,4 @@
-Docker
+docker
 ======
 To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions below.
 
@@ -12,16 +12,17 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions
 
    .. code-block:: bash
 
+    export FRI_CLIENT_VERSION=1.15 # replace by your FRI client version
     mkdir -p lbr-stack/src && cd lbr-stack
-    vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml
+    vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml
 
 #. Copy the Dockerfile and the container scripts to the ``lbr-stack`` directory. Build and start the container
 
    .. code-block:: bash
 
     cp -r src/lbr_fri_ros2_stack/docker/* .
     chmod +x container_build.sh
-    sudo ./container_build.sh 1.15 # replace by your FRI client version
+    sudo ./container_build.sh
 
 #. Inside the container, launch e.g. simulation via (might require re-launch after Gazebo launched first time)
 

diff --git a/lbr_bringup/CMakeLists.txt b/lbr_bringup/CMakeLists.txt
@@ -7,13 +7,14 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
 install(
   DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}
 )
 
 # Launch mixins
-ament_python_install_package(${PROJECT_NAME})
+ament_python_install_package(launch_mixins)
 
 ament_package()
diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst
@@ -1,34 +1,5 @@
-LBR Bringup
+lbr_bringup
 ===========
-The ``lbr_fri_ros2_stack`` is designed for research **and** deployment. It runs standalone, with ``ros2_control``, and thus also with ``MoveIt 2``. Details are described in below sections
-
-- :ref:`ROS 2 Control and MoveIt 2 Launch`
-- :ref:`Standalone Launch`
-
-Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package.
-
-.. note::
-    For the real robot, make sure you have followed :ref:`Robot Setup` first.
-
-.. warning::
-    On the real robot, do always execute in ``T1`` mode first.
-
-General Information on the FRI
-------------------------------
-The ``FRI`` lets the user select a ``FRI control mode`` and a ``FRI client command mode``. When running the ``LBRServer``:
-
-- .. dropdown:: Select ``FRI control mode``
-
-    .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png
-
-- .. dropdown:: Select ``FRI client command mode``
-
-    .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png
-
-The ``FRI control mode`` specifies the mode in which the robot is controlled, and the ``FRI client command mode`` specifies the commands that the user sends.
-
-ROS 2 Control and MoveIt 2 Launch
----------------------------------
 The ``lbr_bringup`` works for the simulation and the real robot. Run:
 
 .. code:: bash
@@ -42,44 +13,44 @@ The ``lbr_bringup`` works for the simulation and the real robot. Run:
 .. note::
     For a list of available parameters, call ``ros2 launch lbr_bringup bringup.launch.py -s``.
 
-When using the real robot
+When using the real robot, select ``sim:=false`` and
 
 .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD``
 
     .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png
 
-and select:
+Select
 
 - ``FRI send period``: ``10 ms``
 - ``IP address``: ``your configuration``
 - ``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.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``).
+Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package.
 
-For using other ``FRI send period``, also change the ``sample_time`` in the `lbr_system_interface.xacro <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_interface.xacro>`_ (automated in the future).
+.. note::
+    For the real robot, make sure you have followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first.
 
-Standalone Launch
------------------
-Standalone launch is great for research. Only the the real robot is supported. It can be launched through:
+.. warning::
+    On the real robot, do always execute in ``T1`` mode first.
 
-.. code:: bash
+General Information on the FRI
+------------------------------
+The ``FRI`` lets the user select a ``FRI control mode`` and a ``FRI client command mode``. When running the ``LBRServer``:
 
-    ros2 launch lbr_fri_ros2 app.launch.py \
-        model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
-        robot_name:=lbr # any robot name
+- .. dropdown:: Select ``FRI control mode``
 
-This runs the :lbr_fri_ros2:`AppComponent <lbr_fri_ros2::AppComponent>`, which creates 2 topics, ``/robot_name/command`` for commands and ``/robot_name/state``. See :ref:`LBR Demos FRI ROS 2` for more examples and :ref:`LBR FRI ROS 2` for more documentation.
+    .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_control_mode.png
 
-.. note::
-    For a list of available parameters, call ``ros2 launch lbr_fri_ros2 app.launch.py -s``.
+- .. dropdown:: Select ``FRI client command mode``
+
+    .. thumbnail:: ../../lbr_fri_ros2_stack/doc/img/controller/raw/lbr_server_client_command_mode.png
+
+The ``FRI control mode`` specifies the mode in which the robot is controlled, and the ``FRI client command mode`` specifies the commands that the user sends.
 
 Troubleshooting
 ---------------
 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.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`.
+- Frequency: Make sure the ``ros2_control_node`` frequency and the ``FRI send period`` are compatible, consider changing ``update_rate`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_ros2_control/config/lbr_controllers.yaml>`_. 
 - Realtime priority: Set real time priority in ``code /etc/security/limits.conf``, add the line: ``user - rtprio 99``, where user is your username.
diff --git a/lbr_bringup/launch/bringup.launch.py b/lbr_bringup/launch/bringup.launch.py
@@ -1,13 +1,11 @@
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.actions import IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition
-from launch.event_handlers import OnIncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_mixins.lbr_description import LBRDescriptionMixin
 from launch_ros.substitutions import FindPackageShare
 
-from lbr_description import LBRDescriptionMixin, RVizMixin
-
 
 def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()

diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py
@@ -3,9 +3,8 @@
 from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
 from launch.actions import OpaqueFunction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-from lbr_bringup import LBRMoveGroupMixin
-from lbr_description import LBRDescriptionMixin, RVizMixin
+from launch_mixins.lbr_bringup import LBRMoveGroupMixin
+from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin
 
 
 def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:

diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py
@@ -3,26 +3,32 @@
 from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
 from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
 from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.event_handlers import OnProcessStart
 from launch.substitutions import (
     AndSubstitution,
     LaunchConfiguration,
     NotSubstitution,
     PathJoinSubstitution,
 )
-
-from lbr_bringup import LBRMoveGroupMixin
-from lbr_description import LBRDescriptionMixin, RVizMixin
-from lbr_ros2_control import LBRROS2ControlMixin
+from launch_mixins.lbr_bringup import LBRMoveGroupMixin
+from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin
+from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin
 
 
 def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     ld = LaunchDescription()
 
     robot_description = LBRDescriptionMixin.param_robot_description(sim=False)
-    ros2_control_node = LBRROS2ControlMixin.node_ros2_control(
-        robot_description=robot_description
+    world_robot_tf = [0, 0, 0, 0, 0, 0]  # keep zero
+
+    # robot state publisher
+    robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
+        robot_description=robot_description, use_sim_time=False
     )
+    ld.add_action(robot_state_publisher)
+
+    # ros2 control node
+    ros2_control_node = LBRROS2ControlMixin.node_ros2_control()
     ld.add_action(ros2_control_node)
 
     # joint state broad caster and controller on ros2 control node start
@@ -52,17 +58,6 @@ 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 = LBRROS2ControlMixin.node_robot_state_publisher(
-        robot_description=robot_description, use_sim_time=False
-    )
-    robot_state_publisher_event_handler = RegisterEventHandler(
-        OnProcessExit(
-            target_action=joint_state_broadcaster, on_exit=[robot_state_publisher]
-        )
-    )
-    ld.add_action(robot_state_publisher_event_handler)
-
     # MoveIt 2
     ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
     ld.add_action(LBRMoveGroupMixin.arg_capabilities())
@@ -77,7 +72,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     robot_name = LaunchConfiguration("robot_name").perform(context)
     ld.add_action(
         LBRDescriptionMixin.node_static_tf(
-            tf=[0, 0, 0, 0, 0, 0],  # keep zero
+            tf=world_robot_tf,
             parent="world",
             child=PathJoinSubstitution(
                 [
@@ -144,7 +139,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     # RViz event handler
     rviz_event_handler = RegisterEventHandler(
         OnProcessStart(
-            target_action=robot_state_publisher, on_start=[rviz_moveit, rviz]
+            target_action=joint_state_broadcaster, on_start=[rviz_moveit, rviz]
         )
     )
     ld.add_action(rviz_event_handler)