diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7aba7b5..a2a5f0b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,18 @@ rock_library(velodyne_lidar - SOURCES velodyneDataDriver.cpp velodynePositioningDriver.cpp pointcloudConvertHelper.cpp - HEADERS velodyneDataDriver.hpp velodynePositioningDriver.hpp pointcloudConvertHelper.hpp velodyneConstants.hpp velodyneProtocolTypes.hpp MultilevelLaserScan.h gps_rmc_type.h + SOURCES velodyneDataDriver.cpp + VelodyneHDL32EDriver.cpp + VelodyneVLP16Driver.cpp + velodynePositioningDriver.cpp + pointcloudConvertHelper.cpp + HEADERS velodyneDataDriver.hpp + VelodyneHDL32EDriver.hpp + VelodyneVLP16Driver.hpp + velodynePositioningDriver.hpp + pointcloudConvertHelper.hpp + velodyneConstants.hpp + velodyneProtocolTypes.hpp + MultilevelLaserScan.h + gps_rmc_type.h DEPS_PKGCONFIG base-lib iodrivers_base aggregator) rock_executable(velodyne_lidar_bin test_velodyneDriver.cpp diff --git a/src/VelodyneHDL32EDriver.cpp b/src/VelodyneHDL32EDriver.cpp new file mode 100644 index 0000000..ac85622 --- /dev/null +++ b/src/VelodyneHDL32EDriver.cpp @@ -0,0 +1,84 @@ +#include "VelodyneHDL32EDriver.hpp" + +using namespace velodyne_lidar; + +static const uint16_t UPPER_HEADER_BYTES = 0xEEFF; //The byte indicating a upper shot +static const uint16_t LOWER_HEADER_BYTES = 0xDDFF; //The byte indicating a lower shot +static const unsigned int NUM_LASERS = 32; +static const unsigned int FIRING_ORDER[] = {31, 29, 27, 25, 23, 21, 19, 17, 15, 13, 11, 9, 7, 5, 3, 1, + 30, 28, 26, 24, 22, 20, 18, 16, 14, 12, 10, 8, 6, 4, 2, 0}; +static const double DRIVER_BROADCAST_FREQ_HZ = 1808.0; +static const double VERTICAL_RESOLUTION = 1.0 + 1.0/3.0; // vertical resolution in degree +static const double VERTICAL_START_ANGLE = -VERTICAL_RESOLUTION * 8.0; // vertical start angle in degree +static const double VERTICAL_END_ANGLE = VERTICAL_RESOLUTION * 23.0; // vertical end angle in degree + +VelodyneHDL32EDriver::VelodyneHDL32EDriver() : VelodyneDataDriver(velodyne_lidar::VELODYNE_HDL32E, DRIVER_BROADCAST_FREQ_HZ) +{ + +} + +VelodyneHDL32EDriver::~VelodyneHDL32EDriver() +{ + +} + +double VelodyneHDL32EDriver::getBroadcastFrequency() +{ + return DRIVER_BROADCAST_FREQ_HZ; +} + +bool VelodyneHDL32EDriver::convertScanToSample(base::samples::DepthMap& sample, bool copy_remission) +{ + if(packets.empty()) + return false; + + if(packets[0].packet.return_mode == DualReturn) + throw std::runtime_error("Return mode dual return is not supported!"); + + // prepare sample + sample.reset(); + unsigned horizontal_steps = packets.size() * VELODYNE_NUM_SHOTS; + unsigned measurement_count = horizontal_steps * NUM_LASERS; + sample.distances.resize(measurement_count); + if(copy_remission) + sample.remissions.resize(measurement_count); + sample.timestamps.resize(horizontal_steps); + sample.time = packets.back().time[VELODYNE_NUM_SHOTS-1]; + sample.horizontal_size = horizontal_steps; + sample.vertical_size = NUM_LASERS; + sample.horizontal_interval.resize(horizontal_steps); + sample.vertical_interval.push_back(base::Angle::deg2Rad(VERTICAL_START_ANGLE)); + sample.vertical_interval.push_back(base::Angle::deg2Rad(VERTICAL_END_ANGLE)); + sample.horizontal_projection = base::samples::DepthMap::POLAR; + sample.vertical_projection = base::samples::DepthMap::POLAR; + base::samples::DepthMap::DepthMatrixMap distances = sample.getDistanceMatrixMap(); + base::samples::DepthMap::DepthMatrixMap remissions = base::samples::DepthMap::DepthMatrixMap(sample.remissions.data(), sample.vertical_size, sample.horizontal_size); + + for(unsigned i = 0; i < packets.size(); i++) + { + for(unsigned j = 0; j < VELODYNE_NUM_SHOTS; j++) + { + unsigned column = (i * VELODYNE_NUM_SHOTS) + j; + sample.timestamps[column] = packets[i].time[j]; + const velodyne_fire& shot = packets[i].packet.shots[j]; + sample.horizontal_interval[column] = base::Angle::fromDeg(360.0 - (double)shot.rotational_pos * 0.01).getRad(); + for(unsigned k = 0; k < NUM_LASERS; k++) + { + const vel_laser_t &laser = shot.lasers[FIRING_ORDER[k]]; + if(laser.distance == 0) // zero means no return within max range + distances(k, column) = base::infinity(); + else if(laser.distance <= MIN_SENSING_DISTANCE) // dismiss all values under 1m + distances(k, column) = 0.f; + else + distances(k, column) = (float)laser.distance * 0.002f; // velodyne acquires in 2mm-units + + if(copy_remission) + remissions(k, column) = (float)laser.intensity / 255.0f; + } + } + } + + packets.clear(); + current_batch_size = 0; + return true; +} diff --git a/src/VelodyneHDL32EDriver.hpp b/src/VelodyneHDL32EDriver.hpp new file mode 100644 index 0000000..e85855e --- /dev/null +++ b/src/VelodyneHDL32EDriver.hpp @@ -0,0 +1,30 @@ +#ifndef _VELODYNE_HDL32E_DRIVER_HPP_ +#define _VELODYNE_HDL32E_DRIVER_HPP_ + +#include "velodyneDataDriver.hpp" + +namespace velodyne_lidar +{ + +class VelodyneHDL32EDriver : public VelodyneDataDriver +{ +public: + VelodyneHDL32EDriver(); + virtual ~VelodyneHDL32EDriver(); + + /** + * Converts a full scan to a DepthMap sample. + * Also clears all collected packets of the corresponding laser head. + * @returns true on success, false if nothing to convert. + */ + virtual bool convertScanToSample(base::samples::DepthMap& sample, bool copy_remission = true); + + /** + * Return frequency of the arriving data samples + */ + virtual double getBroadcastFrequency(); +}; + +} + +#endif \ No newline at end of file diff --git a/src/VelodyneVLP16Driver.cpp b/src/VelodyneVLP16Driver.cpp new file mode 100644 index 0000000..4d78bbc --- /dev/null +++ b/src/VelodyneVLP16Driver.cpp @@ -0,0 +1,116 @@ +#include "VelodyneVLP16Driver.hpp" +#include + +using namespace velodyne_lidar; + +static const uint16_t VLP16_HEADER_BYTES = 0xFFEE; //VLP-16 identification bytes +static const unsigned int NUM_LASERS = 16; // The number of lasers per shot +static const unsigned int FIRING_ORDER[] = {15, 13, 11, 9, 7, 5, 3, 1, + 14, 12, 10, 8, 6, 4, 2, 0}; +static const double DRIVER_BROADCAST_FREQ_HZ = 754.0; //The rate of broadcast packets. +static const double VERTICAL_RESOLUTION = 2.0; // vertical resolution in degree +static const double VERTICAL_START_ANGLE = -VERTICAL_RESOLUTION * 7.5; +static const double VERTICAL_END_ANGLE = VERTICAL_RESOLUTION * 7.5; // vertical end angle in degree +static const uint64_t CYCLE_TIME = 55.296; // cycle time of one firing in micro seconds + +VelodyneVLP16Driver::VelodyneVLP16Driver() : VelodyneDataDriver(velodyne_lidar::VELODYNE_VLP16, DRIVER_BROADCAST_FREQ_HZ) +{ + +} + +VelodyneVLP16Driver::~VelodyneVLP16Driver() +{ + +} + +double VelodyneVLP16Driver::getBroadcastFrequency() +{ + return DRIVER_BROADCAST_FREQ_HZ; +} + +bool VelodyneVLP16Driver::convertScanToSample(base::samples::DepthMap& sample, bool copy_remission) +{ + if(packets.empty()) + return false; + + if(packets[0].packet.return_mode == DualReturn) + throw std::runtime_error("Return mode dual return is not supported!"); + + // prepare sample + sample.reset(); + unsigned horizontal_steps = 2 * packets.size() * VELODYNE_NUM_SHOTS; + unsigned measurement_count = horizontal_steps * NUM_LASERS; + sample.distances.resize(measurement_count); + if(copy_remission) + sample.remissions.resize(measurement_count); + sample.timestamps.resize(horizontal_steps); + sample.time = packets.back().time[VELODYNE_NUM_SHOTS-1] + base::Time::fromMicroseconds(CYCLE_TIME); + sample.horizontal_size = horizontal_steps; + sample.vertical_size = NUM_LASERS; + sample.horizontal_interval.resize(horizontal_steps); + sample.vertical_interval.push_back(base::Angle::deg2Rad(VERTICAL_START_ANGLE)); + sample.vertical_interval.push_back(base::Angle::deg2Rad(VERTICAL_END_ANGLE)); + sample.horizontal_projection = base::samples::DepthMap::POLAR; + sample.vertical_projection = base::samples::DepthMap::POLAR; + base::samples::DepthMap::DepthMatrixMap distances = sample.getDistanceMatrixMap(); + base::samples::DepthMap::DepthMatrixMap remissions = base::samples::DepthMap::DepthMatrixMap(sample.remissions.data(), sample.vertical_size, sample.horizontal_size); + + for(unsigned i = 0; i < packets.size(); i++) + { + for(unsigned j = 0; j < VELODYNE_NUM_SHOTS; j++) + { + const velodyne_fire& shot = packets[i].packet.shots[j]; + // handle first firing + unsigned column = (i * VELODYNE_NUM_SHOTS + j) * 2; + sample.timestamps[column] = packets[i].time[j]; + sample.horizontal_interval[column] = base::Angle::fromDeg(360.0 - (double)shot.rotational_pos * 0.01).getRad(); + + for(unsigned k = 0; k < NUM_LASERS; k++) + { + const vel_laser_t &laser = shot.lasers[FIRING_ORDER[k]]; + if(laser.distance == 0) // zero means no return within max range + distances(k, column) = base::infinity(); + else if(laser.distance <= MIN_SENSING_DISTANCE) // dismiss all values under 1m + distances(k, column) = 0.f; + else + distances(k, column) = (float)laser.distance * 0.002f; // velodyne acquires in 2mm-units + + if(copy_remission) + remissions(k, column) = (float)laser.intensity / 255.0f; + } + // handle second firing + column++; + sample.horizontal_interval[column] = base::NaN(); + for(unsigned k = 0; k < NUM_LASERS; k++) + { + const vel_laser_t &laser = shot.lasers[NUM_LASERS + FIRING_ORDER[k]]; + if(laser.distance == 0) // zero means no return within max range + distances(k, column) = base::infinity(); + else if(laser.distance <= MIN_SENSING_DISTANCE) // dismiss all values under 1m + distances(k, column) = 0.f; + else + distances(k, column) = (float)laser.distance * 0.002f; // velodyne acquires in 2mm-units + + if(copy_remission) + remissions(k, column) = (float)laser.intensity / 255.0f; + } + } + } + + // interpolate angles and timestamps for all odd entries + base::Angle angle_step; + for(unsigned column = 1; column < sample.horizontal_size - 1; column += 2) + { + base::Angle angle_before = base::Angle::fromRad(sample.horizontal_interval[column - 1]); + base::Angle angle_after = base::Angle::fromRad(sample.horizontal_interval[column + 1]); + angle_step = (angle_after - angle_before) * 0.5; + sample.horizontal_interval[column] = (angle_before + angle_step).getRad(); + sample.timestamps[column] = sample.timestamps[column - 1] + (sample.timestamps[column + 1] - sample.timestamps[column - 1]) * 0.5; + } + sample.timestamps[sample.horizontal_size - 1] = sample.time; + sample.horizontal_interval[sample.horizontal_size - 1] = (base::Angle::fromRad(sample.horizontal_interval[sample.horizontal_size - 2]) + angle_step).getRad(); + + packets.clear(); + current_batch_size = 0; + return true; +} \ No newline at end of file diff --git a/src/VelodyneVLP16Driver.hpp b/src/VelodyneVLP16Driver.hpp new file mode 100644 index 0000000..a079849 --- /dev/null +++ b/src/VelodyneVLP16Driver.hpp @@ -0,0 +1,30 @@ +#ifndef _VELODYNE_VLP16_DRIVER_HPP_ +#define _VELODYNE_VLP16_DRIVER_HPP_ + +#include "velodyneDataDriver.hpp" + +namespace velodyne_lidar +{ + +class VelodyneVLP16Driver : public VelodyneDataDriver +{ +public: + VelodyneVLP16Driver(); + virtual ~VelodyneVLP16Driver(); + + /** + * Converts a full scan to a DepthMap sample. + * Also clears all collected packets of the corresponding laser head. + * @returns true on success, false if nothing to convert. + */ + virtual bool convertScanToSample(base::samples::DepthMap& sample, bool copy_remission = true); + + /** + * Return frequency of the arriving data samples + */ + virtual double getBroadcastFrequency(); +}; + +} + +#endif \ No newline at end of file diff --git a/src/test_velodyneDriver.cpp b/src/test_velodyneDriver.cpp index 844eaff..5334c4d 100644 --- a/src/test_velodyneDriver.cpp +++ b/src/test_velodyneDriver.cpp @@ -48,7 +48,7 @@ #include #include -#include "velodyneDataDriver.hpp" +#include "VelodyneHDL32EDriver.hpp" using namespace std; @@ -113,7 +113,7 @@ int main(int argc, char **argv) // need these args for getopt(-long) } - velodyne_lidar::VelodyneDataDriver m_velodyneDriver; + velodyne_lidar::VelodyneHDL32EDriver m_velodyneDriver; m_velodyneDriver.openUDP("", velodyne_lidar::VELODYNE_DATA_UDP_PORT); diff --git a/src/velodyneConstants.hpp b/src/velodyneConstants.hpp index 1fd40f6..c14dc3a 100644 --- a/src/velodyneConstants.hpp +++ b/src/velodyneConstants.hpp @@ -1,29 +1,42 @@ #ifndef _VELODYNE_CONSTANTS_HPP_ #define _VELODYNE_CONSTANTS_HPP_ +#include + + namespace velodyne_lidar { - static const unsigned int VELODYNE_NUM_LASERS = 32; // The number of lasers per shot + static const unsigned int VELODYNE_NUM_SHOTS = 12; // The number of shots per packet + static const unsigned int VELODYNE_NUM_LASER_CHANNELS = 32; // The number of lasers per shot static const unsigned int VELODYNE_ORIENTATION_READINGS = 3; // The number of orientation readings static const unsigned int MIN_SENSING_DISTANCE = 500; //1m in 2mm units static const unsigned int MAX_SENSING_DISTANCE = 35000; //70m in 2mm units static const unsigned int VELODYNE_DATA_MSG_BUFFER_SIZE = 1206; //The size of a data packet static const unsigned int VELODYNE_POSITIONING_MSG_BUFFER_SIZE = 512; //The size of a positioning packet - static const double VELODYNE_DRIVER_BROADCAST_FREQ_HZ = 1808.0; //The rate of broadcast packets. See Velodyne hdl-32e manual page 11. static const unsigned int VELODYNE_DATA_UDP_PORT = 2368; //The port the Velodyne sends laser data to static const unsigned int VELODYNE_POSITIONING_UDP_PORT = 8308; //The port the Velodyne sends positioning data to - static const uint16_t VELODYNE_UPPER_HEADER_BYTES = 0xEEFF; //The byte indicating a upper shot - static const uint16_t VELODYNE_LOWER_HEADER_BYTES = 0xDDFF; //The byte indicating a lower shot - - static const unsigned int VELODYNE_FIRING_ORDER[] = {31, 29, 27, 25, 23, 21, 19, 17, 15, 13, 11, 9, 7, 5, 3, 1, - 30, 28, 26, 24, 22, 20, 18, 16, 14, 12, 10, 8, 6, 4, 2, 0}; - - static const double VELODYNE_VERTICAL_RESOLUTION = 1.0 + 1.0/3.0; // vertical resolution in degree - static const double VELODYNE_VERTICAL_START_ANGLE = -VELODYNE_VERTICAL_RESOLUTION * 8.0; // vertical start angle in degree - static const double VELODYNE_VERTICAL_END_ANGLE = VELODYNE_VERTICAL_RESOLUTION * 23.0; // vertical end angle in degree + /** + * Return mode of the light signal + */ + enum ReturnMode + { + StrongestReturn = 0x37, + LastReturn = 0x38, + DualReturn = 0x39 + }; + + /** + * Sensor type + */ + enum SensorType + { + VELODYNE_HDL64E = 0x20, + VELODYNE_HDL32E = 0x21, + VELODYNE_VLP16 = 0x22 + }; }; #endif diff --git a/src/velodyneDataDriver.cpp b/src/velodyneDataDriver.cpp index cece1ce..a914c62 100644 --- a/src/velodyneDataDriver.cpp +++ b/src/velodyneDataDriver.cpp @@ -10,7 +10,7 @@ using namespace velodyne_lidar; -VelodyneDataDriver::VelodyneDataDriver() : Driver(VELODYNE_DATA_MSG_BUFFER_SIZE) +VelodyneDataDriver::VelodyneDataDriver(SensorType sensor_type, double broadcast_frequency) : Driver(VELODYNE_DATA_MSG_BUFFER_SIZE), sensor_type(sensor_type) { // Confirm that the UDP message buffer matches the structure size assert(sizeof(velodyne_data_packet_t) == VELODYNE_DATA_MSG_BUFFER_SIZE); @@ -19,12 +19,11 @@ VelodyneDataDriver::VelodyneDataDriver() : Driver(VELODYNE_DATA_MSG_BUFFER_SIZE) target_batch_size = 36000; // 360 degree packets_idx = 0; packets_lost = 0; - upper_head.reserve(200); - lower_head.reserve(200); + packets.reserve(200); last_rotational_pos = base::unknown(); last_packet_internal_timestamp = base::unknown(); - timestamp_estimator = new aggregator::TimestampEstimator(base::Time::fromSeconds(20), base::Time::fromSeconds(1.0/VELODYNE_DRIVER_BROADCAST_FREQ_HZ)); - expected_packet_period = 1000000 / VELODYNE_DRIVER_BROADCAST_FREQ_HZ; + timestamp_estimator = new aggregator::TimestampEstimator(base::Time::fromSeconds(20), base::Time::fromSeconds(1.0/broadcast_frequency)); + expected_packet_period = 1000000 / broadcast_frequency; } VelodyneDataDriver::~VelodyneDataDriver() @@ -81,65 +80,9 @@ bool VelodyneDataDriver::isScanComplete() return current_batch_size >= target_batch_size; } -bool VelodyneDataDriver::convertScanToSample(base::samples::DepthMap& sample, LaserHead head, bool copy_remission) -{ - std::vector &packets = head == velodyne_lidar::UpperHead ? upper_head : lower_head; - - if(packets.empty()) - return false; - - // prepare sample - sample.reset(); - unsigned horizontal_steps = packets.size() * VELODYNE_NUM_SHOTS; - unsigned measurement_count = horizontal_steps * VELODYNE_NUM_LASERS; - sample.distances.resize(measurement_count); - if(copy_remission) - sample.remissions.resize(measurement_count); - sample.timestamps.resize(horizontal_steps); - sample.time = packets.back().time[VELODYNE_NUM_SHOTS-1]; - sample.horizontal_size = horizontal_steps; - sample.vertical_size = VELODYNE_NUM_LASERS; - sample.horizontal_interval.resize(horizontal_steps); - sample.vertical_interval.push_back(base::Angle::deg2Rad(VELODYNE_VERTICAL_START_ANGLE)); - sample.vertical_interval.push_back(base::Angle::deg2Rad(VELODYNE_VERTICAL_END_ANGLE)); - sample.horizontal_projection = base::samples::DepthMap::POLAR; - sample.vertical_projection = base::samples::DepthMap::POLAR; - base::samples::DepthMap::DepthMatrixMap distances = sample.getDistanceMatrixMap(); - base::samples::DepthMap::DepthMatrixMap remissions = base::samples::DepthMap::DepthMatrixMap(sample.remissions.data(), sample.vertical_size, sample.horizontal_size); - - for(unsigned i = 0; i < packets.size(); i++) - { - for(unsigned j = 0; j < VELODYNE_NUM_SHOTS; j++) - { - unsigned column = (i * VELODYNE_NUM_SHOTS) + j; - sample.timestamps[column] = packets[i].time[j]; - const velodyne_fire& shot = packets[i].packet.shots[j]; - sample.horizontal_interval[column] = base::Angle::deg2Rad(360.0 - (double)shot.rotational_pos * 0.01); - for(unsigned k = 0; k < VELODYNE_NUM_LASERS; k++) - { - const vel_laser_t &laser = shot.lasers[VELODYNE_FIRING_ORDER[k]]; - if(laser.distance == 0) // zero means no return within max range - distances(k, column) = base::infinity(); - else if(laser.distance <= MIN_SENSING_DISTANCE) // dismiss all values under 1m - distances(k, column) = 0.f; - else - distances(k, column) = (float)laser.distance * 0.002f; // velodyne acquires in 2mm-units - - if(copy_remission) - remissions(k, column) = (float)laser.intensity / 255.0f; - } - } - } - - packets.clear(); - current_batch_size = 0; - return true; -} - void VelodyneDataDriver::clearCurrentScan() { - upper_head.clear(); - lower_head.clear(); + packets.clear(); current_batch_size = 0; } @@ -167,7 +110,7 @@ void VelodyneDataDriver::print_packet(velodyne_data_packet_t &packet) { for (int in = 0; in < 12 ; in++) { - printf("Block %d, %d, %d:\n",in, packet.shots[in].lower_upper, packet.shots[in].rotational_pos); + printf("Block %d, %d, %d:\n",in, packet.shots[in].laser_header, packet.shots[in].rotational_pos); printf("Distance: "); for (int im = 0; im < 32 ; im ++) { @@ -182,7 +125,7 @@ void VelodyneDataDriver::print_packet(velodyne_data_packet_t &packet) } printf("Status: \n"); - std::cout << "Type:"<< (double)packet.status_type << " Value: " <<(double)packet.status_value <<" GPS Timestamp: " << packet.gps_timestamp << std::endl; + std::cout << "Mode:"<< (double)packet.return_mode << " Type: " <<(double)packet.sensor_type <<" GPS Timestamp: " << packet.gps_timestamp << std::endl; base::Time gps_time = base::Time::fromMicroseconds(packet.gps_timestamp); std::cout<< "GPS Time:" << gps_time.toString() << std::endl; printf("\n\n\n\n"); @@ -200,6 +143,14 @@ int VelodyneDataDriver::extractPacket(const uint8_t* buffer, size_t buffer_size) // drop first byte until it is a valid header byte if(buffer[0] != 0xFF) return -1; + + // drop samples from a different sensor head + if(buffer[VELODYNE_DATA_MSG_BUFFER_SIZE-1] != sensor_type) + { + std::cerr << "Received data from a different sensor head!" << std::endl; + return -buffer_size; + } + // found a valid packet return buffer_size; } @@ -209,24 +160,18 @@ int VelodyneDataDriver::extractPacket(const uint8_t* buffer, size_t buffer_size) void VelodyneDataDriver::addNewPacket(const VelodyneDataDriver::StampedDataPacket& packet) { - if(packet.packet.shots[0].lower_upper == VELODYNE_UPPER_HEADER_BYTES) - { - if(!base::isUnknown(last_rotational_pos)) - current_batch_size += packet.packet.shots[0].rotational_pos > last_rotational_pos ? - packet.packet.shots[0].rotational_pos - last_rotational_pos : - packet.packet.shots[0].rotational_pos + (35999 - last_rotational_pos); - for(unsigned i = 1; i < VELODYNE_NUM_SHOTS; i++) - { - current_batch_size += packet.packet.shots[i].rotational_pos > packet.packet.shots[i-1].rotational_pos ? - packet.packet.shots[i].rotational_pos - packet.packet.shots[i-1].rotational_pos : - packet.packet.shots[i].rotational_pos + (35999 - packet.packet.shots[i-1].rotational_pos); - } - last_rotational_pos = packet.packet.shots[VELODYNE_NUM_SHOTS-1].rotational_pos; - - upper_head.push_back(packet); - } - else if(packet.packet.shots[0].lower_upper == VELODYNE_LOWER_HEADER_BYTES) + if(!base::isUnknown(last_rotational_pos)) + current_batch_size += packet.packet.shots[0].rotational_pos > last_rotational_pos ? + packet.packet.shots[0].rotational_pos - last_rotational_pos : + packet.packet.shots[0].rotational_pos + (35999 - last_rotational_pos); + for(unsigned i = 1; i < VELODYNE_NUM_SHOTS; i++) { - lower_head.push_back(packet); + current_batch_size += packet.packet.shots[i].rotational_pos > packet.packet.shots[i-1].rotational_pos ? + packet.packet.shots[i].rotational_pos - packet.packet.shots[i-1].rotational_pos : + packet.packet.shots[i].rotational_pos + (35999 - packet.packet.shots[i-1].rotational_pos); } + last_rotational_pos = packet.packet.shots[VELODYNE_NUM_SHOTS-1].rotational_pos; + + packets.push_back(packet); + } \ No newline at end of file diff --git a/src/velodyneDataDriver.hpp b/src/velodyneDataDriver.hpp index 4db7825..53f63f5 100644 --- a/src/velodyneDataDriver.hpp +++ b/src/velodyneDataDriver.hpp @@ -14,12 +14,6 @@ namespace aggregator namespace velodyne_lidar { -enum LaserHead -{ - LowerHead, - UpperHead -}; - class VelodyneDataDriver : public iodrivers_base::Driver { struct StampedDataPacket @@ -29,7 +23,7 @@ class VelodyneDataDriver : public iodrivers_base::Driver }; public: - VelodyneDataDriver(); + VelodyneDataDriver(SensorType sensor_type, double broadcast_frequency); virtual ~VelodyneDataDriver(); /** @@ -49,7 +43,7 @@ class VelodyneDataDriver : public iodrivers_base::Driver * Also clears all collected packets of the corresponding laser head. * @returns true on success, false if nothing to convert. */ - bool convertScanToSample(base::samples::DepthMap& sample, LaserHead head = UpperHead, bool copy_remission = true); + virtual bool convertScanToSample(base::samples::DepthMap& sample, bool copy_remission = true) = 0; /** * Clears all collected packets. @@ -76,6 +70,11 @@ class VelodyneDataDriver : public iodrivers_base::Driver */ int64_t getPacketReceivedCount(); + /** + * Return frequency of the arriving data samples + */ + virtual double getBroadcastFrequency() = 0; + /** * Prints the packet */ @@ -93,6 +92,7 @@ class VelodyneDataDriver : public iodrivers_base::Driver */ void addNewPacket(const StampedDataPacket& packet); + SensorType sensor_type; uint64_t target_batch_size; uint64_t current_batch_size; uint16_t last_rotational_pos; @@ -101,8 +101,7 @@ class VelodyneDataDriver : public iodrivers_base::Driver int64_t packets_lost; int64_t expected_packet_period; aggregator::TimestampEstimator* timestamp_estimator; - std::vector upper_head; - std::vector lower_head; + std::vector packets; }; }; diff --git a/src/velodyneProtocolTypes.hpp b/src/velodyneProtocolTypes.hpp index 48f2fe3..7528962 100644 --- a/src/velodyneProtocolTypes.hpp +++ b/src/velodyneProtocolTypes.hpp @@ -14,16 +14,16 @@ namespace velodyne_lidar } __attribute__((packed)) vel_laser_t; typedef struct velodyne_fire { - uint16_t lower_upper; + uint16_t laser_header; uint16_t rotational_pos; // 0-35999 divide by 100 for degrees - vel_laser_t lasers[VELODYNE_NUM_LASERS]; + vel_laser_t lasers[VELODYNE_NUM_LASER_CHANNELS]; } __attribute__((packed)) velodyne_fire_t ; typedef struct velodyne_data_packet { velodyne_fire_t shots[VELODYNE_NUM_SHOTS]; uint32_t gps_timestamp; // in microseconds from the top of the hour - uint8_t status_type; - uint8_t status_value; + uint8_t return_mode; + uint8_t sensor_type; velodyne_data_packet() {