add offsets instead of substracting them

ros-controls · Oct 7, 2024 · f7643ba · f7643ba
1 parent b2924e2
commit f7643ba
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 13 deletions.
diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp
@@ -221,12 +221,12 @@ ForceTorqueSensorBroadcaster::on_export_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;
+  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
 

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
@@ -301,12 +301,12 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets
   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]);
+  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();
@@ -328,7 +328,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets
   {
     ASSERT_EQ(
       exported_state_interfaces[i].get_value(),
-      sensor_values_[i] - (i < 3 ? force_offsets[i] : torque_offsets[i - 3]));
+      sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3]));
   }
 }