added force and torque offsets to the parameters of the force_torque_…

…sensor_broadcaster
ros-controls · Jul 17, 2024 · a4c5241 · a4c5241
1 parent 96a8d57
commit a4c5241
Show file tree

Hide file tree

Showing 4 changed files with 81 additions and 0 deletions.
diff --git a/...r_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/...r_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp
@@ -63,6 +63,8 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
+  void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg);
+
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
 

diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
@@ -144,16 +144,31 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate
 controller_interface::return_type ForceTorqueSensorBroadcaster::update(
   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;
 }
 
+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"

diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/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.",
+      }
diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
@@ -276,6 +276,37 @@ 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]);
+}
+
 TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
 {
   SetUpFTSBroadcaster();