23 #define _WINSOCK_DEPRECATED_NO_WARNINGS 24 #if defined(_WIN32_WINNT) && (_WIN32_WINNT < 0x600) 26 #define _WIN32_WINNT 0x600 // Minimum: Windows Vista (required to pollfd) 33 #pragma comment(lib, "WS2_32.LIB") 37 #define INVALID_SOCKET (-1) 39 #include <sys/socket.h> 43 #include <sys/types.h> 44 #include <sys/ioctl.h> 46 #include <arpa/inet.h> 47 #include <netinet/in.h> 48 #include <netinet/tcp.h> 54 #if !defined(PCAP_NETMASK_UNKNOWN) // for older pcap versions 55 #define PCAP_NETMASK_UNKNOWN 0xffffffff 73 static bool init =
false;
81 return modelProperties;
90 it != lst.end(); ++it)
101 m_pos_packets_min_period(0.5),
102 m_pos_packets_timing_timeout(30.0),
104 m_return_frames(true),
105 m_pcap_verbose(true),
109 m_pcap_dumper(nullptr),
110 m_pcap_bpf_program(nullptr),
111 m_pcap_file_empty(true),
112 m_pcap_read_once(false),
113 m_pcap_read_fast(false),
114 m_pcap_read_full_scan_delay_ms(100),
115 m_pcap_repeat_delay(0.0),
116 m_hDataSock(INVALID_SOCKET),
117 m_hPositionSock(INVALID_SOCKET),
120 m_lidar_return(UNCHANGED)
130 WORD wVersionRequested = MAKEWORD(2, 0);
132 if (WSAStartup(wVersionRequested, &wsaData))
195 if (lstModels.find(
m_model) == lstModels.end())
198 "Unrecognized `model` parameter: `%u` . Known values are: %s",
199 static_cast<unsigned int>(
m_model),
228 data_pkt_timestamp, rx_pkt, pos_pkt_timestamp, rx_pos_pkt);
239 mrpt::make_aligned_shared<mrpt::obs::CObservationGPS>();
243 gps_obs->originalReceivedTimestamp = pos_pkt_timestamp;
251 gps_obs->has_satellite_timestamp =
false;
252 gps_obs->timestamp = pos_pkt_timestamp;
277 if ((rx_pkt_start_angle <
278 m_rx_scan->scan_packets.rbegin()->blocks[0].rotation) ||
288 std::this_thread::sleep_for(
289 std::chrono::duration<double, std::milli>(
311 if (it != lstModels.end())
313 m_rx_scan->maxRange = it->second.maxRange;
325 m_rx_scan->originalReceivedTimestamp = data_pkt_timestamp;
354 m_rx_scan->has_satellite_timestamp =
true;
358 m_rx_scan->has_satellite_timestamp =
false;
359 m_rx_scan->timestamp = data_pkt_timestamp;
364 m_rx_scan->scan_packets.push_back(rx_pkt);
371 cerr <<
"[CVelodyneScanner::getObservation] Returning false due to " 374 cerr << e.what() << endl;
393 cerr <<
"ERROR receiving data from Velodyne devic!" << endl;
416 "Could not find default calibration data for the given LIDAR " 417 "`model` name. Please, specify a valid `model` or load a valid " 418 "XML configuration file first.");
439 if (INVALID_SOCKET == (
m_hDataSock = socket(PF_INET, SOCK_DGRAM, 0)))
441 "Error creating UDP socket:\n%s",
444 struct sockaddr_in bindAddr;
445 memset(&bindAddr, 0,
sizeof(bindAddr));
446 bindAddr.sin_family = AF_INET;
448 bindAddr.sin_addr.s_addr = INADDR_ANY;
450 if (
int(INVALID_SOCKET) ==
452 m_hDataSock, (
struct sockaddr*)(&bindAddr),
sizeof(sockaddr)))
456 unsigned long non_block_mode = 1;
457 if (ioctlsocket(
m_hDataSock, FIONBIO, &non_block_mode))
459 "Error entering non-blocking mode with ioctlsocket().");
464 oldflags |= O_NONBLOCK | FASYNC;
471 if (INVALID_SOCKET ==
474 "Error creating UDP socket:\n%s",
479 if (
int(INVALID_SOCKET) == ::bind(
481 (
struct sockaddr*)(&bindAddr),
488 "Error entering non-blocking mode with ioctlsocket().");
493 oldflags |= O_NONBLOCK | FASYNC;
501 char errbuf[PCAP_ERRBUF_SIZE];
505 "\n[CVelodyneScanner] Opening PCAP file \"%s\"\n",
516 "(udp dst port %d || udp dst port %d)",
523 "[CVelodyneScanner] Error calling pcap_compile: ";
537 reinterpret_cast<pcap_t*>(
m_pcap),
539 filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN) < 0)
540 pcap_perror(reinterpret_cast<pcap_t*>(
m_pcap), &sMsgError[0]);
548 "Velodyne: Reading from PCAP requires building MRPT with libpcap " 586 pcap_close(reinterpret_cast<pcap_t*>(
m_pcap));
591 pcap_dump_close(reinterpret_cast<pcap_dumper_t*>(
m_pcap_dumper));
596 pcap_close(reinterpret_cast<pcap_t*>(
m_pcap_out));
605 const uint16_t LidarPacketHeader[21] = {0xffff, 0xffff, 0xffff, 0x7660, 0x0088,
606 0x0000, 0x0008, 0x0045, 0xd204, 0x0000,
607 0x0040, 0x11ff, 0xaab4, 0xa8c0, 0xc801,
611 0xffff, 0x4009, 0x4009, 0xbe04, 0x0000};
612 const uint16_t PositionPacketHeader[21] = {
613 0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204,
614 0x0000, 0x0040, 0x11ff, 0xaab4, 0xa8c0, 0xc801, 0xffff,
618 0xffff, 0x7420, 0x7420, 0x0802, 0x0000};
621 #if defined(_WIN32) && MRPT_HAS_LIBPCAP 622 int gettimeofday(
struct timeval* tp,
void*)
625 ::GetSystemTimeAsFileTime(&ft);
627 (
static_cast<long long>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
628 t -= 116444736000000000LL;
630 tp->tv_sec =
static_cast<long>(
t / 1000000UL);
631 tp->tv_usec =
static_cast<long>(
t % 1000000UL);
644 CObservationVelodyneScan::POS_PACKET_SIZE);
647 CObservationVelodyneScan::PACKET_SIZE);
653 data_pkt_timestamp, (
uint8_t*)&out_data_pkt, pos_pkt_timestamp,
660 CObservationVelodyneScan::PACKET_SIZE,
m_device_ip);
663 CObservationVelodyneScan::POS_PACKET_SIZE,
m_device_ip);
673 char errbuf[PCAP_ERRBUF_SIZE];
677 string sFilePostfix =
"_";
679 "%04u-%02u-%02u_%02uh%02um%02us", (
unsigned int)parts.
year,
680 (
unsigned int)parts.
month, (
unsigned int)parts.
day,
681 (
unsigned int)parts.
hour, (
unsigned int)parts.
minute,
682 (
unsigned int)parts.
second);
683 const string sFileName =
688 m_pcap_out = pcap_open_dead(DLT_EN10MB, 65535);
690 m_pcap_out !=
nullptr,
"Error creating PCAP live capture handle");
693 "\n[CVelodyneScanner] Writing to PCAP file \"%s\"\n",
696 reinterpret_cast<pcap_t*>(
m_pcap_out), sFileName.c_str())) ==
700 "Error creating PCAP live dumper: '%s'", errbuf);
704 "Velodyne: Writing PCAP files requires building MRPT with libpcap " 713 struct pcap_pkthdr
header;
714 struct timeval currentTime;
715 gettimeofday(¤tTime,
nullptr);
716 std::vector<unsigned char> packetBuffer;
722 CObservationVelodyneScan::PACKET_SIZE + 42;
723 packetBuffer.resize(
header.caplen);
726 memcpy(&(packetBuffer[0]), LidarPacketHeader, 42);
728 &(packetBuffer[0]) + 42, (
uint8_t*)&out_data_pkt,
729 CObservationVelodyneScan::PACKET_SIZE);
737 CObservationVelodyneScan::POS_PACKET_SIZE + 42;
738 packetBuffer.resize(
header.caplen);
741 memcpy(&(packetBuffer[0]), PositionPacketHeader, 42);
743 &(packetBuffer[0]) + 42, (
uint8_t*)&out_pos_pkt,
744 CObservationVelodyneScan::POS_PACKET_SIZE);
754 #if MRPT_IS_BIG_ENDIAN 758 for (
int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET; i++)
762 for (
int k = 0; k < CObservationVelodyneScan::SCANS_PER_BLOCK; k++)
800 const size_t expected_packet_size,
const std::string& filter_only_from_IP)
802 if (hSocket == INVALID_SOCKET)
804 "Error: UDP socket is not open yet! Have you called initialize() " 807 unsigned long devip_addr = 0;
808 if (!filter_only_from_IP.empty())
809 devip_addr = inet_addr(filter_only_from_IP.c_str());
813 struct pollfd fds[1];
815 fds[0].events = POLLIN;
816 static const int POLL_TIMEOUT = 1;
818 sockaddr_in sender_address;
819 socklen_t sender_address_len =
sizeof(sender_address);
850 (fds, 1, POLL_TIMEOUT);
855 "Error in UDP poll():\n%s",
862 if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
863 (fds[0].revents & POLLNVAL))
866 "Error in UDP poll() (seems Velodyne device error?)");
868 }
while ((fds[0].revents & POLLIN) == 0);
872 int nbytes = recvfrom(
873 hSocket, (
char*)&out_buffer[0], expected_packet_size, 0,
874 (sockaddr*)&sender_address, &sender_address_len);
880 else if ((
size_t)nbytes == expected_packet_size)
885 if (!filter_only_from_IP.empty() &&
886 sender_address.sin_addr.s_addr != devip_addr)
893 <<
"[CVelodyneScanner] Warning: incomplete Velodyne packet read: " 894 << nbytes <<
" bytes\n";
902 time1.time_since_epoch().count() / 2 +
903 time2.time_since_epoch().count() / 2));
916 char errbuf[PCAP_ERRBUF_SIZE];
917 struct pcap_pkthdr*
header;
918 const u_char* pkt_data;
923 if ((
res = pcap_next_ex(
924 reinterpret_cast<pcap_t*>(
m_pcap), &
header, &pkt_data)) >= 0)
930 if (pcap_offline_filter(
936 <<
"[CVelodyneScanner] DEBUG: Filtering out packet #" 945 ntohs(*reinterpret_cast<const uint16_t*>(pkt_data + 0x24));
950 std::cout <<
"[CVelodyneScanner] DEBUG: Packet #" 952 <<
" in PCAP file is POSITION pkt.\n";
954 out_pos_buffer, pkt_data + 42,
955 CObservationVelodyneScan::POS_PACKET_SIZE);
962 std::cout <<
"[CVelodyneScanner] DEBUG: Packet #" 964 <<
" in PCAP file is DATA pkt.\n";
966 out_data_buffer, pkt_data + 42,
967 CObservationVelodyneScan::PACKET_SIZE);
973 std::cerr <<
"[CVelodyneScanner] ERROR: Packet " 975 <<
" in PCAP file passed the filter but does not " 976 "match expected UDP port numbers! Skipping it.\n";
984 "[CVelodyneScanner] Maybe the PCAP file is empty? Error %d " 985 "reading Velodyne packet: `%s`\n",
986 res, pcap_geterr(reinterpret_cast<pcap_t*>(
m_pcap)));
994 "[CVelodyneScanner] INFO: end of file reached -- done " 996 std::this_thread::sleep_for(250ms);
1004 "[CVelodyneScanner] INFO: end of file reached -- delaying " 1007 std::this_thread::sleep_for(
1008 std::chrono::duration<double, std::milli>(
1013 printf(
"[CVelodyneScanner] INFO: replaying Velodyne dump file.\n");
1016 pcap_close(reinterpret_cast<pcap_t*>(
m_pcap));
1026 "MRPT needs to be built against libpcap to enable this functionality")
1047 strRet =
"Strongest";
1108 !
m_device_ip.empty(),
"A device IP address must be specified first!");
1112 std::vector<uint8_t> post_out;
1113 string post_err_str;
1120 extra_headers[
"Upgrade-Insecure-Requests"] =
"1";
1121 extra_headers[
"Content-Type"] =
"application/x-www-form-urlencoded";
1126 post_err_str, 80 ,
string(),
string(),
1127 &http_rep_code, &extra_headers, &out_headers);
1130 (http_rep_code == 200 || http_rep_code == 204);
std::chrono::duration< rep, period > duration
void timestampToParts(TTimeStamp t, TTimeParts &p, bool localTime=false)
Gets the individual parts of a date/time (days, hours, minutes, seconds) - UTC time or local time...
content_t fields
Message content, accesible by individual fields.
bool loadFromXMLFile(const std::string &velodyne_calibration_xml_filename)
Loads calibration from file, in the format supplied by the manufacturer.
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
unsigned __int16 uint16_t
A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working on Linux and Windows...
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
mrpt::system::TTimeStamp getAsTimestamp(const mrpt::system::TTimeStamp &date) const
Build an MRPT timestamp with the hour/minute/sec of this structure and the date from the given timest...
static short int VELODYNE_POSITION_UDP_PORT
Default: 8308.
std::map< model_t, TModelProperties > model_properties_list_t
raw_block_t blocks[BLOCKS_PER_PACKET]
std::chrono::time_point< Clock > time_point
#define THROW_EXCEPTION(msg)
platform_socket_t m_hPositionSock
uint32_t gps_timestamp
us from top of hour
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
bool getNextObservation(mrpt::obs::CObservationVelodyneScan::Ptr &outScan, mrpt::obs::CObservationGPS::Ptr &outGPS)
Polls the UDP port for incoming data packets.
bool internal_read_PCAP_packet(mrpt::system::TTimeStamp &data_pkt_time, uint8_t *out_data_buffer, mrpt::system::TTimeStamp &pos_pkt_time, uint8_t *out_pos_buffer)
mrpt::system::TTimeStamp buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
bool m_return_frames
Default: true Output whole frames and not data packets.
static std::string getListKnownModels()
Return human-readable string: "`VLP16`,`XXX`,...".
std::string getLastSocketErrorStr()
Returns a description of the last Sockets error.
void close()
Close the UDP sockets set-up in initialize().
bool m_pcap_read_once
Default: false.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Contains classes for various device interfaces.
void * m_pcap_bpf_program
opaque ptr: bpf_program*
float read_float(const std::string §ion, const std::string &name, float defaultValue, bool failIfNotFound=false) const
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char. ...
model_t
LIDAR model types.
void reverseBytesInPlace(bool &v_in_out)
Reverse the order of the bytes of a given type (useful for transforming btw little/big endian) ...
int8_t validity_char
This will be: 'A'=OK or 'V'=void.
bool m_pcap_read_fast
(Default: false) If false, will use m_pcap_read_full_scan_delay_ms
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
mrpt::system::TTimeStamp m_last_pos_packet_timestamp
char NMEA_GPRMC[72+234]
the full $GPRMC message, as received by Velodyne, terminated with "\r\n\0"
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
double m_pcap_repeat_delay
Default: 0 (in seconds)
mrpt::obs::VelodyneCalibration m_velodyne_calib
Device calibration file (supplied by vendor in an XML file)
#define ASSERT_(f)
Defines an assertion mechanism.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
This class allows loading and storing values and vectors of different types from a configuration text...
ERRORCODE_HTTP
Possible returns from a HTTP request.
virtual void initialize()
Tries to initialize the sensor driver, after setting all the parameters with a call to loadConfig...
double m_pos_packets_min_period
Default: 0.5 seconds.
std::shared_ptr< CObservationVelodyneScan > Ptr
return_type_t m_lidar_return
bool setLidarOnOff(bool on)
Switches the LASER on/off (saves energy when not measuring) (via HTTP post interface).
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
bool receivePackets(mrpt::system::TTimeStamp &data_pkt_timestamp, mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket &out_data_pkt, mrpt::system::TTimeStamp &pos_pkt_timestamp, mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket &out_pos_pkt)
Users normally would prefer calling getNextObservation() instead.
The parts of a date/time (it's like the standard 'tm' but with fractions of seconds).
virtual ~CVelodyneScanner()
unsigned int m_pcap_read_count
number of pkts read from the file so far (for debugging)
std::string m_pcap_output_file
Default: "" (do not dump to an offline file)
static const model_properties_list_t & get()
Singleton access.
This namespace contains representation of robot actions and observations.
bool m_pcap_verbose
Default: true Output PCAP Info msgs.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
platform_socket_t m_hDataSock
bool setLidarReturnType(return_type_t ret_type)
Changes among STRONGEST, LAST, DUAL return types (via HTTP post interface).
GLsizei const GLchar ** string
uint16_t header
Block id: UPPER_BANK or LOWER_BANK.
std::string m_pcap_input_file
Default: "" (do not operate from an offline file)
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
static bool parse_NMEA(const std::string &cmd_line, mrpt::obs::CObservationGPS &out_obs, const bool verbose=false)
Parses one line of NMEA data from a GPS receiver, and writes the recognized fields (if any) into an o...
void setFramePublishing(bool on)
Switches whole frame (points in a single revolution) on/off publication to data packet publication...
Payload of one POSITION packet.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
double second
Minute (0-59)
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
void * m_pcap_out
opaque ptr: "pcap_t*"
bool internal_send_http_post(const std::string &post_data)
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double m_pos_packets_timing_timeout
Default: 30 seconds.
laser_return_t laser_returns[SCANS_PER_BLOCK]
uint8_t minute
Hour (0-23)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
One unit of data from the scanner (the payload of one UDP DATA packet)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
bool empty() const
Returns true if no calibration has been loaded yet.
static short int VELODYNE_DATA_UDP_PORT
Default: 2368.
double m_pcap_read_full_scan_delay_ms
(Default:100 ms) delay after each full scan read from a PCAP log
void * m_pcap
opaque ptr: "pcap_t*"
std::shared_ptr< CObservationGPS > Ptr
ERRORCODE_HTTP http_request(const string &http_method, const string &http_send_content, const string &url, std::vector< uint8_t > &out_content, string &out_errormsg, int port=80, const string &auth_user=string(), const string &auth_pass=string(), int *out_http_responsecode=nullptr, mrpt::system::TParameters< string > *extra_headers=nullptr, mrpt::system::TParameters< string > *out_headers=nullptr, int timeout_ms=1000)
Generic function for HTTP GET & POST methods.
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
A set of useful routines for networking.
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
std::shared_ptr< T > make_aligned_shared(Args &&... args)
Creates a shared_ptr with aligned memory via aligned_allocator_cpp11<>.
uint32_t platform_socket_t
Handles for the UDP sockets, or INVALID_SOCKET (-1)
uint16_t rotation
0-35999, divide by 100 to get degrees
#define MRPT_COMPILE_TIME_ASSERT(expression)
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
void * m_pcap_dumper
opaque ptr: "pcap_dumper_t *"
std::string m_device_ip
Default: "" (no IP-based filtering)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
bool loadCalibrationFile(const std::string &velodyne_xml_calib_file_path)
Returns false on error.
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string §ion)
See the class documentation at the top for expected parameters.
return_type_t
LIDAR return type.
const Scalar * const_iterator
mrpt::poses::CPose3D m_sensorPose
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
mrpt::system::TTimeStamp m_last_gps_rmc_age
mrpt::obs::CObservationVelodyneScan::Ptr m_rx_scan
In progress RX scan.
static mrpt::system::TTimeStamp internal_receive_UDP_packet(platform_socket_t hSocket, uint8_t *out_buffer, const size_t expected_packet_size, const std::string &filter_only_from_IP)
model_t m_model
Default: "VLP16".
mrpt::obs::gnss::Message_NMEA_RMC m_last_gps_rmc
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
bool setLidarRPM(int rpm)
Changes Lidar RPM (valid range: 300-600) (via HTTP post interface).