LidarVLP16.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2022, Mathieu Labbe
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the names of its contributors may be used to endorse or promote products
13  derived from this software without specific prior written permission.
14 
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
28 #include <rtabmap/utilite/UTimer.h>
30 #include <pcl/pcl_config.h>
31 
32 #if PCL_VERSION_COMPARE(<, 1, 9, 0)
33 #define VLP_MAX_NUM_LASERS 16
34 #define VLP_DUAL_MODE 0x39
35 #endif
36 
37 namespace rtabmap {
38 
53 double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp) {
54  const int HALFHOUR_TO_SEC = 1800;
55  double retval = stamp;
56  if (nominal_stamp > stamp) {
57  if (nominal_stamp - stamp > HALFHOUR_TO_SEC) {
58  retval = retval + 2*HALFHOUR_TO_SEC;
59  }
60  } else if (stamp - nominal_stamp > HALFHOUR_TO_SEC) {
61  retval = retval - 2*HALFHOUR_TO_SEC;
62  }
63  return retval;
64 }
65 
66 /*
67  * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
68  * Original license: BSD License 2.0
69  * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
70  */
71 double rosTimeFromGpsTimestamp(const uint32_t data) {
72  const int HOUR_TO_SEC = 3600;
73  // time for each packet is a 4 byte uint
74  // It is the number of microseconds from the top of the hour
75  double time_nom = UTimer::now();
76  uint32_t cur_hour = time_nom / HOUR_TO_SEC;
77  double stamp = double(cur_hour * HOUR_TO_SEC) + double(data) / 1000000;
78  stamp = resolveHourAmbiguity(stamp, time_nom);
79  return stamp;
80 }
81 
83  const std::string& pcapFile,
84  bool organized,
85  bool stampLast,
86  float frameRate,
87  Transform localTransform) :
88  Lidar(frameRate, localTransform),
89  pcl::VLPGrabber(pcapFile),
90  timingOffsetsDualMode_(false),
91  startSweepTime_(0),
92  startSweepTimeHost_(0),
93  organized_(organized),
94  useHostTime_(false),
95  stampLast_(stampLast)
96 {
97  UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str());
98 }
100  const boost::asio::ip::address& ipAddress,
101  const std::uint16_t port,
102  bool organized,
103  bool useHostTime,
104  bool stampLast,
105  float frameRate,
106  Transform localTransform) :
107  Lidar(frameRate, localTransform),
108  pcl::VLPGrabber(ipAddress, port),
109  timingOffsetsDualMode_(false),
110  startSweepTime_(0),
111  startSweepTimeHost_(0),
112  organized_(organized),
113  useHostTime_(useHostTime),
114  stampLast_(stampLast)
115 {
116  UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port);
117 }
118 
120 {
121  UDEBUG("Stopping lidar...");
122  stop();
124  UDEBUG("Stopped lidar!");
125 }
126 
127 void LidarVLP16::setOrganized(bool enable)
128 {
129  organized_ = true;
130 }
131 
132 bool LidarVLP16::init(const std::string &, const std::string &)
133 {
134  UDEBUG("Init lidar");
135  if(isRunning())
136  {
137  UDEBUG("Stopping lidar...");
138  stop();
139  uSleep(2000); // make sure all callbacks are finished
140  UDEBUG("Stopped lidar!");
141  }
142  startSweepTime_ = 0.0;
143  startSweepTimeHost_ = 0.0;
144  accumulatedScans_.clear();
145  if(organized_)
146  {
147  accumulatedScans_.resize(16);
148  }
149  else
150  {
151  accumulatedScans_.resize(1);
152  }
153  buildTimings(false);
154  start();
155  UDEBUG("Lidar capture started");
156  return true;
157 }
158 
162 void LidarVLP16::buildTimings(bool dualMode)
163 {
164  // vlp16
165  // timing table calculation, from velodyne user manual
166  timingOffsets_.resize(12);
167  for (size_t i=0; i < timingOffsets_.size(); ++i){
168  timingOffsets_[i].resize(32);
169  }
170  // constants
171  double full_firing_cycle = 55.296 * 1e-6; // seconds
172  double single_firing = 2.304 * 1e-6; // seconds
173  double dataBlockIndex, dataPointIndex;
174  // compute timing offsets
175  for (size_t x = 0; x < timingOffsets_.size(); ++x){
176  for (size_t y = 0; y < timingOffsets_[x].size(); ++y){
177  if (dualMode){
178  dataBlockIndex = (x - (x % 2)) + (y / 16);
179  }
180  else{
181  dataBlockIndex = (x * 2) + (y / 16);
182  }
183  dataPointIndex = y % 16;
184  //timing_offsets[block][firing]
185  timingOffsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
186  }
187  }
188  timingOffsetsDualMode_ = dualMode;
189 }
190 
191 void LidarVLP16::toPointClouds (HDLDataPacket *dataPacket)
192 {
193  if (sizeof(HDLLaserReturn) != 3)
194  return;
195 
196  double receivedHostTime = UTimer::now();
197  double packetStamp = rosTimeFromGpsTimestamp(dataPacket->gpsTimestamp);
198  if(startSweepTime_==0)
199  {
200  startSweepTime_ = packetStamp;
201  startSweepTimeHost_ = receivedHostTime;
202  }
203 
204  bool dualMode = dataPacket->mode == VLP_DUAL_MODE;
205  if(timingOffsets_.empty() || timingOffsetsDualMode_ != dualMode)
206  {
207  // reset everything
208  timingOffsets_.clear();
209  buildTimings(dualMode);
210  startSweepTime_ = packetStamp;
211  startSweepTimeHost_ = receivedHostTime;
212  for(size_t i=0; i<accumulatedScans_.size(); ++i)
213  {
214  accumulatedScans_[i].clear();
215  }
216  }
217 
218  double interpolated_azimuth_delta;
219  std::uint8_t index = 1;
220  if (dualMode)
221  {
222  index = 2;
223  }
224  if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition)
225  {
226  interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
227  }
228  else
229  {
230  interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0;
231  }
232 
233  for (std::uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i)
234  {
235  HDLFiringData firing_data = dataPacket->firingData[i];
236 
237  for (std::uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++)
238  {
239  double current_azimuth = firing_data.rotationalPosition;
240  if (j >= VLP_MAX_NUM_LASERS)
241  {
242  current_azimuth += interpolated_azimuth_delta;
243  }
244  if (current_azimuth > 36000)
245  {
246  current_azimuth -= 36000;
247  }
248 
249  double t = 0;
250  if (timingOffsets_.size())
251  t = timingOffsets_[i][j];
252 
253  if (current_azimuth < HDLGrabber::last_azimuth_)
254  {
255  if (!accumulatedScans_[0].empty())
256  {
258  bool notify = lastScan_.laserScanRaw().empty();
259  if(stampLast_)
260  {
261  double lastStamp = startSweepTime_ + accumulatedScans_[accumulatedScans_.size()-1].back().t;
262  double diff = lastStamp - startSweepTime_;
264  for(size_t r=0; r<accumulatedScans_.size(); ++r)
265  {
266  for(size_t k=0; k<accumulatedScans_[r].size(); ++k)
267  {
268  accumulatedScans_[r][k].t -= diff;
269  }
270  }
271  }
272  else
273  {
275  }
276  if(accumulatedScans_.size() > 1)
277  {
278  cv::Mat organizedScan = cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone();
279  for(size_t k=1; k<accumulatedScans_.size(); ++k)
280  {
281  UASSERT((int)accumulatedScans_[k].size() == organizedScan.cols);
282  organizedScan.push_back(cv::Mat(1, accumulatedScans_[k].size(), CV_32FC(5), accumulatedScans_[k].data()).clone());
283  }
285  }
286  else
287  {
288  lastScan_.setLaserScan(LaserScan(cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone(), 0, 0, LaserScan::kXYZIT, getLocalTransform()));
289  }
290  if(notify)
291  {
293  }
294 
295  startSweepTime_ = packetStamp + t;
296  startSweepTimeHost_ = receivedHostTime + t;
297  }
298  for(size_t k=0; k<accumulatedScans_.size(); ++k)
299  {
300  accumulatedScans_[k].clear();
301  }
302  }
303 
304  double timeSinceStartOfThisScan = packetStamp + t - startSweepTime_;
305 
306  pcl::PointXYZI xyzi;
307  HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
308  PointXYZIT xyzit;
309  xyzit.x = xyzi.y;
310  xyzit.y = -xyzi.x;
311  xyzit.z = xyzi.z;
312  xyzit.i = xyzi.intensity;
313  xyzit.t = timeSinceStartOfThisScan;
314 
315  if(accumulatedScans_.size()>1)
316  {
317  accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back(xyzit);
318  }
319  else if (! (std::isnan (xyzit.x) || std::isnan (xyzit.y) || std::isnan (xyzit.z)))
320  {
321  accumulatedScans_[0].push_back (xyzit);
322  }
323  last_azimuth_ = current_azimuth;
324 
325  if (dualMode)
326  {
327  pcl::PointXYZI dual_xyzi;
328  HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
329 
330  if(accumulatedScans_.size()>1)
331  {
332  xyzit.x = dual_xyzi.y;
333  xyzit.y = -dual_xyzi.x;
334  xyzit.z = dual_xyzi.z;
335  xyzit.i = dual_xyzi.intensity;
336  xyzit.t = timeSinceStartOfThisScan;
337  accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back (xyzit);
338  }
339  else if ((dual_xyzi.x != xyzi.x || dual_xyzi.y != xyzi.y || dual_xyzi.z != xyzi.z)
340  && ! (std::isnan (dual_xyzi.x) || std::isnan (dual_xyzi.y) || std::isnan (dual_xyzi.z)))
341  {
342  xyzit.x = dual_xyzi.y;
343  xyzit.y = -dual_xyzi.x;
344  xyzit.z = dual_xyzi.z;
345  xyzit.i = dual_xyzi.intensity;
346  xyzit.t = timeSinceStartOfThisScan;
347  accumulatedScans_[0].push_back (xyzit);
348  }
349  }
350  }
351  if (dualMode)
352  {
353  i++;
354  }
355  }
356 }
357 
359 {
361  if(scanReady_.acquire(1, 5000))
362  {
364  if(!lastScan_.laserScanRaw().empty())
365  {
366  data = lastScan_;
367  lastScan_ = SensorData();
368  }
369  }
370  else
371  {
372  UWARN("Did not receive any scans for the past 5 seconds.");
373  }
374  return data;
375 }
376 
377 
378 } /* namespace rtabmap */
rtabmap::LidarVLP16::scanReady_
USemaphore scanReady_
Definition: LidarVLP16.h:88
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
pcl
Definition: CameraOpenni.h:47
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
rtabmap::LidarVLP16::buildTimings
void buildTimings(bool dualMode)
Definition: LidarVLP16.cpp:162
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::LidarVLP16::organized_
bool organized_
Definition: LidarVLP16.h:83
LidarVLP16.h
rtabmap::LidarVLP16::lastScanMutex_
UMutex lastScanMutex_
Definition: LidarVLP16.h:89
uint32_t
::uint32_t uint32_t
size
Index size
glm::uint16_t
detail::uint16 uint16_t
Definition: fwd.hpp:912
rtabmap::LaserScan::kXYZIT
@ kXYZIT
Definition: LaserScan.h:51
rtabmap::PointXYZIT::t
float t
Definition: LidarVLP16.h:42
rtabmap::LidarVLP16::captureData
virtual SensorData captureData(SensorCaptureInfo *info=0) override
Definition: LidarVLP16.cpp:358
rtabmap::LidarVLP16::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="") override
Definition: LidarVLP16.cpp:132
rtabmap::LidarVLP16::~LidarVLP16
virtual ~LidarVLP16()
Definition: LidarVLP16.cpp:119
y
Matrix3f y
rtabmap::SensorCapture::getLocalTransform
const Transform & getLocalTransform() const
Definition: SensorCapture.h:62
rtabmap::LaserScan
Definition: LaserScan.h:37
UTimer.h
rtabmap::LidarVLP16::LidarVLP16
LidarVLP16(const std::string &pcapFile, bool organized=false, bool stampLast=true, float frameRate=0.0f, Transform localTransform=Transform::getIdentity())
Definition: LidarVLP16.cpp:82
glm::isnan
GLM_FUNC_DECL genType::bool_type isnan(genType const &x)
rtabmap::LidarVLP16::useHostTime_
bool useHostTime_
Definition: LidarVLP16.h:84
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::LidarVLP16::setOrganized
void setOrganized(bool enable)
Definition: LidarVLP16.cpp:127
rtabmap::PointXYZIT::i
float i
Definition: LidarVLP16.h:41
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
data
int data[]
UScopeMutex
Definition: UMutex.h:157
j
std::ptrdiff_t j
rtabmap::LidarVLP16::stampLast_
bool stampLast_
Definition: LidarVLP16.h:85
rtabmap::LidarVLP16::lastScan_
SensorData lastScan_
Definition: LidarVLP16.h:86
rtabmap::LidarVLP16::timingOffsetsDualMode_
bool timingOffsetsDualMode_
Definition: LidarVLP16.h:80
rtabmap::LidarVLP16::startSweepTime_
double startSweepTime_
Definition: LidarVLP16.h:81
UASSERT
#define UASSERT(condition)
x
x
rtabmap::PointXYZIT::z
float z
Definition: LidarVLP16.h:40
rtabmap::resolveHourAmbiguity
double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp)
Function used to check that hour assigned to timestamp in conversion is correct. Velodyne only return...
Definition: LidarVLP16.cpp:53
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UWARN
#define UWARN(...)
rtabmap::PointXYZIT
Definition: LidarVLP16.h:37
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
rtabmap::Transform
Definition: Transform.h:41
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
empty
UThread.h
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
diff
diff
UDEBUG
#define UDEBUG(...)
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::LidarVLP16::timingOffsets_
std::vector< std::vector< float > > timingOffsets_
Definition: LidarVLP16.h:79
rtabmap::LidarVLP16::accumulatedScans_
std::vector< std::vector< PointXYZIT > > accumulatedScans_
Definition: LidarVLP16.h:87
rtabmap::Lidar
Definition: Lidar.h:40
rtabmap::LidarVLP16::startSweepTimeHost_
double startSweepTimeHost_
Definition: LidarVLP16.h:82
false
#define false
Definition: ConvertUTF.c:56
rtabmap::LidarVLP16::toPointClouds
virtual void toPointClouds(HDLDataPacket *dataPacket) override
Definition: LidarVLP16.cpp:191
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::PointXYZIT::x
float x
Definition: LidarVLP16.h:38
rtabmap::SensorData::setStamp
void setStamp(double stamp)
Definition: SensorData.h:177
i
int i
rtabmap::PointXYZIT::y
float y
Definition: LidarVLP16.h:39
rtabmap::rosTimeFromGpsTimestamp
double rosTimeFromGpsTimestamp(const uint32_t data)
Definition: LidarVLP16.cpp:71


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11