diff --git a/src/devices/Rangefinder2DClient/Rangefinder2DClient.cpp b/src/devices/Rangefinder2DClient/Rangefinder2DClient.cpp index 2b8393af9b2..212c68f8184 100644 --- a/src/devices/Rangefinder2DClient/Rangefinder2DClient.cpp +++ b/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 &data) +bool Rangefinder2DClient::getLaserMeasurement(std::vector &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; } diff --git a/src/devices/Rangefinder2DClient/Rangefinder2DClient.h b/src/devices/Rangefinder2DClient/Rangefinder2DClient.h index 1d828a4af14..5f0b34d2171 100644 --- a/src/devices/Rangefinder2DClient/Rangefinder2DClient.h +++ b/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 &data) override; + bool getLaserMeasurement(std::vector &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 diff --git a/src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_ros.cpp b/src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_ros.cpp index dd4e1b3178c..377c2922e5b 100644 --- a/src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_ros.cpp +++ b/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()); } diff --git a/src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_yarp.cpp b/src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_yarp.cpp index ddbfb7d2850..3d7f3e2bbb0 100644 --- a/src/devices/Rangefinder2DWrapper/Rangefinder2D_nws_yarp.cpp +++ b/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()); } diff --git a/src/devices/laserHokuyo/laserHokuyo.cpp b/src/devices/laserHokuyo/laserHokuyo.cpp index e6a9af1e7b3..8f4caa4a4f3 100644 --- a/src/devices/laserHokuyo/laserHokuyo.cpp +++ b/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 &data) +bool laserHokuyo::getLaserMeasurement(std::vector &data, double* timestamp) { if (internal_status != HOKUYO_STATUS_NOT_READY) { diff --git a/src/devices/laserHokuyo/laserHokuyo.h b/src/devices/laserHokuyo/laserHokuyo.h index bbd87fd4811..1707f87a043 100644 --- a/src/devices/laserHokuyo/laserHokuyo.h +++ b/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 &data) override; + bool getRawData(yarp::sig::Vector &data, double* timestamp) override; + bool getLaserMeasurement(std::vector &data, double* timestamp) override; bool getDeviceStatus (Device_status &status) override; bool getDeviceInfo (std::string &device_info) override; bool getDistanceRange (double& min, double& max) override; diff --git a/src/devices/rpLidar/rpLidar.cpp b/src/devices/rpLidar/rpLidar.cpp index dedd6ce3090..0cd5e9865ec 100644 --- a/src/devices/rpLidar/rpLidar.cpp +++ b/src/devices/rpLidar/rpLidar.cpp @@ -267,7 +267,7 @@ bool RpLidar::setScanRate(double rate) } -bool RpLidar::getRawData(yarp::sig::Vector &out) +bool RpLidar::getRawData(yarp::sig::Vector &out, double* timestamp) { std::lock_guard guard(mutex); out = laser_data; @@ -275,7 +275,7 @@ bool RpLidar::getRawData(yarp::sig::Vector &out) return true; } -bool RpLidar::getLaserMeasurement(std::vector &data) +bool RpLidar::getLaserMeasurement(std::vector &data, double* timestamp) { std::lock_guard guard(mutex); #ifdef LASER_DEBUG diff --git a/src/devices/rpLidar/rpLidar.h b/src/devices/rpLidar/rpLidar.h index 5663a8c1493..77571f8f629 100644 --- a/src/devices/rpLidar/rpLidar.h +++ b/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 &data) override; + bool getRawData(yarp::sig::Vector &data, double* timestamp) override; + bool getLaserMeasurement(std::vector &data, double* timestamp) override; bool getDeviceStatus (Device_status &status) override; bool getDeviceInfo (std::string &device_info) override; bool getDistanceRange (double& min, double& max) override; diff --git a/src/libYARP_dev/src/yarp/dev/IRangefinder2D.h b/src/libYARP_dev/src/yarp/dev/IRangefinder2D.h index bc41a784a3f..bc0a4a8bf54 100644 --- a/src/libYARP_dev/src/yarp/dev/IRangefinder2D.h +++ b/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 &data) = 0; + virtual bool getLaserMeasurement(std::vector &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 diff --git a/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.cpp b/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.cpp index 38f05c6c1ef..7ab86b9cad1 100644 --- a/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.cpp +++ b/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 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& data) +bool Lidar2DDeviceBase::getLaserMeasurement(std::vector& data, double* timestamp) { std::lock_guard guard(m_mutex); #ifdef LASER_DEBUG @@ -86,6 +90,10 @@ bool Lidar2DDeviceBase::getLaserMeasurement(std::vector& 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; } diff --git a/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.h b/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.h index d47f4054818..c06fcc409fe 100644 --- a/src/libYARP_dev/src/yarp/dev/Lidar2DDeviceBase.h +++ b/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& data) override; + bool getRawData(yarp::sig::Vector& data, double* timestamp = nullptr) override; + bool getLaserMeasurement(std::vector& 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;