From d40172fb16a028875d32f5f87245ca3a7db60ef4 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 20 Nov 2023 14:38:58 +0000 Subject: [PATCH] added virtual ft broadcaster template --- lbr_ros2_control/CMakeLists.txt | 33 +++++++------ .../lbr_virtual_ft_broadcaster.hpp | 47 +++++++++++++++++++ lbr_ros2_control/lbr_controllers.xml | 8 ++++ lbr_ros2_control/package.xml | 1 + .../src/lbr_virtual_ft_broadcaster.cpp | 23 +++++++++ 5 files changed, 97 insertions(+), 15 deletions(-) create mode 100644 lbr_ros2_control/include/lbr_ros2_control/lbr_virtual_ft_broadcaster.hpp create mode 100644 lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index f89d7b57..256a96fe 100644 --- 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 new file mode 100644 index 00000000..b88ded4f --- /dev/null +++ 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 +#include +#include + +#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::SharedPtr wrench_stamped_publisher_ptr_; + std::shared_ptr> + 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 index 684fdfe3..e36979a7 100644 --- a/lbr_ros2_control/lbr_controllers.xml +++ b/lbr_ros2_control/lbr_controllers.xml @@ -5,4 +5,12 @@ base_class_type="controller_interface::ControllerInterface"> Broadcaster for LBRState messages, see lbr_fri_msgs/msg/LBRState.msg. + + + + Broadcaster for end-effector force-torque as estimated from external joint + torques. + \ No newline at end of file diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index e4d3afa1..e4c3a244 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -11,6 +11,7 @@ ament_cmake_python fri_vendor + geometry_msgs lbr_fri_msgs lbr_fri_ros2 pluginlib diff --git a/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp b/lbr_ros2_control/src/lbr_virtual_ft_broadcaster.cpp new file mode 100644 index 00000000..6c86230d --- /dev/null +++ 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 \ No newline at end of file