diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 51cb94513b..ae105a511c 100644 --- 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 index a4ddf68786..4540821630 100644 --- 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])); } }