Merge pull request #91 from lbr-stack/dev-humble-fri-vendor

Humble v1.2.3

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/CMakeLists.txt

@@ -9,6 +9,8 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(fri_vendor REQUIRED)
+find_package(FRIClient REQUIRED)
 find_package(kdl_parser REQUIRED)
 find_package(lbr_fri_ros2 REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
@@ -26,6 +28,10 @@ ament_target_dependencies(
   rclcpp
 )
 
+target_link_libraries(admittance_control_node
+  FRIClient::FRIClient
+)
+
 target_include_directories(admittance_control_node
   PRIVATE src
 )

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/package.xml

@@ -13,6 +13,7 @@
   <exec_depend>lbr_description</exec_depend>
 
   <depend>eigen</depend>
+  <depend>fri_vendor</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>lbr_fri_msgs</depend>

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/src/admittance_controller.hpp

@@ -9,7 +9,7 @@
 #include "kdl/tree.hpp"
 #include "kdl_parser/kdl_parser.hpp"
 
-#include "fri/friLBRState.h"
+#include "friLBRState.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
@@ -25,7 +25,7 @@ class AdmittanceController {
                        const std::string &base_link = "lbr_link_0",
                        const std::string &end_effector_link = "lbr_link_ee",
                        const CartesianVector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5},
-                       const JointVector &dq_gains = {1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5},
+                       const JointVector &dq_gains = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0},
                        const CartesianVector &dx_gains = {1.5, 1.5, 1.5, 20., 40., 60.})
       : dq_gains_(dq_gains), dx_gains_(dx_gains), f_ext_th_(f_ext_th) {
     if (!kdl_parser::treeFromString(robot_description, tree_)) {

lbr_demos/lbr_demos_fri_ros2_cpp/CMakeLists.txt

@@ -7,7 +7,8 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(fri REQUIRED)
+find_package(fri_vendor REQUIRED)
+find_package(FRIClient REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 
@@ -17,33 +18,45 @@ add_executable(joint_sine_overlay_node
 )
 
 ament_target_dependencies(joint_sine_overlay_node
-  fri  
+  fri_vendor
   lbr_fri_msgs
   rclcpp
 )
 
+target_link_libraries(joint_sine_overlay_node
+  FRIClient::FRIClient
+)
+
 # torque sine overlay
 add_executable(torque_sine_overlay_node
   src/torque_sine_overlay_node.cpp
 )
 
 ament_target_dependencies(torque_sine_overlay_node
-  fri  
+  fri_vendor
   lbr_fri_msgs
   rclcpp
 )
 
+target_link_libraries(torque_sine_overlay_node
+  FRIClient::FRIClient
+)
+
 # wrench sine overlay
 add_executable(wrench_sine_overlay_node
   src/wrench_sine_overlay_node.cpp
 )
 
 ament_target_dependencies(wrench_sine_overlay_node
-  fri  
+  fri_vendor
   lbr_fri_msgs
   rclcpp
 )
 
+target_link_libraries(wrench_sine_overlay_node
+  FRIClient::FRIClient
+)
+
 install(TARGETS
   joint_sine_overlay_node
   torque_sine_overlay_node

lbr_demos/lbr_demos_fri_ros2_cpp/package.xml

@@ -9,7 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>fri</depend>
+  <depend>fri_vendor</depend>
   <depend>lbr_fri_msgs</depend>
   <depend>rclcpp</depend>
 

lbr_demos/lbr_demos_fri_ros2_cpp/src/joint_sine_overlay_node.cpp

@@ -4,7 +4,7 @@
 #include <string>
 
 // include fri for session state
-#include "fri/friClientIf.h"
+#include "friClientIf.h"
 
 #include "rclcpp/rclcpp.hpp"
 

lbr_demos/lbr_demos_fri_ros2_cpp/src/torque_sine_overlay_node.cpp

@@ -4,7 +4,7 @@
 #include <string>
 
 // include fri for session state
-#include "fri/friClientIf.h"
+#include "friClientIf.h"
 
 #include "rclcpp/rclcpp.hpp"
 

lbr_demos/lbr_demos_fri_ros2_cpp/src/wrench_sine_overlay_node.cpp

@@ -4,7 +4,7 @@
 #include <string>
 
 // include fri for session state
-#include "fri/friClientIf.h"
+#include "friClientIf.h"
 
 #include "rclcpp/rclcpp.hpp"
 

lbr_demos/lbr_demos_ros2_control_cpp/CMakeLists.txt

@@ -8,7 +8,8 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(control_msgs REQUIRED)
-find_package(fri REQUIRED)
+find_package(fri_vendor REQUIRED)
+find_package(FRIClient REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(trajectory_msgs REQUIRED)
@@ -19,12 +20,16 @@ add_executable(lbr_joint_trajectory_executioner_node
 
 ament_target_dependencies(lbr_joint_trajectory_executioner_node
   control_msgs
-  fri
+  fri_vendor
   rclcpp
   rclcpp_action
   trajectory_msgs
 )
 
+target_link_libraries(lbr_joint_trajectory_executioner_node
+  FRIClient::FRIClient
+)
+
 install(TARGETS
   lbr_joint_trajectory_executioner_node
   DESTINATION lib/${PROJECT_NAME}

lbr_demos/lbr_demos_ros2_control_cpp/package.xml

@@ -10,7 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>control_msgs</depend>
-  <depend>fri</depend>
+  <depend>fri_vendor</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>trajectory_msgs</depend>

lbr_demos/lbr_demos_ros2_control_cpp/src/lbr_joint_trajectory_executioner_node.cpp

@@ -3,7 +3,7 @@
 #include <string>
 
 // include fri for number of joints
-#include "fri/friLBRState.h"
+#include "friLBRState.h"
 
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "rclcpp/rclcpp.hpp"

lbr_fri_ros2/CMakeLists.txt

@@ -9,10 +9,12 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(control_toolbox REQUIRED)
-find_package(fri REQUIRED)
+find_package(fri_vendor REQUIRED)
+find_package(FRIClient REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(realtime_tools REQUIRED)
 find_package(urdf REQUIRED)
 
 # lbr_fri_ros2
@@ -31,18 +33,24 @@ target_include_directories(lbr_fri_ros2
 
 ament_target_dependencies(lbr_fri_ros2
   control_toolbox
-  fri
   lbr_fri_msgs
   rclcpp
+  realtime_tools
   urdf
 )
 
+target_link_libraries(lbr_fri_ros2
+  FRIClient::FRIClient
+)
+
 ament_export_targets(lbr_fri_ros2_export HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   control_toolbox
-  fri
+  fri_vendor
+  FRIClient
   lbr_fri_msgs
   rclcpp
+  realtime_tools
   urdf
 )
 
@@ -72,15 +80,16 @@ target_include_directories(lbr_app_component
   PRIVATE src
 )
 
-target_link_libraries(lbr_app_component
-  lbr_fri_ros2
-)
-
 ament_target_dependencies(lbr_app_component
   rclcpp
+  urdf
   rclcpp_components
 )
 
+target_link_libraries(lbr_app_component
+  lbr_fri_ros2
+)
+
 rclcpp_components_register_node(lbr_app_component
   PLUGIN lbr_fri_ros2::LBRAppComponent
   EXECUTABLE lbr_app

lbr_fri_ros2/include/lbr_fri_ros2/lbr_app.hpp

@@ -9,10 +9,11 @@
 #include <thread>
 
 #include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/thread_priority.hpp"
 
-#include "fri/friClientApplication.h"
-#include "fri/friClientIf.h"
-#include "fri/friUdpConnection.h"
+#include "friClientApplication.h"
+#include "friLBRClient.h"
+#include "friUdpConnection.h"
 
 #include "lbr_fri_msgs/srv/app_connect.hpp"
 #include "lbr_fri_msgs/srv/app_disconnect.hpp"
@@ -22,7 +23,7 @@
 namespace lbr_fri_ros2 {
 /**
  * @brief The LBRApp has a node for exposing FRI methods to services. It shares this node with the
- * #lbr_client_, which reads commands / write states via real-time safe topics.
+ * #lbr_client_, which reads commands / write states via realtime safe topics.
  *
  * Services:
  * - <b>~/connect</b> (lbr_fri_msgs::srv::AppConnect)
@@ -114,7 +115,7 @@ class LBRApp {
    * @brief Exchanges commands / states between ROS and the FRI.
    *
    * Calls step() on #app_, which callbacks #lbr_client_. #lbr_client_ reads commands / write states
-   * through real-time safe topics.
+   * through realtime safe topics.
    *
    */
   void run_();
@@ -128,6 +129,7 @@ class LBRApp {
   std::string robot_description_;     /**< The robot description, read from node parameters.*/
   std::string robot_name_;            /**< The robot name, read from node parameters.*/
   std::string command_guard_variant_; /**< The command guard, read from node parameters.*/
+  int rt_prio_;                       /**< The realtime priority, read from node parameters.*/
 
   std::atomic<bool> connected_; /**< True if UDP port open and communication running.*/
 

lbr_fri_ros2/include/lbr_fri_ros2/lbr_client.hpp

@@ -11,8 +11,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/strategies/message_pool_memory_strategy.hpp"
 
-#include "fri/friClientIf.h"
-#include "fri/friLBRClient.h"
+#include "friLBRClient.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"

lbr_fri_ros2/include/lbr_fri_ros2/lbr_command_guard.hpp

@@ -10,8 +10,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "urdf/model.h"
 
-#include "fri/friClientIf.h"
-#include "fri/friLBRState.h"
+#include "friLBRClient.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 
@@ -30,23 +29,24 @@ class LBRCommandGuard {
   /**
    * @brief Construct a new LBRCommandGuard object.
    *
-   * @param[in] node Shared node for logging
+   * @param[in] logger_interface Shared node logger interface
    * @param[in] min_position Minimum joint position [rad]
    * @param[in] max_position Maximum joint position [rad]
    * @param[in] max_velocity Maximum joint velocity [rad/s]
    * @param[in] max_torque Maximal torque [Nm]
    */
-  LBRCommandGuard(const rclcpp::Node::SharedPtr node, const JointArray &min_position,
-                  const JointArray &max_position, const JointArray &max_velocity,
-                  const JointArray &max_torque);
+  LBRCommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
+                  const JointArray &min_position, const JointArray &max_position,
+                  const JointArray &max_velocity, const JointArray &max_torque);
 
   /**
    * @brief Construct a new LBRCommandGuard object.
    *
-   * @param[in] node Shared node for logging
+   * @param[in] logger_interface Shared node logger interface
    * @param robot_description String containing URDF robot rescription
    */
-  LBRCommandGuard(const rclcpp::Node::SharedPtr node, const std::string &robot_description);
+  LBRCommandGuard(const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
+                  const std::string &robot_description);
 
   /**
    * @brief Checks for command validity given the current state.
@@ -103,7 +103,8 @@ class LBRCommandGuard {
   virtual bool command_in_torque_limits_(const lbr_fri_msgs::msg::LBRCommand &lbr_command,
                                          const KUKA::FRI::LBRState &lbr_state) const;
 
-  rclcpp::Node::SharedPtr node_; /**< Shared node for logging.*/
+  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+      logger_interface_; /**< Shared node logger interface.*/
 
   JointArray min_position_; /**< Minimum joint position [rad].*/
   JointArray max_position_; /**< Maximum joint position [rad].*/
@@ -122,11 +123,13 @@ class LBRSafeStopCommandGuard : public LBRCommandGuard {
   /**
    * @brief Construct a new LBRSafeStopCommandGuard object.
    *
-   * @param[in] node Shared node for logging
+   * @param[in] logger_interface Shared node logger interface
    * @param robot_description String containing URDF robot rescription
    */
-  LBRSafeStopCommandGuard(const rclcpp::Node::SharedPtr node, const std::string &robot_description)
-      : LBRCommandGuard(node, robot_description){};
+  LBRSafeStopCommandGuard(
+      const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
+      const std::string &robot_description)
+      : LBRCommandGuard(logger_interface, robot_description){};
 
 protected:
   /**
@@ -145,13 +148,13 @@ class LBRSafeStopCommandGuard : public LBRCommandGuard {
 /**
  * @brief Creates an LBRCommandGuard object.
  *
- * @param[in] node Shared node for logging
+ * @param[in] logger_interface Shared node logger interface
  * @param[in] robot_description String containing URDF robot rescription
  * @param[in] variant Which variant of LBRCommandGuard to create
  * @return std::unique_ptr<LBRCommandGuard> Pointer to LBRCommandGuard object
  */
-std::unique_ptr<LBRCommandGuard> lbr_command_guard_factory(const rclcpp::Node::SharedPtr node,
-                                                           const std::string &robot_description,
-                                                           const std::string &variant);
+std::unique_ptr<LBRCommandGuard> lbr_command_guard_factory(
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
+    const std::string &robot_description, const std::string &variant);
 } // end of namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__LBR_COMMAND_GUARD_HPP_