Merge pull request #5 from rock-drivers/revised

Revised version of the driver

manifest.xml

@@ -9,6 +9,7 @@
   <logo>http://</logo>
 
   <depend package="drivers/iodrivers_base" />
+  <depend package="drivers/aggregator" />
   <depend package="base/types" />
   <depend package="gui/vizkit3d" optional="1" />
 

src/CMakeLists.txt

@@ -1,7 +1,7 @@
 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
-    DEPS_PKGCONFIG base-lib iodrivers_base)
+    DEPS_PKGCONFIG base-lib iodrivers_base aggregator)
 
 rock_executable(velodyne_lidar_bin test_velodyneDriver.cpp
     DEPS velodyne_lidar)

src/velodyneConstants.hpp

@@ -6,12 +6,12 @@ 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_ORIENTATION_READINGS = 3; // The number of orientation readings
-    static const unsigned int MIN_SENSING_DISTANCE = 1000; //2m in 2mm units
-    static const unsigned int MAX_SENSING_DISTANCE = 24000; //1200m in 2mm units
-    static const unsigned int VELODYNE_DATA_MSG_BUFFER_SIZE = 1206; //The sides of a data packet
-    static const unsigned int VELODYNE_POSITIONING_MSG_BUFFER_SIZE = 512; //The sides of a positioning packet
+    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 = 50.0; //The rate of broadcast packets
+    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
 

src/velodyneDataDriver.cpp

@@ -1,80 +1,166 @@
-// Software License Agreement (BSD License)
-//
-// Copyright (c) 2008, Tully Foote
-// All rights reserved.
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions
-// are met:
-//
-//  * Redistributions of source code must retain the above copyright
-//    notice, this list of conditions and the following disclaimer.
-//  * Redistributions in binary form must reproduce the above
-//    copyright notice, this list of conditions and the following
-//    disclaimer in the documentation and/or other materials provided
-//    with the distribution.
-//  * Neither the name of the Willow Garage nor the names of its
-//    contributors may be used to endorse or promote products derived
-//    from this software without specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-
 #include <iostream>
 #include <string.h>
 #include <stdio.h>
 #include <assert.h>
-
+#include <sys/ioctl.h>
+#include <sys/errno.h>
+#include <sys/socket.h>
+#include <aggregator/TimestampEstimator.hpp>
 #include "velodyneDataDriver.hpp"
 
 using namespace velodyne_lidar;
 
 VelodyneDataDriver::VelodyneDataDriver() : Driver(VELODYNE_DATA_MSG_BUFFER_SIZE) 
 {
-  // Confirm that the UDP message buffer matches the structure size
-  assert(sizeof(velodyne_data_packet_t) == VELODYNE_DATA_MSG_BUFFER_SIZE);
+    // Confirm that the UDP message buffer matches the structure size
+    assert(sizeof(velodyne_data_packet_t) == VELODYNE_DATA_MSG_BUFFER_SIZE);
+
+    current_batch_size = 0;
+    target_batch_size = 36000; // 360 degree
+    packets_idx = 0;
+    packets_lost = 0;
+    upper_head.reserve(200);
+    lower_head.reserve(200);
+    last_rotational_pos = base::unknown<uint16_t>();
+    last_packet_internal_timestamp = base::unknown<uint32_t>();
+    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;
+}
+
+VelodyneDataDriver::~VelodyneDataDriver()
+{
+    delete timestamp_estimator;
 }
 
