Timestamp for IRangefinder2D

src/devices/Rangefinder2DClient/Rangefinder2DClient.cpp

@@ -116,14 +116,6 @@ inline int Rangefinder2DInputPortProcessor::getLast(yarp::dev::LaserScan2D&data,
     return ret;
 }
 
-bool Rangefinder2DInputPortProcessor::getData(yarp::sig::Vector &ranges)
-{
-    mutex.lock();
-    ranges= lastScan.scans;
-    mutex.unlock();
-    return true;
-}
-
 yarp::dev::IRangefinder2D::Device_status Rangefinder2DInputPortProcessor::getStatus()
 {
     mutex.lock();
@@ -290,30 +282,42 @@ bool Rangefinder2DClient::close()
     return true;
 }
 
-bool Rangefinder2DClient::getRawData(yarp::sig::Vector &data)
+bool Rangefinder2DClient::getRawData(yarp::sig::Vector &data, double* timestamp)
 {
-    inputPort.getData(data);
+    yarp::dev::LaserScan2D scan;
+    inputPort.getLast(scan, lastTs);
+
+    data = scan.scans;
+
+    if (timestamp != nullptr)
+    {
+        *timestamp = lastTs.getTime();
+    }
     return true;
 }
 
-bool Rangefinder2DClient::getLaserMeasurement(std::vector<LaserMeasurementData> &data)
+bool Rangefinder2DClient::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
 {
-    yarp::sig::Vector ranges;
-    inputPort.getData(ranges);
-    size_t size = ranges.size();
+    yarp::dev::LaserScan2D scan;
+    inputPort.getLast(scan, lastTs);
+    size_t size = scan.scans.size();
     data.resize(size);
     if (scan_angle_max < scan_angle_min) { yCError(RANGEFINDER2DCLIENT) << "getLaserMeasurement failed"; return false; }
     double laser_angle_of_view = scan_angle_max - scan_angle_min;
     for (size_t i = 0; i < size; i++)
     {
         double angle = (i / double(size)*laser_angle_of_view + device_position_theta + scan_angle_min)* DEG2RAD;
-        double value = ranges[i];
+        double value = scan.scans[i];
 #if 1 //cartesian version is preferable, even if more computationally expensive, since it takes in account device_position
         data[i].set_cartesian(value * cos(angle) + device_position_x, value * sin(angle) + device_position_y);
 #else
         data[i].set_polar(value,angle);
 #endif
     }
+    if (timestamp!=nullptr)
+    {
+        *timestamp = lastTs.getTime();
+    }
     return true;
 }
 

src/devices/Rangefinder2DClient/Rangefinder2DClient.h

@@ -55,7 +55,6 @@ class Rangefinder2DInputPortProcessor :
     // time is in ms
     void getEstFrequency(int &ite, double &av, double &min, double &max);
 
-    bool getData(yarp::sig::Vector &data);
     yarp::dev::IRangefinder2D::Device_status getStatus();
 
 };
@@ -115,14 +114,14 @@ class Rangefinder2DClient:
     * @param data a vector containing the measurement data, expressed in cartesian/polar format
     * @return true/false..
     */
-    bool getLaserMeasurement(std::vector<yarp::dev::LaserMeasurementData> &data) override;
+    bool getLaserMeasurement(std::vector<yarp::dev::LaserMeasurementData> &data, double* timestamp = nullptr) override;
 
     /**
     * Get the device measurements
     * @param ranges the vector containing the raw measurement data, as acquired by the device.
     * @return true/false.
     */
