Debian Codegraph @

libs/hwdrivers/src/CVelodyneScanner.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/comms/net_utils.h>
13 #include <mrpt/core/reverse_bytes.h>
14 #include <mrpt/hwdrivers/CGPSInterface.h>
15 #include <mrpt/hwdrivers/CVelodyneScanner.h>
16 #include <mrpt/serialization/CArchive.h>
17 #include <mrpt/system/datetime.h> // timeDifference
18 #include <mrpt/system/filesystem.h>
19
20 #include <chrono>
21 #include <iostream>
22 #include <thread>
23
24 // socket's hdrs:
25 #ifdef _WIN32
26 #define _WINSOCK_DEPRECATED_NO_WARNINGS
27 #if defined(_WIN32_WINNT) && (_WIN32_WINNT < 0x600)
28 #undef _WIN32_WINNT
29 #define _WIN32_WINNT 0x600 // Minimum: Windows Vista (required to pollfd)
30 #endif
31
32 #include <winsock2.h>
33 using socklen_t = int;
34
35 #if defined(_MSC_VER)
36 #pragma comment(lib, "WS2_32.LIB")
37 #endif
38
39 #else
40 #define INVALID_SOCKET (-1)
41 #include <arpa/inet.h>
42 #include <fcntl.h>
43 #include <netdb.h>
44 #include <netinet/in.h>
45 #include <netinet/tcp.h>
46 #include <poll.h>
47 #include <sys/ioctl.h>
48 #include <sys/socket.h>
49 #include <sys/time.h> // gettimeofday()
50 #include <sys/types.h>
51 #include <unistd.h>
52
53 #include <cerrno>
54 #endif
55
56 #if MRPT_HAS_LIBPCAP
57 #include <pcap.h>
58 #if !defined(PCAP_NETMASK_UNKNOWN) // for older pcap versions
59 #define PCAP_NETMASK_UNKNOWN 0xffffffff
60 #endif
61 #endif
62
63 using namespace mrpt;
64 using namespace mrpt::hwdrivers;
65 using namespace mrpt::obs;
66 using namespace std;
67
68 IMPLEMENTS_GENERIC_SENSOR(CVelodyneScanner, mrpt::hwdrivers)
69
70 short int CVelodyneScanner::VELODYNE_DATA_UDP_PORT = 2368;
71 short int CVelodyneScanner::VELODYNE_POSITION_UDP_PORT = 8308;
72
73 const CVelodyneScanner::model_properties_list_t& CVelodyneScanner::TModelPropertiesFactory::get()
74 {
75 static CVelodyneScanner::model_properties_list_t modelProperties;
76 static bool init = false;
77 if (!init)
78 {
79 init = true;
80 modelProperties[CVelodyneScanner::VLP16].maxRange = 120.0;
81 modelProperties[CVelodyneScanner::HDL32].maxRange = 70.0;
82 modelProperties[CVelodyneScanner::HDL64].maxRange = 120.0;
83 }
84 return modelProperties;
85 }
86 std::string CVelodyneScanner::TModelPropertiesFactory::getListKnownModels()
87 {
88 const CVelodyneScanner::model_properties_list_t& lst =
89 CVelodyneScanner::TModelPropertiesFactory::get();
90 std::string s;
91 for (auto it : lst)
92 s += mrpt::format(
93 "`%s`,",
94 mrpt::typemeta::TEnumType<CVelodyneScanner::model_t>::value2name(it.first).c_str());
95 return s;
96 }
97
98 CVelodyneScanner::CVelodyneScanner() :
99 m_device_ip(""),
100
101 m_last_pos_packet_timestamp(INVALID_TIMESTAMP),
102
103 m_hDataSock(INVALID_SOCKET),
104 m_hPositionSock(INVALID_SOCKET),
105 m_last_gps_rmc_age(INVALID_TIMESTAMP)
106
107 {
108 m_sensorLabel = "Velodyne";
109
110 #if MRPT_HAS_LIBPCAP
111 m_pcap_bpf_program = new bpf_program[1];
112 #endif
113
114 #ifdef _WIN32
115 // Init the WinSock Library:
116 WORD wVersionRequested = MAKEWORD(2, 0);
117 WSADATA wsaData;
118 if (WSAStartup(wVersionRequested, &wsaData)) THROW_EXCEPTION("Error calling WSAStartup");
119 #endif
120 }
121
122 CVelodyneScanner::~CVelodyneScanner()
123 {
124 this->close();
125 #ifdef _WIN32
126 WSACleanup();
127 #endif
128
129 #if MRPT_HAS_LIBPCAP
130 delete[] reinterpret_cast<bpf_program*>(m_pcap_bpf_program);
131 m_pcap_bpf_program = nullptr;
132 #endif
133 }
134
135 bool CVelodyneScanner::loadCalibrationFile(const std::string& velodyne_xml_calib_file_path)
136 {
137 return m_velodyne_calib.loadFromXMLFile(velodyne_xml_calib_file_path);
138 }
139
140 void CVelodyneScanner::loadConfig_sensorSpecific(
141 const mrpt::config::CConfigFileBase& cfg, const std::string& sect)
142 {
143 MRPT_START
144
145 cfg.read_enum<CVelodyneScanner::model_t>(sect, "model", m_model);
146 MRPT_LOAD_HERE_CONFIG_VAR(device_ip, string, m_device_ip, cfg, sect);
147 MRPT_LOAD_HERE_CONFIG_VAR(pcap_input, string, m_pcap_input_file, cfg, sect);
148 MRPT_LOAD_HERE_CONFIG_VAR(pcap_output, string, m_pcap_output_file, cfg, sect);
149 MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_once, bool, m_pcap_read_once, cfg, sect);
150 MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_fast, bool, m_pcap_read_fast, cfg, sect);
151 MRPT_LOAD_HERE_CONFIG_VAR(
152 pcap_read_full_scan_delay_ms, double, m_pcap_read_full_scan_delay_ms, cfg, sect);
153 MRPT_LOAD_HERE_CONFIG_VAR(pcap_repeat_delay, double, m_pcap_repeat_delay, cfg, sect);
154 MRPT_LOAD_HERE_CONFIG_VAR(
155 pos_packets_timing_timeout, double, m_pos_packets_timing_timeout, cfg, sect);
156 MRPT_LOAD_HERE_CONFIG_VAR(pos_packets_min_period, double, m_pos_packets_min_period, cfg, sect);
157
158 using mrpt::DEG2RAD;
159 m_sensorPose = mrpt::poses::CPose3D(
160 cfg.read_float(sect, "pose_x", 0), cfg.read_float(sect, "pose_y", 0),
161 cfg.read_float(sect, "pose_z", 0), DEG2RAD(cfg.read_float(sect, "pose_yaw", 0)),
162 DEG2RAD(cfg.read_float(sect, "pose_pitch", 0)),
163 DEG2RAD(cfg.read_float(sect, "pose_roll", 0)));
164
165 std::string calibration_file;
166 MRPT_LOAD_CONFIG_VAR(calibration_file, string, cfg, sect);
167 if (!calibration_file.empty())
168 {
169 bool calibLoadOk = loadCalibrationFile(calibration_file);
170 ASSERT_(calibLoadOk);
171 }
172
173 // Check validity:
174 const model_properties_list_t& lstModels = TModelPropertiesFactory::get();
175 if (lstModels.find(m_model) == lstModels.end())
176 {
177 THROW_EXCEPTION(mrpt::format(
178 "Unrecognized `model` parameter: `%u` . Known values are: %s",
179 static_cast<unsigned int>(m_model), TModelPropertiesFactory::getListKnownModels().c_str()));
180 }
181
182 // Optional HTTP-based settings:
183 MRPT_LOAD_HERE_CONFIG_VAR(rpm, int, m_lidar_rpm, cfg, sect);
184 m_lidar_return = cfg.read_enum<return_type_t>(sect, "return_type", m_lidar_return);
185
186 MRPT_END
187 }
188
189 bool CVelodyneScanner::getNextObservation(
190 mrpt::obs::CObservationVelodyneScan::Ptr& outScan, mrpt::obs::CObservationGPS::Ptr& outGPS)
191 {
192 try
193 {
194 ASSERTMSG_(m_initialized, "initialize() has not been called yet!");
195
196 // Init with empty smart pointers:
197 outScan = mrpt::obs::CObservationVelodyneScan::Ptr();
198 outGPS = mrpt::obs::CObservationGPS::Ptr();
199
200 // Try to get data & pos packets:
201 mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket rx_pkt;
202 mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket rx_pos_pkt;
203 mrpt::system::TTimeStamp data_pkt_timestamp, pos_pkt_timestamp;
204 bool rx_all_ok =
205 this->receivePackets(data_pkt_timestamp, rx_pkt, pos_pkt_timestamp, rx_pos_pkt);
206
207 if (!rx_all_ok)
208 {
209 // PCAP EOF:
210 return false;
211 }
212
213 if (pos_pkt_timestamp != INVALID_TIMESTAMP)
214 {
215 mrpt::obs::CObservationGPS::Ptr gps_obs = mrpt::obs::CObservationGPS::Create();
216 gps_obs->sensorLabel = this->m_sensorLabel + std::string("_GPS");
217 gps_obs->sensorPose = m_sensorPose;
218
219 gps_obs->originalReceivedTimestamp = pos_pkt_timestamp;
220
221 bool parsed_ok = CGPSInterface::parse_NMEA(std::string(rx_pos_pkt.NMEA_GPRMC), *gps_obs);
222 const mrpt::obs::gnss::Message_NMEA_RMC* msg_rmc =
223 gps_obs->getMsgByClassPtr<mrpt::obs::gnss::Message_NMEA_RMC>();
224 if (!parsed_ok || !msg_rmc || msg_rmc->fields.validity_char != 'A')
225 {
226 gps_obs->has_satellite_timestamp = false;
227 gps_obs->timestamp = pos_pkt_timestamp;
228 }
229 else
230 {
231 // We have live GPS signal and a recent RMC frame:
232 m_last_gps_rmc_age = pos_pkt_timestamp;
233 m_last_gps_rmc = *msg_rmc;
234 }
235 outGPS = gps_obs; // save in output object
236 }
237
238 if (data_pkt_timestamp != INVALID_TIMESTAMP)
239 {
240 m_state = ssWorking;
241
242 // Break into a new observation object when the azimuth passes
243 // 360->0 deg:
244 const uint16_t rx_pkt_start_angle = rx_pkt.blocks[0].rotation();
245 // const uint16_t rx_pkt_end_angle =
246 // rx_pkt.blocks[CObservationVelodyneScan::BLOCKS_PER_PACKET-1].rotation;
247
248 // Return the observation as done when a complete 360 deg scan is
249 // ready:
250 if (m_rx_scan && !m_rx_scan->scan_packets.empty())
251 {
252 if ((rx_pkt_start_angle < m_rx_scan->scan_packets.rbegin()->blocks[0].rotation()) ||
253 !m_return_frames)
254 {
255 outScan = m_rx_scan;
256 m_rx_scan.reset();
257
258 if (m_pcap)
259 {
260 // Keep the reader from blowing through the file.
261 if (!m_pcap_read_fast)
262 std::this_thread::sleep_for(
263 std::chrono::duration<double, std::milli>(m_pcap_read_full_scan_delay_ms));
264 }
265 }
266 }
267
268 // Create smart ptr to new in-progress observation:
269 if (!m_rx_scan)
270 {
271 m_rx_scan = mrpt::obs::CObservationVelodyneScan::Create();
272 m_rx_scan->sensorLabel = this->m_sensorLabel + std::string("_SCAN");
273 m_rx_scan->sensorPose = m_sensorPose;
274 m_rx_scan->calibration = m_velodyne_calib; // Embed a copy of the calibration info
275
276 {
277 const model_properties_list_t& lstModels = TModelPropertiesFactory::get();
278 auto it = lstModels.find(this->m_model);
279 if (it != lstModels.end())
280 { // Model params:
281 m_rx_scan->maxRange = it->second.maxRange;
282 }
283 else // default params:
284 {
285 m_rx_scan->maxRange = 120.0;
286 }
287 }
288 }
289
290 // For the first packet, set timestamp:
291 if (m_rx_scan->scan_packets.empty())
292 {
293 m_rx_scan->originalReceivedTimestamp = data_pkt_timestamp;
294 // Using GPS, if available:
295 if (m_last_gps_rmc.fields.validity_char == 'A' &&
296 mrpt::system::timeDifference(m_last_gps_rmc_age, data_pkt_timestamp) <
297 m_pos_packets_timing_timeout)
298 {
299 // Each Velodyne data packet has a timestamp field,
300 // with the number of us since the top of the current HOUR:
301 // take the date and time from the GPS, then modify minutes
302 // and seconds from data pkt:
303 const mrpt::system::TTimeStamp gps_tim =
304 m_last_gps_rmc.fields.UTCTime.getAsTimestamp(m_last_gps_rmc.getDateAsTimestamp());
305
306 mrpt::system::TTimeParts tim_parts;
307 mrpt::system::timestampToParts(gps_tim, tim_parts);
308 tim_parts.minute = rx_pkt.gps_timestamp() /*us from top of hour*/ / 60000000ul;
309 tim_parts.second = (rx_pkt.gps_timestamp() /*us from top of hour*/ % 60000000ul) * 1e-6;
310
311 const mrpt::system::TTimeStamp data_pkt_tim =
312 mrpt::system::buildTimestampFromParts(tim_parts);
313
314 m_rx_scan->timestamp = data_pkt_tim;
315 m_rx_scan->has_satellite_timestamp = true;
316 }
317 else
318 {
319 m_rx_scan->has_satellite_timestamp = false;
320 m_rx_scan->timestamp = data_pkt_timestamp;
321 }
322 }
323
324 // Accumulate pkts in the observation object:
325 m_rx_scan->scan_packets.push_back(rx_pkt);
326 }
327
328 return true;
329 }
330 catch (exception& e)
331 {
332 cerr << "[CVelodyneScanner::getObservation] Returning false due to "
333 "exception: "
334 << endl;
335 cerr << e.what() << endl;
336 return false;
337 }
338 }
339
340 void CVelodyneScanner::doProcess()
341 {
342 CObservationVelodyneScan::Ptr obs;
343 CObservationGPS::Ptr obs_gps;
344
345 if (getNextObservation(obs, obs_gps))
346 {
347 m_state = ssWorking;
348 if (obs) appendObservation(obs);
349 if (obs_gps) appendObservation(obs_gps);
350 }
351 else
352 {
353 m_state = ssError;
354 cerr << "ERROR receiving data from Velodyne devic!" << endl;
355 }
356 }
357
358 /** Tries to initialize the sensor, after setting all the parameters with a call
359 * to loadConfig.
360 * \exception This method must throw an exception with a descriptive message if
361 * some critical error is found. */
362 void CVelodyneScanner::initialize()
363 {
364 this->close();
365
366 // (0) Preparation:
367 // --------------------------------
368 // Make sure we have calibration data:
369 if (m_velodyne_calib.empty())
370 {
371 // Try to load default data:
372 m_velodyne_calib = VelodyneCalibration::LoadDefaultCalibration(
373 mrpt::typemeta::TEnumType<CVelodyneScanner::model_t>::value2name(m_model));
374 if (m_velodyne_calib.empty())
375 THROW_EXCEPTION(
376 "Could not find default calibration data for the given LIDAR "
377 "`model` name. Please, specify a valid `model` or load a valid "
378 "XML configuration file first.");
379 }
380
381 // online vs off line operation??
382 // -------------------------------
383 if (m_pcap_input_file.empty())
384 { // Online
385
386 if (m_lidar_rpm > 0)
387 {
388 if (!setLidarRPM(m_lidar_rpm)) THROW_EXCEPTION("Error in setLidarRPM();");
389 }
390 if (m_lidar_return != UNCHANGED)
391 {
392 if (!setLidarReturnType(m_lidar_return)) THROW_EXCEPTION("Error in setLidarReturnType();");
393 }
394
395 // (1) Create LIDAR DATA socket
396 // --------------------------------
397 if (INVALID_SOCKET == (m_hDataSock = socket(PF_INET, SOCK_DGRAM, 0)))
398 THROW_EXCEPTION(format(
399 "Error creating UDP socket:\n%s", mrpt::comms::net::getLastSocketErrorStr().c_str()));
400
401 struct sockaddr_in bindAddr;
402 memset(&bindAddr, 0, sizeof(bindAddr));
403 bindAddr.sin_family = AF_INET;
404 bindAddr.sin_port = htons(VELODYNE_DATA_UDP_PORT);
405 bindAddr.sin_addr.s_addr = INADDR_ANY;
406
407 if (int(INVALID_SOCKET) == ::bind(m_hDataSock, (struct sockaddr*)(&bindAddr), sizeof(sockaddr)))
408 THROW_EXCEPTION(mrpt::comms::net::getLastSocketErrorStr());
409
410 #ifdef _WIN32
411 unsigned long non_block_mode = 1;
412 if (ioctlsocket(m_hDataSock, FIONBIO, &non_block_mode))
413 THROW_EXCEPTION("Error entering non-blocking mode with ioctlsocket().");
414 #else
415 int oldflags = fcntl(m_hDataSock, F_GETFL, 0);
416 if (oldflags == -1) THROW_EXCEPTION("Error retrieving fcntl() of socket.");
417 oldflags |= O_NONBLOCK | FASYNC;
418 if (-1 == fcntl(m_hDataSock, F_SETFL, oldflags))
419 THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
420 #endif
421
422 // (2) Create LIDAR POSITION socket
423 // --------------------------------
424 if (INVALID_SOCKET == (m_hPositionSock = socket(PF_INET, SOCK_DGRAM, 0)))
425 THROW_EXCEPTION(format(
426 "Error creating UDP socket:\n%s", mrpt::comms::net::getLastSocketErrorStr().c_str()));
427
428 bindAddr.sin_port = htons(VELODYNE_POSITION_UDP_PORT);
429
430 if (int(INVALID_SOCKET) ==
431 ::bind(m_hPositionSock, (struct sockaddr*)(&bindAddr), sizeof(sockaddr)))
432 THROW_EXCEPTION(mrpt::comms::net::getLastSocketErrorStr());
433
434 #ifdef _WIN32
435 if (ioctlsocket(m_hPositionSock, FIONBIO, &non_block_mode))
436 THROW_EXCEPTION("Error entering non-blocking mode with ioctlsocket().");
437 #else
438 oldflags = fcntl(m_hPositionSock, F_GETFL, 0);
439 if (oldflags == -1) THROW_EXCEPTION("Error retrieving fcntl() of socket.");
440 oldflags |= O_NONBLOCK | FASYNC;
441 if (-1 == fcntl(m_hPositionSock, F_SETFL, oldflags))
442 THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
443 #endif
444 }
445 else
446 { // Offline:
447 #if MRPT_HAS_LIBPCAP
448 char errbuf[PCAP_ERRBUF_SIZE];
449
450 if (m_pcap_verbose)
451 printf("\n[CVelodyneScanner] Opening PCAP file \"%s\"\n", m_pcap_input_file.c_str());
452 if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) == nullptr)
453 {
454 THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
455 }
456
457 // Build PCAP filter:
458 {
459 std::string filter_str = mrpt::format(
460 "(udp dst port %d || udp dst port %d)",
461 static_cast<int>(CVelodyneScanner::VELODYNE_DATA_UDP_PORT),
462 static_cast<int>(CVelodyneScanner::VELODYNE_POSITION_UDP_PORT));
463 if (!m_device_ip.empty()) filter_str += "&& src host " + m_device_ip;
464
465 static std::string sMsgError = "[CVelodyneScanner] Error calling pcap_compile: "; // This is
466 // to avoid
467 // the
468 // ill-formed
469 // signature
470 // of
471 // pcap_error()
472 // accepting
473 // "char*",
474 // not
475 // "const
476 // char*"...
477 // sigh
478 if (pcap_compile(
479 reinterpret_cast<pcap_t*>(m_pcap), reinterpret_cast<bpf_program*>(m_pcap_bpf_program),
480 filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN) < 0)
481 pcap_perror(reinterpret_cast<pcap_t*>(m_pcap), &sMsgError[0]);
482 }
483
484 m_pcap_file_empty = true;
485 m_pcap_read_count = 0;
486
487 #else
488 THROW_EXCEPTION(
489 "Velodyne: Reading from PCAP requires building MRPT with libpcap "
490 "support!");
491 #endif
492 }
493
494 m_last_pos_packet_timestamp = INVALID_TIMESTAMP;
495 m_last_gps_rmc_age = INVALID_TIMESTAMP;
496 m_state = ssInitializing;
497
498 m_initialized = true;
499 }
500
501 void CVelodyneScanner::close()
502 {
503 if (m_hDataSock != INVALID_SOCKET)
504 {
505 shutdown(m_hDataSock, 2); // SD_BOTH );
506 #ifdef _WIN32
507 closesocket(m_hDataSock);
508 #else
509 ::close(m_hDataSock);
510 #endif
511 m_hDataSock = INVALID_SOCKET;
512 }
513
514 if (m_hPositionSock != INVALID_SOCKET)
515 {
516 shutdown(m_hPositionSock, 2); // SD_BOTH );
517 #ifdef _WIN32
518 closesocket(m_hPositionSock);
519 #else
520 ::close(m_hPositionSock);
521 #endif
522 m_hPositionSock = INVALID_SOCKET;
523 }
524 #if MRPT_HAS_LIBPCAP
525 if (m_pcap)
526 {
527 pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
528 m_pcap = nullptr;
529 }
530 if (m_pcap_dumper)
531 {
532 pcap_dump_close(reinterpret_cast<pcap_dumper_t*>(m_pcap_dumper));
533 m_pcap_dumper = nullptr;
534 }
535 if (m_pcap_out)
536 {
537 pcap_close(reinterpret_cast<pcap_t*>(m_pcap_out));
538 m_pcap_out = nullptr;
539 }
540 #endif
541 m_initialized = false;
542 }
543
544 // Fixed Ethernet headers for PCAP capture --------
545 #if MRPT_HAS_LIBPCAP
546 const uint16_t LidarPacketHeader[21] = {
547 0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204, 0x0000, 0x0040, 0x11ff,
548 0xaab4, 0xa8c0, 0xc801,
549 0xffff, // checksum 0xa9b4 //source ip
550 // 0xa8c0, 0xc801 is
551 // 192.168.1.200
552 0xffff, 0x4009, 0x4009, 0xbe04, 0x0000};
553 const uint16_t PositionPacketHeader[21] = {
554 0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204, 0x0000, 0x0040, 0x11ff,
555 0xaab4, 0xa8c0, 0xc801, 0xffff, // checksum 0xa9b4
556 // //source ip
557 // 0xa8c0, 0xc801
558 // is 192.168.1.200
559 0xffff, 0x7420, 0x7420, 0x0802, 0x0000};
560 #endif
561
562 #if defined(_WIN32) && MRPT_HAS_LIBPCAP
563 int gettimeofday(struct timeval* tp, void*)
564 {
565 FILETIME ft;
566 ::GetSystemTimeAsFileTime(&ft);
567 long long t = (static_cast<long long>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
568 t -= 116444736000000000LL;
569 t /= 10; // microseconds
570 tp->tv_sec = static_cast<long>(t / 1000000UL);
571 tp->tv_usec = static_cast<long>(t % 1000000UL);
572 return 0;
573 }
574 #endif
575
576 bool CVelodyneScanner::receivePackets(
577 mrpt::system::TTimeStamp& data_pkt_timestamp,
578 mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket& out_data_pkt,
579 mrpt::system::TTimeStamp& pos_pkt_timestamp,
580 mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket& out_pos_pkt)
581 {
582 static_assert(
583 sizeof(mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket) ==
584 CObservationVelodyneScan::POS_PACKET_SIZE,
585 "Velodyne pos packet: wrong size!");
586 static_assert(
587 sizeof(mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket) ==
588 CObservationVelodyneScan::PACKET_SIZE,
589 "Velodyne raw packet: wrong size!");
590
591 bool ret = true; // all ok
592 if (m_pcap)
593 {
594 ret = internal_read_PCAP_packet(
595 data_pkt_timestamp, (uint8_t*)&out_data_pkt, pos_pkt_timestamp, (uint8_t*)&out_pos_pkt);
596 }
597 else
598 {
599 data_pkt_timestamp = internal_receive_UDP_packet(
600 m_hDataSock, (uint8_t*)&out_data_pkt, CObservationVelodyneScan::PACKET_SIZE, m_device_ip);
601 pos_pkt_timestamp = internal_receive_UDP_packet(
602 m_hPositionSock, (uint8_t*)&out_pos_pkt, CObservationVelodyneScan::POS_PACKET_SIZE,
603 m_device_ip);
604 }
605
606 // Optional PCAP dump:
607 #if MRPT_HAS_LIBPCAP
608 // Save to PCAP file?
609 if (!m_pcap_output_file.empty() && !m_pcap_out) // 1st time: create output file
610 {
611 #if MRPT_HAS_LIBPCAP
612 char errbuf[PCAP_ERRBUF_SIZE];
613
614 mrpt::system::TTimeParts parts;
615 mrpt::system::timestampToParts(mrpt::Clock::now(), parts, true);
616 string sFilePostfix = "_";
617 sFilePostfix += mrpt::format(
618 "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year, (unsigned int)parts.month,
619 (unsigned int)parts.day, (unsigned int)parts.hour, (unsigned int)parts.minute,
620 (unsigned int)parts.second);
621 const string sFileName = m_pcap_output_file +
622 mrpt::system::fileNameStripInvalidChars(sFilePostfix) +
623 string(".pcap");
624
625 m_pcap_out = pcap_open_dead(DLT_EN10MB, 65535);
626 ASSERTMSG_(m_pcap_out != nullptr, "Error creating PCAP live capture handle");
627
628 printf("\n[CVelodyneScanner] Writing to PCAP file \"%s\"\n", sFileName.c_str());
629 if ((m_pcap_dumper =
630 pcap_dump_open(reinterpret_cast<pcap_t*>(m_pcap_out), sFileName.c_str())) == nullptr)
631 {
632 THROW_EXCEPTION_FMT("Error creating PCAP live dumper: '%s'", errbuf);
633 }
634 #else
635 THROW_EXCEPTION(
636 "Velodyne: Writing PCAP files requires building MRPT with libpcap "
637 "support!");
638 #endif
639 }
640
641 if (m_pcap_out && m_pcap_dumper &&
642 (data_pkt_timestamp != INVALID_TIMESTAMP || pos_pkt_timestamp != INVALID_TIMESTAMP))
643 {
644 struct pcap_pkthdr header;
645 struct timeval currentTime;
646 gettimeofday(&currentTime, nullptr);
647 std::vector<unsigned char> packetBuffer;
648
649 // Data pkt:
650 if (data_pkt_timestamp != INVALID_TIMESTAMP)
651 {
652 header.caplen = header.len = CObservationVelodyneScan::PACKET_SIZE + 42;
653 packetBuffer.resize(header.caplen);
654 header.ts = currentTime;
655
656 memcpy(&(packetBuffer[0]), LidarPacketHeader, 42);
657 memcpy(
658 &(packetBuffer[0]) + 42, (uint8_t*)&out_data_pkt, CObservationVelodyneScan::PACKET_SIZE);
659 pcap_dump((u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
660 }
661 // Pos pkt:
662 if (pos_pkt_timestamp != INVALID_TIMESTAMP)
663 {
664 header.caplen = header.len = CObservationVelodyneScan::POS_PACKET_SIZE + 42;
665 packetBuffer.resize(header.caplen);
666 header.ts = currentTime;
667
668 memcpy(&(packetBuffer[0]), PositionPacketHeader, 42);
669 memcpy(
670 &(packetBuffer[0]) + 42, (uint8_t*)&out_pos_pkt,
671 CObservationVelodyneScan::POS_PACKET_SIZE);
672 pcap_dump((u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
673 }
674 }
675 #endif
676
677 // Convert from Velodyne's standard little-endian ordering to host byte
678 // ordering (done AFTER saving the pckg as is to pcap above).
679 // 2019-NOV: Removed. Don't do this here, since it's problematic to
680 // "remember" whether internal data is already in big or little endian.
681
682 // Position packet decimation:
683 if (pos_pkt_timestamp != INVALID_TIMESTAMP)
684 {
685 if ((m_last_pos_packet_timestamp != INVALID_TIMESTAMP) &&
686 mrpt::system::timeDifference(m_last_pos_packet_timestamp, pos_pkt_timestamp) <
687 m_pos_packets_min_period)
688 {
689 // Ignore this packet
690 pos_pkt_timestamp = INVALID_TIMESTAMP;
691 }
692 else
693 {
694 // Reset time watch:
695 m_last_pos_packet_timestamp = pos_pkt_timestamp;
696 }
697 }
698
699 return ret;
700 }
701
702 // static method:
703 mrpt::system::TTimeStamp CVelodyneScanner::internal_receive_UDP_packet(
704 platform_socket_t hSocket,
705 uint8_t* out_buffer,
706 size_t expected_packet_size,
707 const std::string& filter_only_from_IP)
708 {
709 if (hSocket == INVALID_SOCKET)
710 THROW_EXCEPTION(
711 "Error: UDP socket is not open yet! Have you called initialize() "
712 "first?");
713
714 unsigned long devip_addr = 0;
715 if (!filter_only_from_IP.empty()) devip_addr = inet_addr(filter_only_from_IP.c_str());
716
717 const mrpt::system::TTimeStamp time1 = mrpt::Clock::now();
718
719 struct pollfd fds[1];
720 fds[0].fd = hSocket;
721 fds[0].events = POLLIN;
722 static const int POLL_TIMEOUT = 1; // (ms)
723
724 sockaddr_in sender_address;
725 socklen_t sender_address_len = sizeof(sender_address);
726
727 while (true)
728 {
729 // Unfortunately, the Linux kernel recvfrom() implementation
730 // uses a non-interruptible std::this_thread::sleep_for(ms) when waiting
731 // for data,
732 // which would cause this method to hang if the device is not
733 // providing data. We poll() the device first to make sure
734 // the recvfrom() will not block.
735 //
736 // Note, however, that there is a known Linux kernel bug:
737 //
738 // Under Linux, select() may report a socket file descriptor
739 // as "ready for reading", while nevertheless a subsequent
740 // read blocks. This could for example happen when data has
741 // arrived but upon examination has wrong checksum and is
742 // discarded. There may be other circumstances in which a
743 // file descriptor is spuriously reported as ready. Thus it
744 // may be safer to use O_NONBLOCK on sockets that should not
745 // block.
746
747 // poll() until input available
748 do
749 {
750 int retval =
751 #if !defined(_WIN32)
752 poll
753 #else
754 WSAPoll
755 #endif
756 (fds, 1, POLL_TIMEOUT);
757 if (retval < 0) // poll() error?
758 {
759 if (errno != EINTR)
760 THROW_EXCEPTION(format(
761 "Error in UDP poll():\n%s", mrpt::comms::net::getLastSocketErrorStr().c_str()));
762 }
763 if (retval == 0) // poll() timeout?
764 {
765 return INVALID_TIMESTAMP;
766 }
767 if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
768 (fds[0].revents & POLLNVAL)) // device error?
769 {
770 THROW_EXCEPTION("Error in UDP poll() (seems Velodyne device error?)");
771 }
772 } while ((fds[0].revents & POLLIN) == 0);
773
774 // Receive packets that should now be available from the
775 // socket using a blocking read.
776 int nbytes = recvfrom(
777 hSocket, (char*)&out_buffer[0], expected_packet_size, 0, (sockaddr*)&sender_address,
778 &sender_address_len);
779
780 if (nbytes < 0)
781 {
782 if (errno != EWOULDBLOCK) THROW_EXCEPTION("recvfrom() failed!?!");
783 }
784 else if ((size_t)nbytes == expected_packet_size)
785 {
786 // read successful,
787 // if packet is not from the lidar scanner we selected by IP,
788 // continue otherwise we are done
789 if (!filter_only_from_IP.empty() && sender_address.sin_addr.s_addr != devip_addr)
790 continue;
791 else
792 break; // done
793 }
794
795 std::cerr << "[CVelodyneScanner] Warning: incomplete Velodyne packet read: " << nbytes
796 << " bytes\n";
797 }
798
799 // Average the times at which we begin and end reading. Use that to
800 // estimate when the scan occurred.
801 const mrpt::system::TTimeStamp time2 = mrpt::Clock::now();
802
803 return mrpt::Clock::time_point(mrpt::Clock::duration(
804 time1.time_since_epoch().count() / 2 + time2.time_since_epoch().count() / 2));
805 }
806
807 bool CVelodyneScanner::internal_read_PCAP_packet(
808 mrpt::system::TTimeStamp& data_pkt_time,
809 uint8_t* out_data_buffer,
810 mrpt::system::TTimeStamp& pos_pkt_time,
811 uint8_t* out_pos_buffer)
812 {
813 #if MRPT_HAS_LIBPCAP
814 ASSERT_(m_pcap);
815
816 data_pkt_time = INVALID_TIMESTAMP;
817 pos_pkt_time = INVALID_TIMESTAMP;
818
819 char errbuf[PCAP_ERRBUF_SIZE];
820 struct pcap_pkthdr* header;
821 const u_char* pkt_data;
822
823 while (true)
824 {
825 int res;
826 if ((res = pcap_next_ex(reinterpret_cast<pcap_t*>(m_pcap), &header, &pkt_data)) >= 0)
827 {
828 ++m_pcap_read_count;
829
830 // if packet is not from the lidar scanner we selected by IP,
831 // continue
832 if (pcap_offline_filter(
833 reinterpret_cast<bpf_program*>(m_pcap_bpf_program), header, pkt_data) == 0)
834 {
835 if (m_verbose)
836 std::cout << "[CVelodyneScanner] DEBUG: Filtering out packet #" << m_pcap_read_count
837 << " in PCAP file.\n";
838 continue;
839 }
840
841 // Determine whether it is a DATA or POSITION packet:
842 m_pcap_file_empty = false;
843 const mrpt::system::TTimeStamp tim = mrpt::Clock::now();
844 const uint16_t udp_dst_port = ntohs(*reinterpret_cast<const uint16_t*>(pkt_data + 0x24));
845
846 if (udp_dst_port == CVelodyneScanner::VELODYNE_POSITION_UDP_PORT)
847 {
848 if (m_verbose)
849 std::cout << "[CVelodyneScanner] DEBUG: Packet #" << m_pcap_read_count
850 << " in PCAP file is POSITION pkt.\n";
851 memcpy(out_pos_buffer, pkt_data + 42, CObservationVelodyneScan::POS_PACKET_SIZE);
852 pos_pkt_time = tim; // success
853 return true;
854 }
855 else if (udp_dst_port == CVelodyneScanner::VELODYNE_DATA_UDP_PORT)
856 {
857 if (m_verbose)
858 std::cout << "[CVelodyneScanner] DEBUG: Packet #" << m_pcap_read_count
859 << " in PCAP file is DATA pkt.\n";
860 memcpy(out_data_buffer, pkt_data + 42, CObservationVelodyneScan::PACKET_SIZE);
861 data_pkt_time = tim; // success
862 return true;
863 }
864 else
865 {
866 std::cerr << "[CVelodyneScanner] ERROR: Packet " << m_pcap_read_count
867 << " in PCAP file passed the filter but does not "
868 "match expected UDP port numbers! Skipping it.\n";
869 }
870 }
871
872 if (m_pcap_file_empty) // no data in file?
873 {
874 fprintf(
875 stderr,
876 "[CVelodyneScanner] Maybe the PCAP file is empty? Error %d "
877 "reading Velodyne packet: `%s`\n",
878 res, pcap_geterr(reinterpret_cast<pcap_t*>(m_pcap)));
879 return true;
880 }
881
882 if (m_pcap_read_once)
883 {
884 if (m_pcap_verbose)
885 printf(
886 "[CVelodyneScanner] INFO: end of file reached -- done "
887 "reading.\n");
888 std::this_thread::sleep_for(250ms);
889 return false;
890 }
891
892 if (m_pcap_repeat_delay > 0.0)
893 {
894 if (m_pcap_verbose)
895 printf(
896 "[CVelodyneScanner] INFO: end of file reached -- delaying "
897 "%.3f seconds.\n",
898 m_pcap_repeat_delay);
899 std::this_thread::sleep_for(
900 std::chrono::duration<double, std::milli>(m_pcap_repeat_delay * 1000.0));
901 }
902
903 if (m_pcap_verbose) printf("[CVelodyneScanner] INFO: replaying Velodyne dump file.\n");
904
905 // rewind the file
906 pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
907 if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) == nullptr)
908 {
909 THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
910 }
911 m_pcap_file_empty = true; // maybe the file disappeared?
912 } // loop back and try again
913 #else
914 THROW_EXCEPTION("MRPT needs to be built against libpcap to enable this functionality");
915 #endif
916 }
917
918 bool CVelodyneScanner::setLidarReturnType(return_type_t ret_type)
919 {
920 /* HTTP-based config: http://10.0.0.100/tab/config.html
921 <form name='returns' method="post" action="/cgi/setting">
922 <span>Return&nbsp;Type:&nbsp;</span> <select name="returns"
923 onchange='javascript:this.form.submit()'>
924 <option>Strongest</option>
925 <option>Last</option>
926 <option>Dual</option>
927 </select>
928 </form>
929 */
930 MRPT_START
931 std::string strRet;
932 switch (ret_type)
933 {
934 case STRONGEST:
935 strRet = "Strongest";
936 break;
937 case DUAL:
938 strRet = "Dual";
939 break;
940 case LAST:
941 strRet = "Last";
942 break;
943 case UNCHANGED:
944 return true;
945 default:
946 THROW_EXCEPTION("Invalid value for return type!");
947 };
948
949 const std::string cmd = mrpt::format("returns=%s", strRet.c_str());
950 return this->internal_send_http_post(cmd);
951 MRPT_END
952 }
953
954 bool CVelodyneScanner::setLidarRPM(int rpm)
955 {
956 /* HTTP-based config: http://10.0.0.100/tab/config.html
957 <form name='rpm' method="post" action="/cgi/setting">
958 Motor
959 &nbsp;RPM:&nbsp;<input type="text" name="rpm" size="5"
960 style="text-align:right" /><input type="button" value="+"
961 onclick="javascript:this.form.rpm.value++;this.form.submit()" /><input
962 type="button" value="-"
963 onclick="javascript:this.form.rpm.value--;this.form.submit()" />
964 &nbsp;<input type="submit" value="Set" />
965 </form>
966 */
967
968 MRPT_START
969 const std::string cmd = mrpt::format("rpm=%i", rpm);
970 return this->internal_send_http_post(cmd);
971 MRPT_END
972 }
973
974 bool CVelodyneScanner::setLidarOnOff(bool on)
975 {
976 // laser = on|off
977 MRPT_START
978 const std::string cmd = mrpt::format("laser=%s", on ? "on" : "off");
979 return this->internal_send_http_post(cmd);
980 MRPT_END
981 }
982
983 void CVelodyneScanner::setFramePublishing(bool on)
984 {
985 // frame publishing | data packet publishing = on|off
986 MRPT_START
987 m_return_frames = on;
988 MRPT_END
989 }
990
991 bool CVelodyneScanner::internal_send_http_post(const std::string& post_data)
992 {
993 MRPT_START
994
995 ASSERTMSG_(!m_device_ip.empty(), "A device IP address must be specified first!");
996
997 using namespace mrpt::comms::net;
998
999 std::vector<uint8_t> post_out;
1000
1001 HttpRequestOutput httpOut;
1002 HttpRequestOptions ho;
1003
1004 ho.extra_headers["Origin"] = mrpt::format("http://%s", m_device_ip.c_str());
1005 ho.extra_headers["Referer"] = mrpt::format("http://%s", m_device_ip.c_str());
1006 ho.extra_headers["Upgrade-Insecure-Requests"] = "1";
1007 ho.extra_headers["Content-Type"] = "application/x-www-form-urlencoded";
1008
1009 http_errorcode ret = http_request(
1010 "POST", post_data, mrpt::format("http://%s/cgi/setting", m_device_ip.c_str()), post_out, ho,
1011 httpOut);
1012
1013 const int http_rep_code = httpOut.http_responsecode;
1014
1015 return mrpt::comms::net::http_errorcode::Ok == ret &&
1016 (http_rep_code == 200 || http_rep_code == 204); // OK codes
1017
1018 MRPT_END
1019 }