Change the ForceTorqueSensorBroadcaster to a ChainableController to e…

…xpose interfaces removing the offsets

force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml

@@ -1,6 +1,6 @@
 <library path="force_torque_sensor_broadcaster">
   <class name="force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"
-         type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ControllerInterface">
+         type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface">
   <description>
     This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message.
   </description>

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

@@ -20,8 +20,9 @@
 #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
 
 #include <memory>
+#include <vector>
 
-#include "controller_interface/controller_interface.hpp"
+#include "controller_interface/chainable_controller_interface.hpp"
 #include "force_torque_sensor_broadcaster/visibility_control.h"
 // auto-generated by generate_parameter_library
 #include "force_torque_sensor_broadcaster_parameters.hpp"
@@ -32,7 +33,7 @@
 
 namespace force_torque_sensor_broadcaster
 {
-class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface
+class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface
 {
 public:
   FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
@@ -59,9 +60,16 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte
     const rclcpp_lifecycle::State & previous_state) override;
 
   FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
-  controller_interface::return_type update(
+  controller_interface::return_type update_and_write_commands(
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
+  FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+  FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
+  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
+
 protected:
   void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg);
 

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -24,7 +24,7 @@
 namespace force_torque_sensor_broadcaster
 {
 ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster()
-: controller_interface::ControllerInterface()
+: controller_interface::ChainableControllerInterface()
 {
 }
 
@@ -141,7 +141,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type ForceTorqueSensorBroadcaster::update(
+controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands(
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   if (param_listener_->is_old(params_))
@@ -159,6 +159,65 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update(
   return controller_interface::return_type::OK;
 }
 
+controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers(
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+  return controller_interface::return_type::OK;
+}
+
+std::vector<hardware_interface::StateInterface>
+ForceTorqueSensorBroadcaster::on_export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> exported_state_interfaces;
+
+  std::vector<std::string> force_names(
+    {params_.interface_names.force.x, params_.interface_names.force.y,
+     params_.interface_names.force.z});
+  std::vector<std::string> torque_names(
+    {params_.interface_names.torque.x, params_.interface_names.torque.y,
+     params_.interface_names.torque.z});
+  if (!params_.sensor_name.empty())
+  {
+    const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names();
+    std::copy(
+      semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin());
+    std::copy(
+      semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin());
+  }
+  const std::string controller_name = get_node()->get_name();
+  if (!force_names[0].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x));
+  }
+  if (!force_names[1].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y));
+  }
+  if (!force_names[2].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z));
+  }
+  if (!torque_names[0].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x));
+  }
+  if (!torque_names[1].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y));
+  }
+  if (!torque_names[2].empty())
+  {
+    exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
+      controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z));
+  }
+  return exported_state_interfaces;
+}
+
 void ForceTorqueSensorBroadcaster::apply_sensor_offset(
   const Params & params, geometry_msgs::msg::WrenchStamped & msg)
 {
@@ -175,4 +234,4 @@ void ForceTorqueSensorBroadcaster::apply_sensor_offset(
 
 PLUGINLIB_EXPORT_CLASS(
   force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster,
-  controller_interface::ControllerInterface)
+  controller_interface::ChainableControllerInterface)