-    bool getRawData(yarp::sig::Vector &data) override;
+    bool getRawData(yarp::sig::Vector &data, double* timestamp = nullptr) override;
 
     /**
     * get the device status

src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_ros.cpp

@@ -217,13 +217,15 @@ void Rangefinder2D_nws_ros::run()
         bool ret = true;
         IRangefinder2D::Device_status status;
         yarp::sig::Vector ranges;
-        ret &= sens_p->getRawData(ranges);
+        double synchronized_timestamp=0;
+        ret &= sens_p->getRawData(ranges, &synchronized_timestamp);
         ret &= sens_p->getDeviceStatus(status);
 
         if (ret)
         {
             if (iTimed) {
-                lastStateStamp = iTimed->getLastInputStamp();
+                //lastStateStamp = iTimed->getLastInputStamp();
+                lastStateStamp.update(synchronized_timestamp);
             } else {
                 lastStateStamp.update(yarp::os::Time::now());
             }

src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_yarp.cpp

@@ -380,13 +380,15 @@ void Rangefinder2D_nws_yarp::run()
         bool ret = true;
         IRangefinder2D::Device_status status;
         yarp::sig::Vector ranges;
-        ret &= sens_p->getRawData(ranges);
+        double synchronized_timestamp = 0;
+        ret &= sens_p->getRawData(ranges, &synchronized_timestamp);
         ret &= sens_p->getDeviceStatus(status);
 
         if (ret)
         {
             if (iTimed) {
-                lastStateStamp = iTimed->getLastInputStamp();
+                //lastStateStamp = iTimed->getLastInputStamp();
+                lastStateStamp.update(synchronized_timestamp);
             } else {
                 lastStateStamp.update(yarp::os::Time::now());
             }

src/devices/laserHokuyo/laserHokuyo.cpp

@@ -331,7 +331,7 @@ bool laserHokuyo::setScanRate(double rate)
     return false;
 }
 
-bool laserHokuyo::getRawData(yarp::sig::Vector &out)
+bool laserHokuyo::getRawData(yarp::sig::Vector &out, double* timestamp)
 {
     if (internal_status != HOKUYO_STATUS_NOT_READY)
     {
@@ -346,7 +346,7 @@ bool laserHokuyo::getRawData(yarp::sig::Vector &out)
     return false;
 }
 
-bool laserHokuyo::getLaserMeasurement(std::vector<LaserMeasurementData> &data)
+bool laserHokuyo::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
 {
     if (internal_status != HOKUYO_STATUS_NOT_READY)
     {

src/devices/laserHokuyo/laserHokuyo.h

@@ -104,8 +104,8 @@ class laserHokuyo : public PeriodicThread, public yarp::dev::IRangefinder2D, pub
 
 public:
     //IRangefinder2D interface
-    bool getRawData(yarp::sig::Vector &data) override;
-    bool getLaserMeasurement(std::vector<LaserMeasurementData> &data) override;
+    bool getRawData(yarp::sig::Vector &data, double* timestamp) override;
+    bool getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp) override;
     bool getDeviceStatus     (Device_status &status) override;
     bool getDeviceInfo       (std::string &device_info) override;
     bool getDistanceRange    (double& min, double& max) override;

src/devices/rpLidar/rpLidar.cpp

@@ -267,15 +267,15 @@ bool RpLidar::setScanRate(double rate)
 }
 
 
-bool RpLidar::getRawData(yarp::sig::Vector &out)
+bool RpLidar::getRawData(yarp::sig::Vector &out, double* timestamp)
 {
     std::lock_guard<std::mutex> guard(mutex);
     out = laser_data;
     device_status = yarp::dev::IRangefinder2D::DEVICE_OK_IN_USE;
     return true;
 }
 
-bool RpLidar::getLaserMeasurement(std::vector<LaserMeasurementData> &data)
+bool RpLidar::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
 {
     std::lock_guard<std::mutex> guard(mutex);
 #ifdef LASER_DEBUG

src/devices/rpLidar/rpLidar.h

@@ -234,8 +234,8 @@ class RpLidar : public PeriodicThread, public yarp::dev::IRangefinder2D, public
 
 public:
     //IRangefinder2D interface
-    bool getRawData(yarp::sig::Vector &data) override;
-    bool getLaserMeasurement(std::vector<LaserMeasurementData> &data) override;
+    bool getRawData(yarp::sig::Vector &data, double* timestamp) override;
+    bool getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp) override;
     bool getDeviceStatus     (Device_status &status) override;
     bool getDeviceInfo       (std::string &device_info) override;
     bool getDistanceRange    (double& min, double& max) override;

src/libYARP_dev/src/yarp/dev/IRangefinder2D.h

@@ -45,16 +45,18 @@ class YARP_dev_API yarp::dev::IRangefinder2D
     /**
     * Get the device measurements
     * @param data a vector containing the measurement data, expressed in cartesian/polar format
-    * @return true/false..
+    * @param timestamp the timestamp of the retrieved data.
+    * @return true/false
     */
-    virtual bool getLaserMeasurement(std::vector<LaserMeasurementData> &data) = 0;
+    virtual bool getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp = nullptr) = 0;
 
     /**
     * Get the device measurements
     * @param ranges the vector containing the raw measurement data, as acquired by the device.
+    * @param timestamp the timestamp of the retrieved data.
     * @return true/false.
     */
-    virtual bool getRawData(yarp::sig::Vector &data) = 0;
+    virtual bool getRawData(yarp::sig::Vector &data, double* timestamp = nullptr) = 0;
 
     /**
     * get the device status

src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.cpp

@@ -50,10 +50,14 @@ bool Lidar2DDeviceBase::getDeviceStatus(Device_status& status)
     return true;
 }
 
-bool Lidar2DDeviceBase::getRawData(yarp::sig::Vector& out)
+bool Lidar2DDeviceBase::getRawData(yarp::sig::Vector& out, double* timestamp)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
     out = m_laser_data;
+    if (timestamp != nullptr)
+    {
+        *timestamp = m_timestamp.getTime();
+    }
     return true;
 }
 
@@ -71,7 +75,7 @@ bool Lidar2DDeviceBase::getDeviceInfo(std::string& device_info)
     return true;
 }
 
-bool Lidar2DDeviceBase::getLaserMeasurement(std::vector<LaserMeasurementData>& data)
+bool Lidar2DDeviceBase::getLaserMeasurement(std::vector<LaserMeasurementData>& data, double* timestamp)
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 #ifdef LASER_DEBUG
@@ -86,6 +90,10 @@ bool Lidar2DDeviceBase::getLaserMeasurement(std::vector<LaserMeasurementData>& d
         double angle = (i / double(size) * laser_angle_of_view + m_min_angle) * DEG2RAD;
         data[i].set_polar(m_laser_data[i], angle);
     }
+    if (timestamp!=nullptr)
+    {
+        *timestamp = m_timestamp.getTime();
+    }
     return true;
 }
 

src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.h

@@ -92,8 +92,8 @@ class YARP_dev_API Lidar2DDeviceBase: public yarp::dev::IRangefinder2D,
 
 public:
     //IRangefinder2D interface
-    bool getRawData(yarp::sig::Vector& data) override;
-    bool getLaserMeasurement(std::vector<LaserMeasurementData>& data) override;
+    bool getRawData(yarp::sig::Vector& data, double* timestamp = nullptr) override;
+    bool getLaserMeasurement(std::vector<LaserMeasurementData>& data, double* timestamp = nullptr) override;
     bool getDeviceStatus(Device_status& status) override;
     bool getDeviceInfo(std::string& device_info) override;
     bool getDistanceRange(double& min, double& max) override;