added the estimated ft sensor and broadcaster

lbr_bringup/launch/real.launch.py

@@ -29,6 +29,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
     joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
         controller="joint_state_broadcaster"
     )
+    force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
+        controller="force_torque_broadcaster"
+    )
     lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
         controller="lbr_state_broadcaster"
     )
@@ -41,6 +44,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
             target_action=ros2_control_node,
             on_start=[
                 joint_state_broadcaster,
+                force_torque_broadcaster,
                 lbr_state_broadcaster,
                 controller,
             ],

lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp

@@ -17,14 +17,14 @@
 namespace lbr_fri_ros2 {
 struct CommandGuardParameters {
   // ROS IDL types
-  using joint_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
-  using joint_name_array_t = std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
-
-  joint_name_array_t joint_names;                         /**< Joint names.*/
-  joint_array_t min_position{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint position [rad].*/
-  joint_array_t max_position{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint position [rad].*/
-  joint_array_t max_velocity{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocity [rad/s].*/
-  joint_array_t max_torque{0., 0., 0., 0., 0., 0., 0.};   /**< Maximum joint torque [Nm].*/
+  using jnt_array_t = std::array<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+  using jnt_name_array_t = std::array<std::string, KUKA::FRI::LBRState::NUMBER_OF_JOINTS>;
+
+  jnt_name_array_t joint_names;                         /**< Joint names.*/
+  jnt_array_t min_position{0., 0., 0., 0., 0., 0., 0.}; /**< Minimum joint position [rad].*/
+  jnt_array_t max_position{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint position [rad].*/
+  jnt_array_t max_velocity{0., 0., 0., 0., 0., 0., 0.}; /**< Maximum joint velocity [rad/s].*/
+  jnt_array_t max_torque{0., 0., 0., 0., 0., 0., 0.};   /**< Maximum joint torque [Nm].*/
 };
 
 class CommandGuard {
@@ -34,8 +34,8 @@ class CommandGuard {
   // ROS IDL types
   using idl_command_t = lbr_fri_msgs::msg::LBRCommand;
   using const_idl_command_t_ref = const idl_command_t &;
-  using joint_array_t = idl_command_t::_joint_position_type;
-  using const_joint_array_t_ref = const joint_array_t &;
+  using jnt_array_t = idl_command_t::_joint_position_type;
+  using const_jnt_array_t_ref = const jnt_array_t &;
 
   // FRI types
   using fri_state_t = KUKA::FRI::LBRState;

lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp

@@ -12,25 +12,30 @@
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
 
+#include "friLBRState.h"
+
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/pinv.hpp"
 
-#include "friLBRState.h"
-
 namespace lbr_fri_ros2 {
 class FTEstimator {
 protected:
-  static constexpr uint8_t CARTESIAN_DOF = 6;
   static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator";
+  using jnt_pos_array_t = lbr_fri_msgs::msg::LBRState::_measured_joint_position_type;
+  using const_jnt_pos_array_t_ref = const jnt_pos_array_t &;
+  using ext_tau_array_t = lbr_fri_msgs::msg::LBRState::_external_torque_type;
+  using const_ext_tau_array_t_ref = const ext_tau_array_t &;
 
 public:
+  static constexpr uint8_t CARTESIAN_DOF = 6;
+  using cart_array_t = std::array<double, CARTESIAN_DOF>;
+  using cart_array_t_ref = cart_array_t &;
+
   FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0",
               const std::string &chain_tip = "link_ee");
 
-  void
-  compute(const lbr_fri_msgs::msg::LBRState::_measured_joint_position_type &measured_joint_position,
-          const lbr_fri_msgs::msg::LBRState::_external_torque_type &external_torque,
-          std::array<double, CARTESIAN_DOF> &f_ext);
+  void compute(const_jnt_pos_array_t_ref measured_joint_position,
+               const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext);
 
 protected:
   KDL::Tree tree_;

lbr_fri_ros2/src/ft_estimator.cpp

@@ -18,10 +18,8 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string
   q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS);
 }
 
-void FTEstimator::compute(
-    const lbr_fri_msgs::msg::LBRState::_measured_joint_position_type &measured_joint_position,
-    const lbr_fri_msgs::msg::LBRState::_external_torque_type &external_torque,
-    std::array<double, CARTESIAN_DOF> &f_ext) {
+void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position,
+                          const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext) {
   q_.data = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(
       measured_joint_position.data());
   tau_ext_ = Eigen::Map<const Eigen::Matrix<double, KUKA::FRI::LBRState::NUMBER_OF_JOINTS, 1>>(

lbr_ros2_control/config/lbr_controllers.yaml

@@ -8,6 +8,9 @@
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    force_torque_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
     # LBR ROS 2 control broadcasters
     lbr_state_broadcaster:
       type: lbr_ros2_control/LBRStateBroadcaster
@@ -26,6 +29,11 @@
     forward_lbr_torque_command_controller:
       type: lbr_ros2_control/ForwardLBRTorqueCommandController
 
+/**/force_torque_broadcaster:
+  ros__parameters:
+    frame_id: lbr/link_ee
+    sensor_name: estimated_ft_sensor
+
 /**/joint_trajectory_controller:
   ros__parameters:
     joints:

lbr_ros2_control/config/lbr_system_interface.xacro

@@ -64,7 +64,7 @@
             <!-- define lbr specific state interfaces as sensor, see
             https://github.com/ros-controls/roadmap/blob/master/design_drafts/components_architecture_and_urdf_examples.md -->
             <xacro:unless value="${sim}">
-                <sensor name="fri_sensor">
+                <sensor name="auxiliary_sensor">
                     <!-- see KUKA::FRI::LBRState -->
                     <state_interface name="sample_time" />
                     <state_interface name="session_state" />
@@ -79,6 +79,15 @@
                     <state_interface name="time_stamp_nano_sec" />
                     <state_interface name="tracking_performance" />
                 </sensor>
+
+                <sensor name="estimated_ft_sensor">
+                    <state_interface name="force.x" />
+                    <state_interface name="force.y" />
+                    <state_interface name="force.z" />
+                    <state_interface name="torque.x" />
+                    <state_interface name="torque.y" />
+                    <state_interface name="torque.z" />
+                </sensor>
             </xacro:unless>
 
             <!-- define joints and command/state interfaces for each joint -->

lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp

@@ -24,6 +24,7 @@
 #include "lbr_fri_ros2/command_guard.hpp"
 #include "lbr_fri_ros2/enum_maps.hpp"
 #include "lbr_fri_ros2/filters.hpp"
+#include "lbr_fri_ros2/ft_estimator.hpp"
 #include "lbr_fri_ros2/state_interface.hpp"
 #include "lbr_ros2_control/system_interface_type_values.hpp"
 
@@ -46,11 +47,13 @@ struct SystemInterfaceParameters {
 
 class SystemInterface : public hardware_interface::SystemInterface {
 protected:
-  static constexpr char LOGGER[] = "lbr_ros2_control::SystemInterface";
+  static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface";
 
   static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7;
   static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2;
-  static constexpr uint8_t LBR_FRI_SENSOR_SIZE = 12;
+  static constexpr uint8_t LBR_FRI_SENSORS = 2;
+  static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12;
+  static constexpr uint8_t ESTIMATED_FT_SENSOR_SIZE = 6;
 
 public:
   SystemInterface() = default;
@@ -83,6 +86,8 @@ class SystemInterface : public hardware_interface::SystemInterface {
   bool verify_joint_command_interfaces_();
   bool verify_joint_state_interfaces_();
   bool verify_sensors_();
+  bool verify_auxiliary_sensor_();
+  bool verify_estimated_ft_sensor_();
 
   // monitor end of commanding active
   bool exit_commanding_active_(const KUKA::FRI::ESessionState &previous_session_state,
@@ -112,7 +117,7 @@ class SystemInterface : public hardware_interface::SystemInterface {
   double hw_time_stamp_sec_;
   double hw_time_stamp_nano_sec_;
 
-  // added velocity state interface
+  // additional velocity state interface
   lbr_fri_msgs::msg::LBRState::_measured_joint_position_type last_hw_measured_joint_position_;
   double last_hw_time_stamp_sec_;
   double last_hw_time_stamp_nano_sec_;
@@ -124,6 +129,10 @@ class SystemInterface : public hardware_interface::SystemInterface {
   void update_last_hw_states_();
   void compute_hw_velocity_();
 
+  // additional force-torque state interface
+  lbr_fri_ros2::FTEstimator::cart_array_t hw_ft_;
+  std::unique_ptr<lbr_fri_ros2::FTEstimator> ft_estimator_ptr_;
+
   // exposed command interfaces
   lbr_fri_msgs::msg::LBRCommand hw_lbr_command_;
 };

lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp

@@ -26,6 +26,14 @@ constexpr char HW_IF_EXTERNAL_TORQUE[] = "external_torque";
 constexpr char HW_IF_IPO_JOINT_POSITION[] = "ipo_joint_position";
 constexpr char HW_IF_TRACKING_PERFORMANCE[] = "tracking_performance";
 
+// additional estimated force-torque state interface
+constexpr char HW_IF_FORCE_X[] = "force.x";
+constexpr char HW_IF_FORCE_Y[] = "force.y";
+constexpr char HW_IF_FORCE_Z[] = "force.z";
+constexpr char HW_IF_TORQUE_X[] = "torque.x";
+constexpr char HW_IF_TORQUE_Y[] = "torque.y";
+constexpr char HW_IF_TORQUE_Z[] = "torque.z";
+
 // additional LBR command interfaces, reference KUKA::FRI::LBRCommand
 constexpr char HW_IF_WRENCH[] = "wrench";
 } // end of namespace lbr_ros2_control

lbr_ros2_control/launch/system_interface.launch.py

@@ -28,6 +28,9 @@ def generate_launch_description() -> LaunchDescription:
     joint_state_broadcaster = LBRSystemInterface.node_controller_spawner(
         controller="joint_state_broadcaster"
     )
+    force_torque_broadcaster = LBRSystemInterface.node_controller_spawner(
+        controller="force_torque_broadcaster"
+    )
     lbr_state_broadcaster = LBRSystemInterface.node_controller_spawner(
         controller="lbr_state_broadcaster"
     )
@@ -39,6 +42,7 @@ def generate_launch_description() -> LaunchDescription:
             target_action=ros2_control_node,
             on_start=[
                 joint_state_broadcaster,
+                force_torque_broadcaster,
                 lbr_state_broadcaster,
                 controller,
             ],

lbr_ros2_control/src/lbr_state_broadcaster.cpp

@@ -50,29 +50,29 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time
   if (rt_state_publisher_ptr_->trylock()) {
     // FRI related states
     rt_state_publisher_ptr_->msg_.client_command_mode =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_CLIENT_COMMAND_MODE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_CLIENT_COMMAND_MODE]);
     rt_state_publisher_ptr_->msg_.connection_quality =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_CONNECTION_QUALITY]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_CONNECTION_QUALITY]);
     rt_state_publisher_ptr_->msg_.control_mode =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_CONTROL_MODE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_CONTROL_MODE]);
     rt_state_publisher_ptr_->msg_.drive_state =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_DRIVE_STATE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_DRIVE_STATE]);
     rt_state_publisher_ptr_->msg_.operation_mode =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_OPERATION_MODE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_OPERATION_MODE]);
     rt_state_publisher_ptr_->msg_.overlay_type =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_OVERLAY_TYPE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_OVERLAY_TYPE]);
     rt_state_publisher_ptr_->msg_.safety_state =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_SAFETY_STATE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_SAFETY_STATE]);
     rt_state_publisher_ptr_->msg_.sample_time =
