updated correct timestamp GazeboYarpLaserSensorDriver (#604)

Co-authored-by: Silvio Traversaro <silvio@traversaro.it>
robotology · Jan 25, 2022 · bb5afed · bb5afed
1 parent de6231d
commit bb5afed
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 15 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+### Fixed
+- Removed `getLastInputStamp` method from `LaserSensorDriver` class in `gazebo_yarp_lasersensor`. Furthermore, fix bug in `gazebo_yarp_lasersensor`, by adding the update to the variable `m_timestamp` inherited from `yarp::dev::Lidar2DDeviceBase` (https://github.com/robotology/gazebo-yarp-plugins/pull/604).
+
 ## [4.1.2] - 2022-01-19
 
 ### Fixed 

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -46,7 +46,7 @@ if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
     list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface")
 endif()
 
-find_package(YARP 3.4.102 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
+find_package(YARP 3.6 REQUIRED COMPONENTS os sig dev math idl_tools ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
 find_package(Gazebo REQUIRED)
 if (Gazebo_VERSION_MAJOR LESS 11.0)
     message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH})

diff --git a/plugins/lasersensor/include/yarp/dev/LaserSensorDriver.h b/plugins/lasersensor/include/yarp/dev/LaserSensorDriver.h
@@ -12,7 +12,6 @@
 #include <yarp/dev/LaserMeasurementData.h>
 #include <yarp/dev/Lidar2DDeviceBase.h>
 #include <yarp/os/Stamp.h>
-#include <yarp/dev/IPreciselyTimed.h>
 #include <boost/shared_ptr.hpp>
 
 #include <gazebo/common/Plugin.hh>
@@ -64,11 +63,6 @@ class yarp::dev::GazeboYarpLaserSensorDriver:
     virtual bool setHorizontalResolution (double step) override;
     virtual bool setScanRate (double rate) override;
 
-    //PRECISELY TIMED
-    // TODO(traversaro): Remove once we require YARP 3.6
-    // See https://github.com/robotology/gazebo-yarp-plugins/issues/598
-    virtual yarp::os::Stamp getLastInputStamp();
-
 public:
     //Lidar2DDeviceBase
     bool acquireDataFromHW() override final;
@@ -83,7 +77,6 @@ class yarp::dev::GazeboYarpLaserSensorDriver:
     double m_gazebo_scan_rate;
     bool   m_first_run;
 
-    yarp::os::Stamp m_lastTimestamp; //buffer for last timestamp data
     gazebo::sensors::RaySensor* m_parentSensor;
     gazebo::event::ConnectionPtr m_updateConnection;
 

diff --git a/plugins/lasersensor/src/LaserSensorDriver.cpp b/plugins/lasersensor/src/LaserSensorDriver.cpp
@@ -52,7 +52,8 @@ void GazeboYarpLaserSensorDriver::onUpdate(const gazebo::common::UpdateInfo& _in
 #endif
 
     this->applyLimitsOnLaserData();
-    m_lastTimestamp.update(_info.simTime.Double());
+    m_timestamp.update(_info.simTime.Double());
+
     m_first_run = false;
     return;
 }
@@ -111,12 +112,6 @@ bool GazeboYarpLaserSensorDriver::acquireDataFromHW()
     return true;
 }
 
-//PRECISELY TIMED
-yarp::os::Stamp GazeboYarpLaserSensorDriver::getLastInputStamp()
-{
-    return m_lastTimestamp;
-}
-
 bool GazeboYarpLaserSensorDriver::setDistanceRange (double min, double max)
 {
     std::lock_guard<std::mutex> guard(m_mutex);