diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index d5b3f792f2..c36b1f0dd4 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -12,13 +12,18 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(control_toolbox REQUIRED) find_package(controller_interface REQUIRED) +find_package(filters REQUIRED) find_package(geometry_msgs REQUIRED) find_package(hardware_interface REQUIRED) 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 +37,18 @@ target_include_directories( ) ament_target_dependencies( force_torque_sensor_broadcaster - geometry_msgs + control_toolbox controller_interface + filters + 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. @@ -102,12 +112,18 @@ ament_export_libraries( force_torque_sensor_broadcaster ) ament_export_dependencies( + control_toolbox controller_interface + filters + 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..fafc67b066 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 @@ -24,16 +24,21 @@ #include #include "controller_interface/controller_interface.hpp" +#include "filters/filter_chain.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #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 +71,27 @@ 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_; + WrenchMsgType wrench_filtered_; + std::unique_ptr> filter_chain_; + + using WrenchPublisher = rclcpp::Publisher::SharedPtr; + using WrenchRTPublisher = realtime_tools::RealtimePublisher; + WrenchPublisher wrench_raw_pub_; + std::unique_ptr wrench_raw_publisher_; + WrenchPublisher wrench_filtered_pub_; + std::unique_ptr wrench_filtered_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..f445be535d 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -12,12 +12,17 @@ ament_cmake controller_interface + filters + control_toolbox geometry_msgs hardware_interface pluginlib 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..89906272fd 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -18,9 +18,13 @@ #include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp" +#include #include #include +#include "tf2/utils.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() @@ -32,14 +36,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 +59,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 +73,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 +82,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 +110,38 @@ 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_); + filter_chain_ = + std::make_unique>("geometry_msgs::msg::WrenchStamped"); + } + catch (const std::exception & e) + { + fprintf( + stderr, + "Exception thrown during filter chain creation at configure stage with message : %s \n", + e.what()); + return CallbackReturn::ERROR; + } + + if (!filter_chain_->configure( + "sensor_filter_chain", get_node()->get_node_logging_interface(), + get_node()->get_node_parameters_interface())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Could not configure sensor filter chain, please check if the " + "parameters are provided correctly."); + return CallbackReturn::ERROR; + } + + try + { + // register ft sensor data publishers + wrench_raw_pub_ = + get_node()->create_publisher("~/wrench", rclcpp::SystemDefaultsQoS()); + wrench_raw_publisher_ = std::make_unique(wrench_raw_pub_); + wrench_filtered_pub_ = + get_node()->create_publisher("~/wrench_filtered", rclcpp::SystemDefaultsQoS()); + wrench_filtered_publisher_ = std::make_unique(wrench_filtered_pub_); } catch (const std::exception & e) { @@ -117,11 +151,43 @@ 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_filtered_.header.frame_id = frame_id_; - RCLCPP_DEBUG(node_->get_logger(), "configure successful"); + wrench_raw_publisher_->lock(); + wrench_raw_publisher_->msg_.header.frame_id = frame_id_; + wrench_raw_publisher_->unlock(); + wrench_filtered_publisher_->lock(); + wrench_filtered_publisher_->msg_.header.frame_id = frame_id_; + wrench_filtered_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_filtered_" + 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_INFO(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } @@ -159,11 +225,54 @@ 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); + + // Filter sensor data + filter_chain_->update(wrench_raw_, wrench_filtered_); + + // 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(); + } + + if (wrench_filtered_publisher_ && wrench_filtered_publisher_->trylock()) { - realtime_publisher_->msg_.header.stamp = time; - force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); - realtime_publisher_->unlockAndPublish(); + wrench_filtered_publisher_->msg_.header.stamp = time; + wrench_filtered_publisher_->msg_.wrench = wrench_filtered_.wrench; + wrench_filtered_publisher_->unlockAndPublish(); + } + + for (const auto & publisher : wrench_aditional_frames_publishers_) + { + try + { + auto transform = + tf_buffer_->lookupTransform(publisher->msg_.header.frame_id, frame_id_, tf2::TimePointZero); + tf2::doTransform(wrench_filtered_, 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;