| 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(¤tTime, 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 Type: </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 |
RPM: <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 |
<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 |
} |