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..ea49c401 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; @@ -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 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());