-void VelodyneDataDriver::collectColumn(const velodyne_fire_t& velodyne_fire, Eigen::Matrix<base::samples::DepthMap::scalar, Eigen::Dynamic, Eigen::Dynamic> &distances, Eigen::Matrix<base::samples::DepthMap::scalar, Eigen::Dynamic, Eigen::Dynamic> &remissions, unsigned int col, bool useRemissions)
+bool VelodyneDataDriver::readNewPacket()
 {
-	if(distances.cols() <= col)
-		distances.conservativeResize(VELODYNE_NUM_LASERS, col+1);
-
-	if(useRemissions && remissions.cols() <= col)
-		remissions.conservativeResize(VELODYNE_NUM_LASERS, col+1);
-
-	for(unsigned i = 0; i < VELODYNE_NUM_LASERS; i++)
+    if(getFileDescriptor() == INVALID_FD)
+         iodrivers_base::UnixError("Invalid file descriptor!");
+    try
+    {
+        StampedDataPacket data_packet;
+        if(readPacket((uint8_t*)&data_packet.packet, VELODYNE_DATA_MSG_BUFFER_SIZE) == VELODYNE_DATA_MSG_BUFFER_SIZE)
+        {
+            timeval tv;
+            if(ioctl(getFileDescriptor(), SIOCGSTAMP, &tv) >= 0)
+            {
+                base::Time receive_time = base::Time::fromSeconds(tv.tv_sec, tv.tv_usec);
+
+                if(!base::isUnknown<uint32_t>(last_packet_internal_timestamp))
+                {
+                    uint32_t current_period = data_packet.packet.gps_timestamp - last_packet_internal_timestamp;
+                    //check for wrap around
+                    if(last_packet_internal_timestamp > data_packet.packet.gps_timestamp)
+                        current_period = (std::numeric_limits<uint32_t>::max() - last_packet_internal_timestamp) + data_packet.packet.gps_timestamp;
+                    int64_t new_packets = (int64_t)((double)current_period / (double)expected_packet_period + 0.5);
+                    if(new_packets > 1)
+                        packets_lost += new_packets - 1;
+                    packets_idx += new_packets;
+                }
+                last_packet_internal_timestamp = data_packet.packet.gps_timestamp;
+                base::Time packet_timestamp = timestamp_estimator->update(receive_time, packets_idx);
+                base::Time time_between_shots = timestamp_estimator->getPeriod() * (1./(double)VELODYNE_NUM_SHOTS);
+                for(unsigned i = 1; i <= VELODYNE_NUM_SHOTS; i++)
+                    data_packet.time[i-1] = packet_timestamp - time_between_shots * (VELODYNE_NUM_SHOTS-i);
+
+                addNewPacket(data_packet);
+                return true;
+            }
+            throw iodrivers_base::UnixError("Failed to receive socket timestamp of the last packet passed to the user! " + std::string(strerror(errno)));
+        }
+    }
+    catch(const iodrivers_base::TimeoutError& e)
     {
-		const vel_laser_t &velodyne_laser = velodyne_fire.lasers[VELODYNE_FIRING_ORDER[i]];
-		if(velodyne_laser.distance <= MIN_SENSING_DISTANCE/2 && velodyne_laser.distance > 0) // dismiss all values under 1m
-			distances(i, col) = 0;
-		else if(velodyne_laser.distance == 0) // zero means no return within max range
-			distances(i, col) = base::infinity<base::samples::DepthMap::scalar>();
-		else
-			distances(i, col) = velodyne_laser.distance * 0.002; // velodyne acquires in 2mm-units, divide by 1000 to get meters
-
-		if(useRemissions)
-			remissions(i, col) = static_cast<float>(velodyne_laser.intensity) / 255.0f; // set remission because of valid distance
-	}
+        // this is an expected case
+    }
+    return false;
 }
 
+bool VelodyneDataDriver::isScanComplete()
+{
+    return current_batch_size >= target_batch_size;
+}
 
-int VelodyneDataDriver::extractPacket(const uint8_t* buffer, size_t buffer_size) const
+bool VelodyneDataDriver::convertScanToSample(base::samples::DepthMap& sample, LaserHead head, bool copy_remission)
 {
-    if(buffer_size == VELODYNE_DATA_MSG_BUFFER_SIZE)
-        return buffer_size;
-
-    return -buffer_size;
+    std::vector<StampedDataPacket> &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<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;
+}
+
+void VelodyneDataDriver::clearCurrentScan()
+{
+    upper_head.clear();
+    lower_head.clear();
+    current_batch_size = 0;
+}
+
+double VelodyneDataDriver::getScanSize()
+{
+    return base::Angle::deg2Rad((double)target_batch_size * 0.01);
+}
+
+void VelodyneDataDriver::setScanSize(double angular_size)
+{
+    target_batch_size = (uint64_t)(base::Angle::rad2Deg(std::abs(angular_size)) * 100.0);
+}
+
+int64_t VelodyneDataDriver::getPacketLostCount()
+{
+    return packets_lost;
+}
+
+int64_t VelodyneDataDriver::getPacketReceivedCount()
+{
+    return packets_idx - packets_lost;
 }
 
 void VelodyneDataDriver::print_packet(velodyne_data_packet_t &packet) 
@@ -96,8 +182,51 @@ 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 << "Type:"<< (double)packet.status_type  << " Value: " <<(double)packet.status_value  <<" 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");
 }
+
+int VelodyneDataDriver::extractPacket(const uint8_t* buffer, size_t buffer_size) const
+{
+    if(buffer_size < VELODYNE_DATA_MSG_BUFFER_SIZE)
+    {
+        // wait for complete packet
+        return 0;
+    }
+    else if(buffer_size == VELODYNE_DATA_MSG_BUFFER_SIZE)
+    {
+        // drop first byte until it is a valid header byte
+        if(buffer[0] != 0xFF)
+            return -1;
+        // found a valid packet
+        return buffer_size;
+    }
+    // drop buffer
+    return -buffer_size;
+}
+
+void VelodyneDataDriver::addNewPacket(const VelodyneDataDriver::StampedDataPacket& packet)
+{
+    if(packet.packet.shots[0].lower_upper == VELODYNE_UPPER_HEADER_BYTES)
+    {
+        if(!base::isUnknown<uint16_t>(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)
+    {
+        lower_head.push_back(packet);
+    }
+}