lbr-stack · mhubii · Nov 21, 2023 · Oct 20, 2023 · Oct 20, 2023 · Nov 1, 2023
diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py
@@ -13,32 +13,47 @@
 
 from lbr_bringup import LBRMoveGroupMixin
 from lbr_description import LBRDescriptionMixin, RVizMixin
-from lbr_ros2_control import LBRSystemInterfaceMixin
+from 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 = LBRSystemInterfaceMixin.node_ros2_control(
+    ros2_control_node = LBRROS2ControlMixin.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 = LBRSystemInterfaceMixin.node_joint_state_broadcaster()
-    controller = LBRSystemInterfaceMixin.node_controller()
+    joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+        controller="joint_state_broadcaster"
+    )
+    lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+        controller="lbr_state_broadcaster"
+    )
+    lbr_estimated_ft_broadcast = LBRROS2ControlMixin.node_controller_spawner(
+        controller="lbr_estimated_ft_broadcaster"
+    )
+    controller = LBRROS2ControlMixin.node_controller_spawner(
+        controller=LaunchConfiguration("ctrl")
+    )
 
     controller_event_handler = RegisterEventHandler(
         OnProcessStart(
             target_action=ros2_control_node,
-            on_start=[joint_state_broadcaster, controller],
+            on_start=[
+                joint_state_broadcaster,
+                lbr_state_broadcaster,
+                lbr_estimated_ft_broadcast,
+                controller,
+            ],
         )
     )
     ld.add_action(controller_event_handler)
 
     # robot state publisher on joint state broadcaster spawn exit
-    robot_state_publisher = LBRSystemInterfaceMixin.node_robot_state_publisher(
+    robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
         robot_description=robot_description, use_sim_time=False
     )
     robot_state_publisher_event_handler = RegisterEventHandler(
@@ -176,8 +191,8 @@ def generate_launch_description() -> LaunchDescription:
             name="rviz", default_value="true", description="Whether to launch RViz."
         )
     )
-    ld.add_action(LBRSystemInterfaceMixin.arg_ctrl_cfg_pkg())
-    ld.add_action(LBRSystemInterfaceMixin.arg_ctrl_cfg())
-    ld.add_action(LBRSystemInterfaceMixin.arg_ctrl())
+    ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
+    ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
+    ld.add_action(LBRROS2ControlMixin.arg_ctrl())
     ld.add_action(OpaqueFunction(function=launch_setup))
     return ld
diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py
@@ -13,7 +13,7 @@
 
 from lbr_bringup import LBRMoveGroupMixin
 from lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin
-from lbr_ros2_control import LBRSystemInterfaceMixin
+from lbr_ros2_control import LBRROS2ControlMixin
 
 
 def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
@@ -23,15 +23,21 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     ld.add_action(GazeboMixin.include_gazebo())  # Gazebo has its own controller manager
     spawn_entity = GazeboMixin.node_spawn_entity()
     ld.add_action(spawn_entity)
-    joint_state_broadcaster = LBRSystemInterfaceMixin.node_joint_state_broadcaster()
+    joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+        controller="joint_state_broadcaster"
+    )
     ld.add_action(joint_state_broadcaster)
-    robot_state_publisher = LBRSystemInterfaceMixin.node_robot_state_publisher(
+    robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
         robot_description=robot_description, use_sim_time=True
     )
     ld.add_action(
         robot_state_publisher
     )  # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description
-    ld.add_action(LBRSystemInterfaceMixin.node_controller())
+    ld.add_action(
+        LBRROS2ControlMixin.node_controller_spawner(
+            controller=LaunchConfiguration("ctrl")
+        )
+    )
 
     # MoveIt 2
     ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
@@ -159,7 +165,7 @@ def generate_launch_description() -> LaunchDescription:
         )
     )
     ld.add_action(
-        LBRSystemInterfaceMixin.arg_ctrl()
+        LBRROS2ControlMixin.arg_ctrl()
     )  # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
     ld.add_action(OpaqueFunction(function=launch_setup))
     return ld
diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_bringup</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>LBR launch files.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt
@@ -18,7 +18,7 @@ find_package(FRIClient REQUIRED)
 find_package(kdl_parser REQUIRED)
 find_package(lbr_fri_ros2 REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
-find_package(orocos_kdl REQUIRED)
+find_package(orocos_kdl_vendor REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 
@@ -38,7 +38,7 @@ ament_target_dependencies(
   kdl_parser
   lbr_fri_ros2
   lbr_fri_msgs
-  orocos_kdl
+  orocos_kdl_vendor
   rclcpp
   rclcpp_components
 )

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_fri_ros2_advanced_cpp</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>Advanced C++ demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>
@@ -17,7 +17,7 @@
   <depend>kdl_parser</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>lbr_fri_msgs</depend>
-  <depend>orocos_kdl</depend>
+  <depend>orocos_kdl_vendor</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_advanced_python/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_fri_ros2_advanced_python</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>Advanced Python demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_advanced_python/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version="1.3.0",
+    version="1.3.1",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),

diff --git a/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml b/lbr_demos/lbr_demos_fri_ros2_cpp/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_fri_ros2_cpp</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>C++ demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_python/package.xml b/lbr_demos/lbr_demos_fri_ros2_python/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_fri_ros2_python</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>Python demos for the lbr_fri_ros2.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_fri_ros2_python/setup.py b/lbr_demos/lbr_demos_fri_ros2_python/setup.py
@@ -6,7 +6,7 @@
 
 setup(
     name=package_name,
-    version="1.3.0",
+    version="1.3.1",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),

diff --git a/lbr_demos/lbr_demos_ros2_control_cpp/package.xml b/lbr_demos/lbr_demos_ros2_control_cpp/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_ros2_control_cpp</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>C++ demos for the LBR ros2_control integration.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_ros2_control_python/package.xml b/lbr_demos/lbr_demos_ros2_control_python/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_demos_ros2_control_python</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>Python demos for the LBR ros2_control integration.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_demos/lbr_demos_ros2_control_python/setup.py b/lbr_demos/lbr_demos_ros2_control_python/setup.py
@@ -4,7 +4,7 @@
 
 setup(
     name=package_name,
-    version="1.3.0",
+    version="1.3.1",
     packages=[package_name],
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),

diff --git a/lbr_description/package.xml b/lbr_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_description</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>KUKA LBR description files</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_fri_msgs/package.xml b/lbr_fri_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_fri_msgs</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>ROS 2 message for the Fast Robot Interface (FRI) specific states.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/utils.hpp
@@ -46,7 +46,9 @@ class ExponentialFilter {
    * @param[in] previous The previous smoothed value.
    * @return double The returned smoothed value.
    */
-  inline double compute(const double &current, const double &previous);
+  inline double compute(const double &current, const double &previous) {
+    return filters::exponentialSmoothing(current, previous, alpha_);
+  };
 
   /**
    * @brief Set the cutoff frequency object. Internally computes the new #alpha_.
@@ -61,14 +63,14 @@ class ExponentialFilter {
    *
    * @return const double&
    */
-  inline const double &get_sample_time() const;
+  inline const double &get_sample_time() const { return sample_time_; };
 
   /**
    * @brief Get #alpha_.
    *
    * @return const double&
    */
-  inline const double &get_alpha() const;
+  inline const double &get_alpha() const { return alpha_; };
 
 protected:
   /**

diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_fri_ros2</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS
     2. Robot states can be extracted and commanded.</description>
   <maintainer email="[email protected]">mhubii</maintainer>

diff --git a/lbr_fri_ros2/src/utils.cpp b/lbr_fri_ros2/src/utils.cpp
@@ -7,10 +7,6 @@ ExponentialFilter::ExponentialFilter(const double &cutoff_frequency, const doubl
   set_cutoff_frequency(cutoff_frequency, sample_time);
 }
 
-inline double ExponentialFilter::compute(const double &current, const double &previous) {
-  return filters::exponentialSmoothing(current, previous, alpha_);
-}
-
 void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency,
                                              const double &sample_time) {
   cutoff_frequency_ = cutoff_frequency;
@@ -24,10 +20,6 @@ void ExponentialFilter::set_cutoff_frequency(const double &cutoff_frequency,
   }
 }
 
-inline const double &ExponentialFilter::get_sample_time() const { return sample_time_; }
-
-inline const double &ExponentialFilter::get_alpha() const { return alpha_; }
-
 double ExponentialFilter::compute_alpha_(const double &cutoff_frequency,
                                          const double &sample_time) {
   double omega_3db = 2.0 * M_PI * sample_time * cutoff_frequency;

diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>lbr_fri_ros2_stack</name>
-  <version>1.3.0</version>
+  <version>1.3.1</version>
   <description>ROS 2 stack for KUKA LBRs.</description>
   <maintainer email="[email protected]">mhubii</maintainer>
   <license>MIT</license>