dual return

ros-drivers · Jan 18, 2019 · 1898524 · 1898524
1 parent c458e15
commit 1898524
Show file tree

Hide file tree

Showing 16 changed files with 160 additions and 62 deletions.
diff --git a/velodyne_driver/include/velodyne_driver/driver.h b/velodyne_driver/include/velodyne_driver/driver.h
@@ -58,6 +58,7 @@ class VelodyneDriver
     double rpm;                      ///< device rotation rate (RPMs)
     int cut_angle;                   ///< cutting angle in 1/100°
     double time_offset;              ///< time in seconds added to each velodyne time stamp
+    bool is_dual_return_mode;        ///< is the lidar in dual return mode
   } config_;
 
   boost::shared_ptr<Input> input_;

diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch
@@ -14,6 +14,7 @@
   <arg name="repeat_delay" default="0.0" />
   <arg name="rpm" default="600.0" />
   <arg name="cut_angle" default="-0.01" />
+  <arg name="dual_return_mode" default="false" />
 
   <!-- start nodelet manager -->
   <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" />
@@ -31,6 +32,7 @@
     <param name="repeat_delay" value="$(arg repeat_delay)"/>
     <param name="rpm" value="$(arg rpm)"/>
     <param name="cut_angle" value="$(arg cut_angle)"/>
+    <param name="dual_return_mode" value="$(arg dual_return_mode)" />
   </node>    
 
 </launch>
diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc
@@ -35,6 +35,8 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
 
   // get model name, validate string, determine packet rate
   private_nh.param("model", config_.model, std::string("64E"));
+  private_nh.param("dual_return_mode", config_.is_dual_return_mode,
+    false);
   double packet_rate;                   // packet frequency (Hz)
   std::string model_full_name;
   if ((config_.model == "64E_S2") || 
@@ -60,7 +62,9 @@ VelodyneDriver::VelodyneDriver(ros::NodeHandle node,
     }
   else if (config_.model == "VLP16")
     {
-      packet_rate = 754;             // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual)
+      packet_rate = 753.5;             // 753.5 Packets/Second for Last or
+      // Strongest mode 1508 for dual (VLP-16 User Manual)
+      if(config_.is_dual_return_mode) { packet_rate *= 2.0; }
       model_full_name = "VLP-16";
     }
   else

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/colors.h b/velodyne_pointcloud/include/velodyne_pointcloud/colors.h
@@ -26,7 +26,7 @@
 namespace velodyne_pointcloud
 {
   // shorter names for point cloud types in this namespace
-  typedef velodyne_pointcloud::PointXYZIR VPoint;
+  typedef velodyne_pointcloud::PointXYZIRR VPoint;
   typedef pcl::PointCloud<VPoint> VPointCloud;
 
   class RingColors

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h
@@ -21,7 +21,7 @@
 
 #include <sensor_msgs/PointCloud2.h>
 #include <velodyne_pointcloud/rawdata.h>
-#include <velodyne_pointcloud/pointcloudXYZIR.h>
+#include <velodyne_pointcloud/pointcloudXYZIRR.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <velodyne_pointcloud/CloudNodeConfig.h>

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h
@@ -5,12 +5,17 @@
 
 #include <ros/ros.h>
 
+#include "velodyne_pointcloud/point_types.h"
+
 namespace velodyne_rawdata 
 {
   class DataContainerBase 
   {
   public:
-    virtual void addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity) = 0;
+    virtual void addPoint(const float& x, const float& y, const float& z,
+        const uint16_t& ring, const uint16_t& azimuth, const float& distance,
+        const float& intensity,
+        const velodyne_pointcloud::ReturnType& return_type) = 0;
   };
 }
 #endif //__DATACONTAINERBASE_H

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h b/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h
@@ -23,24 +23,40 @@
 
 namespace velodyne_pointcloud
 {
-  /** Euclidean Velodyne coordinate, including intensity and ring number. */
-  struct PointXYZIR
+  enum class ReturnType { UNKNOWN, STRONGEST, LAST };
+  inline uint8_t returnTypeToU8 (const ReturnType& r) {
+    switch(r) {
+      case ReturnType::STRONGEST:
+        return 1;
+      case ReturnType::LAST:
+        return 2;
+      case ReturnType::UNKNOWN:
+      default:
+        return 0;
+    }
+  }
+
+  /** Euclidean Velodyne coordinate, including intensity, ring number and
+   * return type. */
+  struct PointXYZIRR
   {
     PCL_ADD_POINT4D;                    // quad-word XYZ
     float    intensity;                 ///< laser intensity reading
     uint16_t ring;                      ///< laser ring number
+    uint8_t  return_type;               ///< see enum
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW     // ensure proper alignment
   } EIGEN_ALIGN16;
 
 }; // namespace velodyne_pointcloud
 
 
-POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_pointcloud::PointXYZIR,
+POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_pointcloud::PointXYZIRR,
                                   (float, x, x)
                                   (float, y, y)
                                   (float, z, z)
                                   (float, intensity, intensity)
-                                  (uint16_t, ring, ring))
+                                  (uint16_t, ring, ring)
+                                  (uint8_t,return_type, return_type))
 
 #endif // __VELODYNE_POINTCLOUD_POINT_TYPES_H
 
diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h
diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h
@@ -0,0 +1,23 @@
+
+
+#ifndef __POINTCLOUDXYZIRR_H
+#define __POINTCLOUDXYZIRR_H
+
+#include <velodyne_pointcloud/rawdata.h>
+
+namespace velodyne_pointcloud 
+{
+  class PointcloudXYZIRR : public velodyne_rawdata::DataContainerBase
+  {
+  public:
+    velodyne_rawdata::VPointCloud::Ptr pc;
+
+    PointcloudXYZIRR() : pc(new velodyne_rawdata::VPointCloud) {}
+
+    virtual void addPoint(const float& x, const float& y, const float& z,
+        const uint16_t& ring, const uint16_t& azimuth, const float& distance,
+        const float& intensity, const ReturnType& return_type) override;
+  };
+}
+#endif //__POINTCLOUDXYZIRR_H
+
diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h
@@ -37,7 +37,7 @@
 namespace velodyne_rawdata
 {
     // Shorthand typedefs for point cloud representations
-  typedef velodyne_pointcloud::PointXYZIR VPoint;
+  typedef velodyne_pointcloud::PointXYZIRR VPoint;
   typedef pcl::PointCloud<VPoint> VPointCloud;
 
   /**
@@ -54,14 +54,19 @@ namespace velodyne_rawdata
   /** @todo make this work for both big and little-endian machines */
   static const uint16_t UPPER_BANK = 0xeeff;
   static const uint16_t LOWER_BANK = 0xddff;
-
+  static const uint8_t  RETURN_MODE_STRONGEST = 0x37;
+  static const uint8_t  RETURN_MODE_LAST = 0x38;
+  static const uint8_t  RETURN_MODE_DUAL = 0x39;
 
   /** Special Defines for VLP16 support **/
   static const int    VLP16_FIRINGS_PER_BLOCK =   2;
   static const int    VLP16_SCANS_PER_FIRING  =  16;
   static const float  VLP16_BLOCK_TDURATION   = 110.592f;   // [µs]
   static const float  VLP16_DSR_TOFFSET       =   2.304f;   // [µs]
   static const float  VLP16_FIRING_TOFFSET    =  55.296f;   // [µs]
+  static const int    VLP16_FACTORY_BYTES_SIZE=   2;
+  static const int    VLP16_FACTORY_RETURN_MODE_IDX = 0;
+  static const int    VLP16_FACTORY_PRODUCT_ID_IDX  = 1;
 
 
   /** \brief Raw Velodyne data block.
@@ -110,9 +115,16 @@ namespace velodyne_rawdata
   {
     raw_block_t blocks[BLOCKS_PER_PACKET];
     uint16_t revolution;
-    uint8_t status[PACKET_STATUS_SIZE]; 
+    uint8_t status[PACKET_STATUS_SIZE];
   } raw_packet_t;
 
+  typedef struct raw_packet_vlp16
+  {
+    raw_block_t blocks[BLOCKS_PER_PACKET];
+    uint32_t    timestamp;
+    uint8_t     factory[VLP16_FACTORY_BYTES_SIZE];
+  } raw_packet_vlp16_t;
+
   /** \brief Velodyne data conversion class */
   class RawData
   {
@@ -185,6 +197,35 @@ namespace velodyne_rawdata
       return (range >= config_.min_range
               && range <= config_.max_range);
     }
+
+    static inline bool isDualReturnMode(const raw_packet_vlp16_t* raw) {
+      return raw->factory[VLP16_FACTORY_RETURN_MODE_IDX]
+          == RETURN_MODE_DUAL;
+    }
+
+    static inline velodyne_pointcloud::ReturnType
+    returnModeToReturnType(const raw_packet_vlp16_t* raw) {
+      using namespace velodyne_pointcloud;
+      switch(raw->factory[VLP16_FACTORY_RETURN_MODE_IDX]) {
+        case RETURN_MODE_STRONGEST:
+          return ReturnType::STRONGEST;
+        case RETURN_MODE_LAST:
+          return ReturnType::LAST;
+        default:
+          return ReturnType::UNKNOWN;
+      }
+    }
+
+    static inline velodyne_pointcloud::ReturnType
+    blockIdxToReturnType(const int &block_idx) {
+      using namespace velodyne_pointcloud;
+      if(block_idx % 2 == 0) {
+        return ReturnType::LAST;
+      } else {
+        return ReturnType::STRONGEST; // Or second Strongest?? Should we
+        // distinguish between the two?
+      }
+    }
   };
 
 } // namespace velodyne_rawdata

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h
@@ -24,7 +24,7 @@
 #include <sensor_msgs/PointCloud2.h>
 
 #include <velodyne_pointcloud/rawdata.h>
