[ForceTorqueSensorBroadcaster] added force and torque offsets to the …

…parameters + export state interfaces (#1215)

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,10 +60,19 @@ 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);
+
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
 

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,23 +141,97 @@ 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_))
+  {
+    params_ = param_listener_->get_params();
+  }
   if (realtime_publisher_ && realtime_publisher_->trylock())
   {
     realtime_publisher_->msg_.header.stamp = time;
     force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench);
+    this->apply_sensor_offset(params_, realtime_publisher_->msg_);
     realtime_publisher_->unlockAndPublish();
   }
 
   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)
+{
+  msg.wrench.force.x += params.offset.force.x;
+  msg.wrench.force.y += params.offset.force.y;
+  msg.wrench.force.z += params.offset.force.z;
+  msg.wrench.torque.x += params.offset.torque.x;
+  msg.wrench.torque.y += params.offset.torque.y;
+  msg.wrench.torque.z += params.offset.torque.z;
+}
 }  // namespace force_torque_sensor_broadcaster
 
 #include "pluginlib/class_list_macros.hpp"
 
 PLUGINLIB_EXPORT_CLASS(
   force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster,
-  controller_interface::ControllerInterface)
+  controller_interface::ChainableControllerInterface)

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml

@@ -46,3 +46,36 @@ force_torque_sensor_broadcaster:
         default_value: "",
         description: "Name of the state interface with torque values around 'z' axis.",
       }
+  offset:
+    force:
+      x: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of force values on 'x' axis.",
+      }
+      y: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of force values on 'y' axis.",
+      }
+      z: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of force values on 'z' axis.",
+      }
+    torque:
+      x: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of torque values around 'x' axis.",
+      }
+      y: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of torque values around 'y' axis.",
+      }
+      z: {
+        type: double,
+        default_value: 0.0,
+        description: "The offset of torque values around 'z' axis.",
+      }

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -278,6 +278,60 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success)
   ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]);
 }
 
+TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets)
+{
+  SetUpFTSBroadcaster();
+
+  std::array<double, 3> force_offsets = {10.0, 30.0, -50.0};
+  std::array<double, 3> torque_offsets = {1.0, -1.2, -5.2};
+  // set the params 'sensor_name' and 'frame_id'
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+  fts_broadcaster_->get_node()->set_parameter({"offset.force.x", force_offsets[0]});
+  fts_broadcaster_->get_node()->set_parameter({"offset.force.y", force_offsets[1]});
+  fts_broadcaster_->get_node()->set_parameter({"offset.force.z", force_offsets[2]});
+  fts_broadcaster_->get_node()->set_parameter({"offset.torque.x", torque_offsets[0]});
+  fts_broadcaster_->get_node()->set_parameter({"offset.torque.y", torque_offsets[1]});
+  fts_broadcaster_->get_node()->set_parameter({"offset.torque.z", torque_offsets[2]});
+
+  ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  geometry_msgs::msg::WrenchStamped wrench_msg;
+  subscribe_and_get_message(wrench_msg);
+
+  ASSERT_EQ(wrench_msg.header.frame_id, frame_id_);
+  ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]);
+  ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] + force_offsets[1]);
+  ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] + force_offsets[2]);
+  ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] + torque_offsets[0]);
+  ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] + torque_offsets[1]);
+  ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] + torque_offsets[2]);
+
+  // Check the exported state interfaces
+  const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces();
+  ASSERT_EQ(exported_state_interfaces.size(), 6u);
+  const std::string controller_name = fts_broadcaster_->get_node()->get_name();
+  ASSERT_EQ(
+    exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x");
+  ASSERT_EQ(
+    exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y");
+  ASSERT_EQ(
+    exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z");
+  ASSERT_EQ(
+    exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x");
+  ASSERT_EQ(
+    exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y");
+  ASSERT_EQ(
+    exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z");
+  for (size_t i = 0; i < 6; ++i)
+  {
+    ASSERT_EQ(
+      exported_state_interfaces[i]->get_value(),
+      sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3]));
+  }
+}
+
 TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
 {
   SetUpFTSBroadcaster();
@@ -300,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
   ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x));
   ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y));
   ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]);
+
+  // Check the exported state interfaces
+  const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces();
+  ASSERT_EQ(exported_state_interfaces.size(), 2u);
+  const std::string controller_name = fts_broadcaster_->get_node()->get_name();
+  ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x");
+  ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z");
+  ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]);
+  ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]);
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
@@ -328,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
   ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]);
   ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]);
   ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]);
+
+  // Check the exported state interfaces
+  const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces();
+  ASSERT_EQ(exported_state_interfaces.size(), 6u);
+  const std::string controller_name = fts_broadcaster_->get_node()->get_name();
+  ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x");
+  ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/force.y");
+  ASSERT_EQ(exported_state_interfaces[2]->get_name(), controller_name + "/fts_sensor/force.z");
+  ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x");
+  ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y");
+  ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z");
+  for (size_t i = 0; i < 6; ++i)
+  {
+    ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]);
+  }
 }
 
 int main(int argc, char ** argv)