Humble v1.5.0 Release

README.md

@@ -14,10 +14,10 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t
             <th align="left" width="25%">LBR Med14 R820</th>
         </tr>
         <tr>
-            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/foxglove/iiwa7_r800.png" alt="LBR IIWA7 R800"></td>
-            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/foxglove/iiwa14_r820.png" alt="LBR IIWA14 R820"></td>
-            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/foxglove/med7_r800.png" alt="LBR Med7 R800"></td>
-            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/foxglove/med14_r820.png" alt="LBR Med14 R820"></td>
+            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/rviz/iiwa7_r800.png" alt="LBR IIWA7 R800"></td>
+            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/rviz/iiwa14_r820.png" alt="LBR IIWA14 R820"></td>
+            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/rviz/med7_r800.png" alt="LBR Med7 R800"></td>
+            <td align="center"><img src="lbr_fri_ros2_stack/doc/img/rviz/med14_r820.png" alt="LBR Med14 R820"></td>
         </tr>
     </table>
 </body>

lbr_bringup/doc/lbr_bringup.rst

@@ -1,34 +1,7 @@
 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
+TODO: Mention different controllers here already
 
-- :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 +15,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 :ref:`Robot 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.

lbr_bringup/launch/bringup.launch.py

@@ -1,12 +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_ros.substitutions import FindPackageShare
 
-from lbr_description import LBRDescriptionMixin, RVizMixin
+from lbr_description import LBRDescriptionMixin
 
 
 def generate_launch_description() -> LaunchDescription:

lbr_bringup/launch/real.launch.py

@@ -3,7 +3,7 @@
 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,
@@ -20,9 +20,15 @@ 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
+
+    # 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())
@@ -142,7 +137,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)

lbr_demos/doc/lbr_demos.rst

@@ -5,6 +5,9 @@ Demos for controlling the LBR through the Fast Robot Interface (FRI) from ROS 2.
 .. warning::
     On the real robot, do always execute in ``T1`` mode first.
 
+TODO: Differentiate demos through controllers
+TODO: indicate which demos run in simulation and which on the real robot
+
 LBR Demos FRI ROS 2
 -------------------
 .. note::

lbr_description/gazebo/lbr.gazebo.xacro

@@ -6,7 +6,11 @@
         <gazebo>
             <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
                 <parameters>$(find lbr_ros2_control)/config/lbr_controllers.yaml</parameters>
-                <robotNamespace>/${robot_name}</robotNamespace>
+                <ros>
+                    <namespace>/${robot_name}</namespace>
+                    <!-- remapping for controller manager inside Gazebo plugin -->
+                    <remapping>~/robot_description:=robot_description</remapping>
+                </ros>
             </plugin>
         </gazebo>
 

lbr_description/lbr_description/launch_mixin.py

@@ -30,43 +30,42 @@ def include_gazebo(**kwargs) -> IncludeLaunchDescription:
 
     @staticmethod
     def node_spawn_entity(
-        robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs
+        robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "robot_name", default="lbr"
+        ),
+        **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"),
-                "-robot_namespace",
-                LaunchConfiguration("robot_name"),
+                robot_name,
             ],
             output="screen",
-            namespace=LaunchConfiguration("robot_name"),
+            namespace=robot_name,
             **kwargs,
         )
 
 
 class LBRDescriptionMixin:
     @staticmethod
     def param_robot_description(
-        model: Optional[Union[LaunchConfiguration, str]] = None,
-        robot_name: Optional[Union[LaunchConfiguration, str]] = None,
-        port_id: Optional[Union[LaunchConfiguration, str]] = None,
-        sim: Optional[Union[LaunchConfiguration, bool]] = None,
+        model: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "model", default="iiwa7"
+        ),
+        robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "robot_name", default="lbr"
+        ),
+        port_id: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "port_id", default="30200"
+        ),
+        sim: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration(
+            "sim", default="true"
+        ),
     ) -> Dict[str, str]:
-        if model is None:
-            model = LaunchConfiguration("model", default="iiwa7")
-        if robot_name is None:
-            robot_name = LaunchConfiguration("robot_name", default="lbr")
-        if port_id is None:
-            port_id = LaunchConfiguration("port_id", default="30200")
-        if sim is None:
-            sim = LaunchConfiguration("sim", default="true")
         if type(sim) is bool:
             sim = "true" if sim else "false"
         robot_description = {
@@ -187,18 +186,14 @@ def arg_rviz_config(
 
     @staticmethod
     def node_rviz(
-        rviz_config_pkg: Optional[Union[LaunchConfiguration, str]] = None,
-        rviz_config: Optional[Union[LaunchConfiguration, str]] = None,
+        rviz_config_pkg: Optional[
+            Union[LaunchConfiguration, str]
+        ] = LaunchConfiguration("rviz_config_pkg", default="lbr_description"),
+        rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "rviz_config", default="config/config.rviz"
+        ),
         **kwargs,
     ) -> Node:
-        if rviz_config_pkg is None:
-            rviz_config_pkg = LaunchConfiguration(
-                "rviz_config_pkg", default="lbr_description"
-            )
-        if rviz_config is None:
-            rviz_config = LaunchConfiguration(
-                "rviz_config", default="config/config.rviz"
-            )
         return Node(
             package="rviz2",
             executable="rviz2",

lbr_fri_ros2/doc/lbr_fri_ros2.rst

@@ -1,5 +1,7 @@
 LBR FRI ROS 2
 =============
+TODO: massive re-write of this section, since architecture has changed.
+
 The ``lbr_fri_ros2`` package provides a ROS 2 interface for the KUKA LBRs. It is designed to run stand-alone **and** within ``ros2_control``.
 
 Quick Start

lbr_fri_ros2/include/lbr_fri_ros2/app.hpp

@@ -13,6 +13,7 @@
 #include "friUdpConnection.h"
 
 #include "lbr_fri_ros2/async_client.hpp"
+#include "lbr_fri_ros2/formatting.hpp"
 
 namespace lbr_fri_ros2 {
 class App {

lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp

@@ -11,8 +11,8 @@
 #include "friLBRClient.h"
 
 #include "lbr_fri_ros2/command_interface.hpp"
-#include "lbr_fri_ros2/enum_maps.hpp"
 #include "lbr_fri_ros2/filters.hpp"
+#include "lbr_fri_ros2/formatting.hpp"
 #include "lbr_fri_ros2/state_interface.hpp"
 
 namespace lbr_fri_ros2 {

lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp

@@ -13,6 +13,7 @@
 #include "friLBRState.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
+#include "lbr_fri_ros2/formatting.hpp"
 
 namespace lbr_fri_ros2 {
 struct CommandGuardParameters {

lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp → lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp

@@ -1,11 +1,24 @@
-#ifndef LBR_FRI_ROS2__ENUM_MAPS_HPP_
-#define LBR_FRI_ROS2__ENUM_MAPS_HPP_
+#ifndef LBR_FRI_ROS2__FORMATTING_HPP_
+#define LBR_FRI_ROS2__FORMATTING_HPP_
 
 #include <string>
 
 #include "friLBRClient.h"
 
 namespace lbr_fri_ros2 {
+struct ColorScheme {
+  // refer https://stackoverflow.com/a/287944
+  static constexpr char HEADER[] = "\033[95m";
+  static constexpr char OKBLUE[] = "\033[94m";
+  static constexpr char OKCYAN[] = "\033[96m";
+  static constexpr char OKGREEN[] = "\033[92m";
+  static constexpr char WARNING[] = "\033[93m";
+  static constexpr char FAIL[] = "\033[91m";
+  static constexpr char ENDC[] = "\033[0m";
+  static constexpr char BOLD[] = "\033[1m";
+  static constexpr char UNDERLINE[] = "\033[4m";
+};
+
 struct EnumMaps {
   static std::string session_state_map(const int &session_state) {
     switch (session_state) {

lbr_fri_ros2/lbr_fri_ros2/launch_mixin.py

@@ -136,10 +136,11 @@ def param_port_id() -> Dict[str, LaunchConfiguration]:
 
     @staticmethod
     def node_app(
-        robot_name: Optional[Union[LaunchConfiguration, str]] = None, **kwargs
+        robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration(
+            "robot_name", default="lbr"
+        ),
+        **kwargs
     ) -> DeclareLaunchArgument:
-        if robot_name is None:
-            robot_name = LaunchConfiguration("robot_name", default="lbr")
         return Node(
             package="lbr_fri_ros2",
             executable="app",