feature: dual return

ros-drivers · Mar 29, 2019 · 1efe00d · 1efe00d
1 parent 664aef9
commit 1efe00d
Show file tree

Hide file tree

Showing 16 changed files with 175 additions and 81 deletions.
diff --git a/velodyne_driver/include/velodyne_driver/driver.h b/velodyne_driver/include/velodyne_driver/driver.h
@@ -74,6 +74,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_;
 

diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch
@@ -15,6 +15,7 @@
   <arg name="rpm" default="600.0" />
   <arg name="gps_time" default="false" />
   <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" />
@@ -33,6 +34,7 @@
     <param name="rpm" value="$(arg rpm)"/>
     <param name="gps_time" value="$(arg gps_time)"/>
     <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
@@ -58,6 +58,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") || 
@@ -88,7 +90,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
@@ -50,7 +50,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
@@ -45,7 +45,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
@@ -34,6 +34,7 @@
 #define VELODYNE_POINTCLOUD_DATACONTAINERBASE_H
 
 #include <ros/ros.h>
+#include <velodyne_pointcloud/point_types.h>
 
 namespace velodyne_rawdata
 {
@@ -47,7 +48,8 @@ class DataContainerBase
     const uint16_t& ring,
     const uint16_t& azimuth,
     const float& distance,
-    const float& intensity) = 0;
+    const float& intensity,
+    const uint8_t& return_type) = 0;
 };
 }  // namespace velodyne_rawdata
 

diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h b/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h
@@ -46,22 +46,31 @@
 
 namespace velodyne_pointcloud
 {
+enum ReturnType { 
+  RETURN_UNKNOWN, 
+  RETURN_STRONGEST, 
+  RETURN_LAST 
+};
+
 /** Euclidean Velodyne coordinate, including intensity and ring number. */
-struct PointXYZIR
+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/...ude/velodyne_pointcloud/pointcloudXYZIR.h → ...de/velodyne_pointcloud/pointcloudXYZIRR.h b/...ude/velodyne_pointcloud/pointcloudXYZIR.h → ...de/velodyne_pointcloud/pointcloudXYZIRR.h
@@ -1,3 +1,4 @@
+
 // Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley
 // All rights reserved.
 //
@@ -30,29 +31,32 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#ifndef VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H
-#define VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H
+
+#ifndef VELODYNE_POINTCLOUD_POINTCLOUDXYZIRR_H
+#define VELODYNE_POINTCLOUD_POINTCLOUDXYZIRR_H
 
 #include <velodyne_pointcloud/rawdata.h>
+#include <velodyne_pointcloud/point_types.h>
 
 namespace velodyne_pointcloud
 {
-class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase
+class PointcloudXYZIRR : public velodyne_rawdata::DataContainerBase
 {
-public:
+ public:
   velodyne_rawdata::VPointCloud::Ptr pc;
 
-  PointcloudXYZIR() : pc(new velodyne_rawdata::VPointCloud) {}
+  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 float& x,
+      const float& y,
+      const float& z,
+      const uint16_t& ring,
+      const uint16_t& azimuth,
+      const float& distance,
+      const float& intensity,
+      const uint8_t& return_type);
 };
 }  // namespace velodyne_pointcloud
 
-#endif  // VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H
+#endif  // VELODYNE_POINTCLOUD_POINTCLOUDXYZIRR_H
diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h
@@ -58,7 +58,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;
 
 /**
@@ -73,16 +73,22 @@ static const float ROTATION_RESOLUTION      =     0.01f;  // [deg]
 static const uint16_t ROTATION_MAX_UNITS    = 36000u;     // [deg/100]
 
 /** @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 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_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.
@@ -136,6 +142,14 @@ typedef struct raw_packet
 }
 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
 {
@@ -208,6 +222,36 @@ class RawData
     return (range >= config_.min_range
             && range <= config_.max_range);
   }
+
+  /** Only for vlp16, check factory bytes for return mode **/
+  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 RETURN_STRONGEST;
+      case RETURN_MODE_LAST:
+        return RETURN_LAST;
+      default:
+        return RETURN_UNKNOWN;
+    }
+  }
+
+  static inline velodyne_pointcloud::ReturnType
+  blockIdxToReturnType(const int &block_idx) {
+    using namespace velodyne_pointcloud;
+    if(block_idx % 2 == 0) {
+      return RETURN_LAST;
+    } else {
+      return RETURN_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
@@ -48,7 +48,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>
@@ -104,8 +104,8 @@ class Transform
   // 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
-  velodyne_rawdata::VPointCloud tfPc_;  // transformed packet point cloud
+  PointcloudXYZIRR inPc_;                // input packet point cloud
+  velodyne_rawdata::VPointCloud tfPc_;   // transformed packet point cloud
 
   // diagnostics updater
   diagnostic_updater::Updater diagnostics_;
@@ -115,4 +115,4 @@ class Transform
 };
 }  // namespace velodyne_pointcloud
 
-#endif  // VELODYNE_POINTCLOUD_TRANSFORM_H
+#endif  // VELODYNE_POINTCLOUD_TRANSFORM_H
diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch
@@ -18,6 +18,7 @@
   <arg name="rpm" default="600.0" />
   <arg name="gps_time" default="false" />
   <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" />
 
@@ -35,6 +36,7 @@
     <arg name="rpm" value="$(arg rpm)"/>
     <arg name="gps_time" value="$(arg gps_time)"/>
     <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
@@ -72,7 +72,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/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc
diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc
@@ -0,0 +1,27 @@
+
+
+
+#include <velodyne_pointcloud/point_types.h>
+#include <velodyne_pointcloud/pointcloudXYZIRR.h>
+
+namespace velodyne_pointcloud 
+{
+  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 uint8_t& return_type)
+  {
+    // convert polar coordinates to Euclidean XYZ
+    velodyne_rawdata::VPoint point;
+    point.ring = ring;
+    point.x = x;
+    point.y = y;
+    point.z = z;
+    point.intensity = intensity;
+    point.return_type = return_type;
+
+    // append this point to the cloud
+    pc->points.push_back(point);
+    ++pc->width;
+  }
+}
+