Add multiplier support to ForceTorqueSensorBroadcaster (#1647)

Signed-off-by: Edward <[email protected]>

Author: edxmorgan

doc/release_notes.rst

@@ -3,3 +3,7 @@
 Release Notes: Jazzy to Kilted
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 This list summarizes the changes between Jazzy (previous) and Kilted (current) releases.
+
+force_torque_sensor_broadcaster
+*******************************
+* Multiplier support was added. Users can now specify per–axis scaling factors for both force and torque readings, applied after the existing offset logic. (`#1647 <https://github.com/ros-controls/ros2_controllers/pull/1647/files>`__.

force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp

@@ -61,6 +61,7 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ChainableContr
 
 protected:
   void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg);
+  void apply_sensor_multiplier(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

@@ -175,6 +175,7 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write
     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_);
+    this->apply_sensor_multiplier(params_, realtime_publisher_->msg_);
     realtime_publisher_->unlockAndPublish();
   }
 
@@ -267,6 +268,17 @@ void ForceTorqueSensorBroadcaster::apply_sensor_offset(
   msg.wrench.torque.y += params.offset.torque.y;
   msg.wrench.torque.z += params.offset.torque.z;
 }
+
+void ForceTorqueSensorBroadcaster::apply_sensor_multiplier(
+  const Params & params, geometry_msgs::msg::WrenchStamped & msg)
+{
+  msg.wrench.force.x *= params.multiplier.force.x;
+  msg.wrench.force.y *= params.multiplier.force.y;
+  msg.wrench.force.z *= params.multiplier.force.z;
+  msg.wrench.torque.x *= params.multiplier.torque.x;
+  msg.wrench.torque.y *= params.multiplier.torque.y;
+  msg.wrench.torque.z *= params.multiplier.torque.z;
+}
 }  // namespace force_torque_sensor_broadcaster
 
 #include "pluginlib/class_list_macros.hpp"

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml

@@ -79,3 +79,36 @@ force_torque_sensor_broadcaster:
         default_value: 0.0,
         description: "The offset of torque values around 'z' axis.",
       }
+  multiplier:
+    force:
+      x: {
+        type: double,
+        default_value: 1.0,
+        description: "The multiplier of force value on 'x' axis.",
+      }
+      y: {
+        type: double,
+        default_value: 1.0,
+        description: "The multiplier of force value on 'y' axis.",
+      }
+      z: {
+        type: double,
+        default_value: 1.0,
+        description: "The multiplier of force value on 'z' axis.",
+      }
+    torque:
+      x: {
+        type: double,
+        default_value: 1.0,
+        description: "The multiplier of torque value around 'x' axis.",
+      }
+      y: {
+        type: double,
+        default_value: 1.0,
+        description: "The multiplier of torque value around 'y' axis.",
+      }
+      z: {
+        type: double,
+        default_value: 1.0,
+        description: "The multiplier of torque value around 'z' axis.",
+      }

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -355,6 +355,76 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets
   }
 }
 
+TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Multipliers)
+{
+  SetUpFTSBroadcaster();
+
+  // some non‐trivial multipliers
+  std::array<double, 3> force_multipliers = {{2.0, 0.5, -1.0}};
+  std::array<double, 3> torque_multipliers = {{-2.0, 3.0, 0.0}};
+
+  // Set the required params
+  fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+  fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+
+  // Set all multiplier parameters
+  fts_broadcaster_->get_node()->set_parameter({"multiplier.force.x", force_multipliers[0]});
+  fts_broadcaster_->get_node()->set_parameter({"multiplier.force.y", force_multipliers[1]});
+  fts_broadcaster_->get_node()->set_parameter({"multiplier.force.z", force_multipliers[2]});
+  fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.x", torque_multipliers[0]});
+  fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.y", torque_multipliers[1]});
+  fts_broadcaster_->get_node()->set_parameter({"multiplier.torque.z", torque_multipliers[2]});
+
+  // Configure & activate
+  ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+  ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  // Publish & grab the message
+  geometry_msgs::msg::WrenchStamped wrench_msg;
+  subscribe_and_get_message(wrench_msg);
+
+  // Check header
+  ASSERT_EQ(wrench_msg.header.frame_id, frame_id_);
+
+  // Check that each field was scaled accordingly
+  ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] * force_multipliers[0]);
+  ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] * force_multipliers[1]);
+  ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] * force_multipliers[2]);
+  ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] * torque_multipliers[0]);
+  ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] * torque_multipliers[1]);
+  ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] * torque_multipliers[2]);
+
+  // the exported state interfaces reflect the same scaled values
+  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");
+  ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "force.x");
+  ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "force.y");
+  ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "force.z");
+  ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "torque.x");
+  ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "torque.y");
+  ASSERT_EQ(exported_state_interfaces[5]->get_interface_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_multipliers[i] : torque_multipliers[i - 3]));
+  }
+}
+
 TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
 {
   SetUpFTSBroadcaster();