-        state_interface_map_["fri_sensor"][HW_IF_SAMPLE_TIME];
+        state_interface_map_["auxiliary_sensor"][HW_IF_SAMPLE_TIME];
     rt_state_publisher_ptr_->msg_.session_state =
-        static_cast<int8_t>(state_interface_map_["fri_sensor"][HW_IF_SESSION_STATE]);
+        static_cast<int8_t>(state_interface_map_["auxiliary_sensor"][HW_IF_SESSION_STATE]);
     rt_state_publisher_ptr_->msg_.time_stamp_nano_sec =
-        static_cast<uint32_t>(state_interface_map_["fri_sensor"][HW_IF_TIME_STAMP_NANO_SEC]);
+        static_cast<uint32_t>(state_interface_map_["auxiliary_sensor"][HW_IF_TIME_STAMP_NANO_SEC]);
     rt_state_publisher_ptr_->msg_.time_stamp_sec =
-        static_cast<uint32_t>(state_interface_map_["fri_sensor"][HW_IF_TIME_STAMP_SEC]);
+        static_cast<uint32_t>(state_interface_map_["auxiliary_sensor"][HW_IF_TIME_STAMP_SEC]);
     rt_state_publisher_ptr_->msg_.tracking_performance =
-        state_interface_map_["fri_sensor"][HW_IF_TRACKING_PERFORMANCE];
+        state_interface_map_["auxiliary_sensor"][HW_IF_TRACKING_PERFORMANCE];
 
     // joint related states
     std::for_each(joint_names_.begin(), joint_names_.end(),