ForceTorqueSensor: add API for newest measurement

This allows the most recent Wrench message to be accessed via the API instead of only via gz-transport. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Jul 19, 2024 · dacbd2e · dacbd2e
1 parent b91047f
commit dacbd2e
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 1 deletion.
diff --git a/include/gz/sensors/ForceTorqueSensor.hh b/include/gz/sensors/ForceTorqueSensor.hh
@@ -26,6 +26,8 @@
 
 #include <gz/math/Pose3.hh>
 
+#include <gz/msgs/wrench.pb.h>
+
 #include <gz/sensors/config.hh>
 #include <gz/sensors/force_torque/Export.hh>
 
@@ -93,6 +95,11 @@ namespace gz
       /// \param[in] _torque torque vector in newton.
       public: void SetTorque(const math::Vector3d &_torque);
 
+      /// \brief Get the most recent wrench measurement. This matches the data
+      /// published over the gz-transport topic.
+      /// \return The most recent wrench measurement.
+      public: const msgs::Wrench &MeasuredWrench() const;
+
       /// \brief Set the rotation of the joint parent relative to the sensor
       /// frame.
       /// \return The current rotation the parent in the sensor frame.

diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc
@@ -55,6 +55,9 @@ class gz::sensors::ForceTorqueSensorPrivate
   /// \brief Noise free torque as set by SetTorque
   public: gz::math::Vector3d torque{0, 0, 0};
 
+  /// \brief Most recent wrench measurement.
+  public: gz::msgs::Wrench measuredWrench;
+
   /// \brief Frame in which we return the measured force torque info.
   public: sdf::ForceTorqueFrame measureFrame;
 
@@ -247,7 +250,7 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now)
   applyNoise(TORQUE_Y_NOISE_N_M, measuredTorque.Y());
   applyNoise(TORQUE_Z_NOISE_N_M, measuredTorque.Z());
 
-  msgs::Wrench msg;
+  msgs::Wrench &msg = this->dataPtr->measuredWrench;
   *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);
   auto frame = msg.mutable_header()->add_data();
   frame->set_key("frame_id");
@@ -288,6 +291,12 @@ void ForceTorqueSensor::SetTorque(const math::Vector3d &_torque)
   this->dataPtr->torque = _torque;
 }
 
+//////////////////////////////////////////////////
+const msgs::Wrench &ForceTorqueSensor::MeasuredWrench() const
+{
+  return this->dataPtr->measuredWrench;
+}
+
 //////////////////////////////////////////////////
 math::Quaterniond ForceTorqueSensor::RotationParentInSensor() const
 {

diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc
@@ -15,6 +15,7 @@
  *
 */
 
+#include <google/protobuf/util/message_differencer.h>
 #include <gtest/gtest.h>
 
 #include <sdf/ForceTorque.hh>
@@ -245,6 +246,9 @@ TEST_P(ForceTorqueSensorTest, SensorReadings)
   sensor->Update(dt, false);
   EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper;
   auto msg = msgHelper.Message();
+  EXPECT_TRUE(
+      google::protobuf::util::MessageDifferencer::Equals(
+          msg, sensor->MeasuredWrench()));
   EXPECT_EQ(1, msg.header().stamp().sec());
   EXPECT_EQ(0, msg.header().stamp().nsec());