diff --git a/include/gz/sensors/ForceTorqueSensor.hh b/include/gz/sensors/ForceTorqueSensor.hh index c6cf1c85..784fe3d3 100644 --- a/include/gz/sensors/ForceTorqueSensor.hh +++ b/include/gz/sensors/ForceTorqueSensor.hh @@ -26,6 +26,8 @@ #include +#include + #include #include @@ -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 index 85d00791..db449c65 100644 --- 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; @@ -92,6 +95,10 @@ class gz::sensors::ForceTorqueSensorPrivate ForceTorqueSensor::ForceTorqueSensor() : dataPtr(std::make_unique()) { + // 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 { diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc index 488e2b25..1f0694ea 100644 --- a/test/integration/force_torque.cc +++ b/test/integration/force_torque.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -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());