diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index d5b3f792f2..575553423d 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -19,6 +19,9 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) add_library( force_torque_sensor_broadcaster @@ -32,13 +35,16 @@ target_include_directories( ) ament_target_dependencies( force_torque_sensor_broadcaster - geometry_msgs controller_interface + geometry_msgs hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools + tf2 + tf2_geometry_msgs + tf2_ros ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -103,11 +109,15 @@ ament_export_libraries( ) ament_export_dependencies( controller_interface + geometry_msgs + hardware_interface pluginlib rclcpp rclcpp_lifecycle - geometry_msgs - hardware_interface + realtime_tools + tf2 + tf2_geometry_msgs + tf2_ros ) ament_package() diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index a1c0be6335..362163dab6 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -30,10 +30,14 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/force_torque_sensor.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" namespace force_torque_sensor_broadcaster { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using WrenchMsgType = geometry_msgs::msg::WrenchStamped; class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface { @@ -66,12 +70,23 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte std::string sensor_name_; std::array interface_names_; std::string frame_id_; + std::vector additional_frames_to_publish_; std::unique_ptr force_torque_sensor_; - using StatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr sensor_state_publisher_; - std::unique_ptr realtime_publisher_; + WrenchMsgType wrench_raw_; + + using WrenchPublisher = rclcpp::Publisher::SharedPtr; + using WrenchRTPublisher = realtime_tools::RealtimePublisher; + WrenchPublisher wrench_raw_pub_; + std::unique_ptr wrench_raw_publisher_; + + // Transformation variables + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::vector wrench_aditional_frames_pubs_; + std::vector> wrench_aditional_frames_publishers_; }; } // namespace force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 35dbe9e3be..dfb18c7110 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -18,6 +18,9 @@ rclcpp rclcpp_lifecycle realtime_tools + tf2 + tf2_geometry_msgs + tf2_ros ament_cmake_gmock controller_manager diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 5f5c880c39..a13787d7fa 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -18,8 +18,13 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include #include #include +#include + +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace force_torque_sensor_broadcaster { @@ -32,14 +37,16 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init() { try { - auto_declare("sensor_name", ""); - auto_declare("interface_names.force.x", ""); - auto_declare("interface_names.force.y", ""); - auto_declare("interface_names.force.z", ""); - auto_declare("interface_names.torque.x", ""); - auto_declare("interface_names.torque.y", ""); - auto_declare("interface_names.torque.z", ""); - auto_declare("frame_id", ""); + get_node()->declare_parameter("sensor_name", ""); + get_node()->declare_parameter("interface_names.force.x", ""); + get_node()->declare_parameter("interface_names.force.y", ""); + get_node()->declare_parameter("interface_names.force.z", ""); + get_node()->declare_parameter("interface_names.torque.x", ""); + get_node()->declare_parameter("interface_names.torque.y", ""); + get_node()->declare_parameter("interface_names.torque.z", ""); + get_node()->declare_parameter("frame_id", ""); + get_node()->declare_parameter>( + "additional_frames_to_publish", std::vector({})); } catch (const std::exception & e) { @@ -53,13 +60,13 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_init() CallbackReturn ForceTorqueSensorBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - sensor_name_ = node_->get_parameter("sensor_name").as_string(); - interface_names_[0] = node_->get_parameter("interface_names.force.x").as_string(); - interface_names_[1] = node_->get_parameter("interface_names.force.y").as_string(); - interface_names_[2] = node_->get_parameter("interface_names.force.z").as_string(); - interface_names_[3] = node_->get_parameter("interface_names.torque.x").as_string(); - interface_names_[4] = node_->get_parameter("interface_names.torque.y").as_string(); - interface_names_[5] = node_->get_parameter("interface_names.torque.z").as_string(); + sensor_name_ = get_node()->get_parameter("sensor_name").as_string(); + interface_names_[0] = get_node()->get_parameter("interface_names.force.x").as_string(); + interface_names_[1] = get_node()->get_parameter("interface_names.force.y").as_string(); + interface_names_[2] = get_node()->get_parameter("interface_names.force.z").as_string(); + interface_names_[3] = get_node()->get_parameter("interface_names.torque.x").as_string(); + interface_names_[4] = get_node()->get_parameter("interface_names.torque.y").as_string(); + interface_names_[5] = get_node()->get_parameter("interface_names.torque.z").as_string(); const bool no_interface_names_defined = std::count(interface_names_.begin(), interface_names_.end(), "") == 6; @@ -67,7 +74,7 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (sensor_name_.empty() && no_interface_names_defined) { RCLCPP_ERROR( - node_->get_logger(), + get_node()->get_logger(), "'sensor_name' or at least one " "'interface_names.[force|torque].[x|y|z]' parameter has to be specified."); return CallbackReturn::ERROR; @@ -76,16 +83,16 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( if (!sensor_name_.empty() && !no_interface_names_defined) { RCLCPP_ERROR( - node_->get_logger(), + get_node()->get_logger(), "both 'sensor_name' and " "'interface_names.[force|torque].[x|y|z]' parameters can not be specified together."); return CallbackReturn::ERROR; } - frame_id_ = node_->get_parameter("frame_id").as_string(); + frame_id_ = get_node()->get_parameter("frame_id").as_string(); if (frame_id_.empty()) { - RCLCPP_ERROR(node_->get_logger(), "'frame_id' parameter has to be provided."); + RCLCPP_ERROR(get_node()->get_logger(), "'frame_id' parameter has to be provided."); return CallbackReturn::ERROR; } @@ -104,10 +111,10 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( try { - // register ft sensor data publisher - sensor_state_publisher_ = node_->create_publisher( - "~/wrench", rclcpp::SystemDefaultsQoS()); - realtime_publisher_ = std::make_unique(sensor_state_publisher_); + // register ft sensor data publishers + wrench_raw_pub_ = + get_node()->create_publisher("~/wrench", rclcpp::SystemDefaultsQoS()); + wrench_raw_publisher_ = std::make_unique(wrench_raw_pub_); } catch (const std::exception & e) { @@ -117,11 +124,39 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_configure( return CallbackReturn::ERROR; } - realtime_publisher_->lock(); - realtime_publisher_->msg_.header.frame_id = frame_id_; - realtime_publisher_->unlock(); + wrench_raw_.header.frame_id = frame_id_; + + wrench_raw_publisher_->lock(); + wrench_raw_publisher_->msg_.header.frame_id = frame_id_; + wrench_raw_publisher_->unlock(); + + // Add additional frames to publish if any exits + additional_frames_to_publish_ = + get_node()->get_parameter("additional_frames_to_publish").as_string_array(); + + if (!additional_frames_to_publish_.empty()) + { + auto nr_frames = additional_frames_to_publish_.size(); + wrench_aditional_frames_pubs_.reserve(nr_frames); + wrench_aditional_frames_publishers_.reserve(nr_frames); + for (const auto & frame : additional_frames_to_publish_) + { + WrenchPublisher pub = get_node()->create_publisher( + "~/wrench_" + frame, rclcpp::SystemDefaultsQoS()); + wrench_aditional_frames_pubs_.emplace_back(pub); + wrench_aditional_frames_publishers_.emplace_back(std::make_unique(pub)); + + wrench_aditional_frames_publishers_.back()->lock(); + wrench_aditional_frames_publishers_.back()->msg_.header.frame_id = frame; + wrench_aditional_frames_publishers_.back()->unlock(); + } + + // initialize buffer transforms + tf_buffer_ = std::make_shared(get_node()->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -159,11 +194,44 @@ CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate( controller_interface::return_type ForceTorqueSensorBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + wrench_raw_.header.stamp = time; + force_torque_sensor_->get_values_as_message(wrench_raw_.wrench); + + // Publish sensor data + if (wrench_raw_publisher_ && wrench_raw_publisher_->trylock()) + { + wrench_raw_publisher_->msg_.header.stamp = time; + wrench_raw_publisher_->msg_.wrench = wrench_raw_.wrench; + wrench_raw_publisher_->unlockAndPublish(); + } + + for (const auto & publisher : wrench_aditional_frames_publishers_) { - realtime_publisher_->msg_.header.stamp = time; - force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); - realtime_publisher_->unlockAndPublish(); + try + { + auto transform = + tf_buffer_->lookupTransform(publisher->msg_.header.frame_id, frame_id_, tf2::TimePointZero); + tf2::doTransform(wrench_raw_, publisher->msg_, transform); + + if (publisher && publisher->trylock()) + { + publisher->msg_.header.stamp = time; + publisher->unlockAndPublish(); + } + } + catch (const tf2::TransformException & e) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 5000, + "LookupTransform failed from '%s' to '%s'.", frame_id_.c_str(), + publisher->msg_.header.frame_id.c_str()); + publisher->msg_.wrench.force.x = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.force.y = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.force.z = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.torque.x = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.torque.y = std::numeric_limits::quiet_NaN(); + publisher->msg_.wrench.torque.z = std::numeric_limits::quiet_NaN(); + } } return controller_interface::return_type::OK;