added virtual ft broadcaster template

lbr-stack · Nov 20, 2023 · d40172f · d40172f
1 parent 8f44fdf
commit d40172f
Show file tree

Hide file tree

Showing 5 changed files with 97 additions and 15 deletions.
diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt
@@ -16,15 +16,16 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
-find_package(hardware_interface REQUIRED)
 find_package(controller_interface REQUIRED)
+find_package(fri_vendor REQUIRED)
+find_package(FRIClient REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(lbr_fri_msgs REQUIRED)
+find_package(lbr_fri_ros2 REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(realtime_tools REQUIRED)
-find_package(lbr_fri_msgs REQUIRED)
-find_package(lbr_fri_ros2 REQUIRED)
-find_package(fri_vendor REQUIRED)
-find_package(FRIClient REQUIRED)
 
 # LBR ROS 2 control library
 add_library(
@@ -47,14 +48,15 @@ target_include_directories(
 # Link against dependencies
 ament_target_dependencies(
   ${PROJECT_NAME}
-  hardware_interface
   controller_interface
+  fri_vendor
+  geometry_msgs
+  hardware_interface
+  lbr_fri_msgs
+  lbr_fri_ros2
   pluginlib
   rclcpp
   realtime_tools
-  lbr_fri_msgs
-  lbr_fri_ros2
-  fri_vendor
 )
 
 target_link_libraries(${PROJECT_NAME}
@@ -70,17 +72,18 @@ ament_export_targets(
 )
 
 ament_export_dependencies(
-  hardware_interface
   controller_interface
+  fri_vendor
+  FRIClient
+  geometry_msgs
+  hardware_interface
+  lbr_fri_msgs
+  lbr_fri_ros2
   pluginlib
   rclcpp
   realtime_tools
-  lbr_fri_msgs
-  lbr_fri_ros2
-  fri_vendor
-  FRIClient
 )
-
+  
 install(
   DIRECTORY include/
   DESTINATION include

diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp
@@ -0,0 +1,47 @@
+#ifndef LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_
+#define LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_
+
+#include <limits>
+#include <memory>
+#include <stdexcept>
+
+#include "controller_interface/controller_interface.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "realtime_tools/realtime_publisher.h"
+
+#include "friClientIf.h"
+
+#include "lbr_ros2_control/lbr_system_interface_type_values.hpp"
+
+namespace lbr_ros2_control {
+class LBRVirtualFTBroadcaster : public controller_interface::ControllerInterface {
+public:
+  LBRVirtualFTBroadcaster() = default;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::return_type update(const rclcpp::Time &time,
+                                           const rclcpp::Duration &period) override;
+
+  controller_interface::CallbackReturn
+  on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+  controller_interface::CallbackReturn
+  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+protected:
+  rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_stamped_publisher_ptr_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>>
+      rt_wrench_stamped_publisher_ptr_;
+};
+} // end of namespace lbr_ros2_control
+#endif // LBR_ROS2_CONTROL__LBR_VIRTUAL_FT_BROADCASTER_HPP_
diff --git a/lbr_ros2_control/lbr_controllers.xml b/lbr_ros2_control/lbr_controllers.xml
@@ -5,4 +5,12 @@
         base_class_type="controller_interface::ControllerInterface">
         <description>Broadcaster for LBRState messages, see lbr_fri_msgs/msg/LBRState.msg.</description>
     </class>
+
+    <!-- LBR virtual force-torque broadcaster plugin -->
+    <class name="lbr_ros2_control/LBRVirtualFTBroadcaster"
+        type="lbr_ros2_control::LBRVirtualFTBroadcaster"
+        base_class_type="controller_interface::ControllerInterface">
+        <description>Broadcaster for end-effector force-torque as estimated from external joint
+            torques.</description>
+    </class>
 </library>
diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>fri_vendor</depend>
+  <depend>geometry_msgs</depend>
   <depend>lbr_fri_msgs</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>pluginlib</depend>

diff --git a/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp
@@ -0,0 +1,23 @@
+#include "lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp"
+
+namespace lbr_ros2_control {
+controller_interface::InterfaceConfiguration
+LBRVirtualFTBroadcaster::command_interface_configuration() const override;
+
+controller_interface::InterfaceConfiguration
+LBRVirtualFTBroadcaster::state_interface_configuration() const override;
+
+controller_interface::CallbackReturn LBRVirtualFTBroadcaster::on_init() override;
+
+controller_interface::return_type
+LBRVirtualFTBroadcaster::update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+controller_interface::CallbackReturn
+LBRVirtualFTBroadcaster::on_configure(const rclcpp_lifecycle::State &previous_state) override;
+
+controller_interface::CallbackReturn
+LBRVirtualFTBroadcaster::on_activate(const rclcpp_lifecycle::State &previous_state) override;
+
+controller_interface::CallbackReturn
+LBRVirtualFTBroadcaster::on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+} // end of namespace lbr_ros2_control