ForceTorqueSensor: add API for newest measurement (#449)

This allows the most recent Wrench message to be
accessed via the API instead of only via gz-transport.

This also reuses the wrench message instead of
recreating it at each Update.

Signed-off-by: Steve Peters <[email protected]>

Author: scpeters

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.

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;
 
@@ -92,6 +95,10 @@ class gz::sensors::ForceTorqueSensorPrivate
 ForceTorqueSensor::ForceTorqueSensor()
   : dataPtr(std::make_unique<ForceTorqueSensorPrivate>())
 {
+  // measuredWrench is reused, so allocate the first header data-value pair
+  auto frame = this->dataPtr->measuredWrench.mutable_header()->add_data();
+  frame->set_key("frame_id");
+  frame->add_value("");
 }
 
 //////////////////////////////////////////////////
@@ -247,11 +254,12 @@ 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");
-  frame->add_value(this->FrameId());
+  auto frame = msg.mutable_header()->mutable_data(0);
+  // header.data[0].key is already set to "frame_id" in the constructor
+  // header.data[0].value[0] is already allocated in the constructor
+  frame->set_value(0, this->FrameId());
 
   msgs::Set(msg.mutable_force(), measuredForce);
   msgs::Set(msg.mutable_torque(), measuredTorque);
@@ -288,6 +296,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
 {

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());