-#include <velodyne_pointcloud/pointcloudXYZIR.h>
+#include <velodyne_pointcloud/pointcloudXYZIRR.h>
 
 #include <dynamic_reconfigure/server.h>
 #include <velodyne_pointcloud/TransformNodeConfig.h>
@@ -74,7 +74,7 @@ namespace velodyne_pointcloud
     // Point cloud buffers for collecting points within a packet.  The
     // inPc_ and tfPc_ are class members only to avoid reallocation on
     // every message.
-    PointcloudXYZIR inPc_;              ///< input packet point cloud
+    PointcloudXYZIRR inPc_;              ///< input packet point cloud
     velodyne_rawdata::VPointCloud tfPc_;              ///< transformed packet point cloud
   };
 

diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch
@@ -17,6 +17,7 @@
   <arg name="repeat_delay" default="0.0" />
   <arg name="rpm" default="600.0" />
   <arg name="cut_angle" default="-0.01" />
+  <arg name="dual_return_mode" default="true" />
   <arg name="laserscan_ring" default="-1" />
   <arg name="laserscan_resolution" default="0.007" />
 
@@ -33,6 +34,7 @@
     <arg name="repeat_delay" value="$(arg repeat_delay)"/>
     <arg name="rpm" value="$(arg rpm)"/>
     <arg name="cut_angle" value="$(arg cut_angle)"/>
+    <arg name="dual_return_mode" value="$(arg dual_return_mode)" />
   </include>
 
   <!-- start cloud nodelet -->

diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt
@@ -1,11 +1,11 @@
-add_executable(cloud_node cloud_node.cc convert.cc pointcloudXYZIR.cc)
+add_executable(cloud_node cloud_node.cc convert.cc pointcloudXYZIRR.cc)
 add_dependencies(cloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(cloud_node velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
 install(TARGETS cloud_node
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-add_library(cloud_nodelet cloud_nodelet.cc convert.cc pointcloudXYZIR.cc)
+add_library(cloud_nodelet cloud_nodelet.cc convert.cc pointcloudXYZIRR.cc)
 add_dependencies(cloud_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(cloud_nodelet velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
@@ -28,14 +28,14 @@ install(TARGETS ringcolors_nodelet
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 
-add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIR.cc)
+add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIRR.cc)
 add_dependencies(transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(transform_node velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})
 install(TARGETS transform_node
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-add_library(transform_nodelet transform_nodelet.cc transform.cc pointcloudXYZIR.cc)
+add_library(transform_nodelet transform_nodelet.cc transform.cc pointcloudXYZIRR.cc)
 add_dependencies(transform_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(transform_nodelet velodyne_rawdata
                       ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES})

diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc
@@ -59,7 +59,7 @@ namespace velodyne_pointcloud
       return;                                     // avoid much work
 
     // allocate a point cloud with same time and frame ID as raw data
-    PointcloudXYZIR outMsg;
+    PointcloudXYZIRR outMsg;
     // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment
     outMsg.pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp;
     outMsg.pc->header.frame_id = scanMsg->header.frame_id;

diff --git a/...tcloud/src/conversions/pointcloudXYZIR.cc → ...cloud/src/conversions/pointcloudXYZIRR.cc b/...tcloud/src/conversions/pointcloudXYZIR.cc → ...cloud/src/conversions/pointcloudXYZIRR.cc
@@ -1,11 +1,13 @@
 
 
 
-#include <velodyne_pointcloud/pointcloudXYZIR.h>
+#include <velodyne_pointcloud/pointcloudXYZIRR.h>
 
 namespace velodyne_pointcloud 
 {
-  void PointcloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& /*distance*/, const float& intensity) 
+  void PointcloudXYZIRR::addPoint(const float& x, const float& y, const float& z,
+      const uint16_t& ring, const uint16_t& /*azimuth*/, const float& /*distance*/,
+      const float& intensity, const ReturnType& return_type)
   {
     // convert polar coordinates to Euclidean XYZ
     velodyne_rawdata::VPoint point;
@@ -14,6 +16,7 @@ namespace velodyne_pointcloud
     point.y = y;
     point.z = z;
     point.intensity = intensity;
+    point.return_type = returnTypeToU8(return_type);
 
     // append this point to the cloud
     pc->points.push_back(point);