Merge pull request #6 from rock-drivers/vlp_16_support

added support for the velodyne VLP-16

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

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<base::samples::DepthMap::scalar>();
+                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;
+}

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

src/VelodyneVLP16Driver.cpp

@@ -0,0 +1,116 @@
+#include "VelodyneVLP16Driver.hpp"
+#include <base/Time.hpp>
+
+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<base::samples::DepthMap::scalar>();
+                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<double>();
+            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<base::samples::DepthMap::scalar>();
+                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;
+}

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

src/test_velodyneDriver.cpp

@@ -48,7 +48,7 @@
 #include <stdio.h>
 #include <string>
 
-#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);
 

src/velodyneConstants.hpp

@@ -1,29 +1,42 @@
 #ifndef _VELODYNE_CONSTANTS_HPP_
 #define _VELODYNE_CONSTANTS_HPP_
 
+#include <vector>
+
+
 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