From 1898524d773b634caf6252ce4d72e76fc68e2dbb Mon Sep 17 00:00:00 2001 From: Andre Phu-Van Nguyen Date: Fri, 21 Dec 2018 17:30:12 -0500 Subject: [PATCH] dual return --- .../include/velodyne_driver/driver.h | 1 + velodyne_driver/launch/nodelet_manager.launch | 2 + velodyne_driver/src/driver/driver.cc | 6 +- .../include/velodyne_pointcloud/colors.h | 2 +- .../include/velodyne_pointcloud/convert.h | 2 +- .../velodyne_pointcloud/datacontainerbase.h | 7 +- .../include/velodyne_pointcloud/point_types.h | 24 +++++-- .../velodyne_pointcloud/pointcloudXYZIR.h | 21 ------ .../velodyne_pointcloud/pointcloudXYZIRR.h | 23 +++++++ .../include/velodyne_pointcloud/rawdata.h | 47 +++++++++++++- .../include/velodyne_pointcloud/transform.h | 4 +- .../launch/VLP16_points.launch | 2 + .../src/conversions/CMakeLists.txt | 8 +-- .../src/conversions/convert.cc | 2 +- ...pointcloudXYZIR.cc => pointcloudXYZIRR.cc} | 7 +- velodyne_pointcloud/src/lib/rawdata.cc | 64 +++++++++++++------ 16 files changed, 160 insertions(+), 62 deletions(-) delete mode 100644 velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h create mode 100644 velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h rename velodyne_pointcloud/src/conversions/{pointcloudXYZIR.cc => pointcloudXYZIRR.cc} (50%) diff --git a/velodyne_driver/include/velodyne_driver/driver.h b/velodyne_driver/include/velodyne_driver/driver.h index b0ff26bf9..e0750e485 100644 --- 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_; diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch index 44eb60c4a..c75b9927c 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -14,6 +14,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index ca6bf6a18..7c7ebf870 100644 --- 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 index a88e308ef..dc9f19742 100644 --- 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 VPointCloud; class RingColors diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index ae394c6d8..f0a056eab 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index 9ed7b41cc..c00779d00 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -5,12 +5,17 @@ #include +#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 index 75a75e280..32ae21305 100644 --- 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 deleted file mode 100644 index cf86e268d..000000000 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ /dev/null @@ -1,21 +0,0 @@ - - -#ifndef __POINTCLOUDXYZIR_H -#define __POINTCLOUDXYZIR_H - -#include - -namespace velodyne_pointcloud -{ - class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase - { - public: - velodyne_rawdata::VPointCloud::Ptr pc; - - PointcloudXYZIR() : 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); - }; -} -#endif //__POINTCLOUDXYZIR_H - diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h new file mode 100644 index 000000000..a5a5586aa --- /dev/null +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h @@ -0,0 +1,23 @@ + + +#ifndef __POINTCLOUDXYZIRR_H +#define __POINTCLOUDXYZIRR_H + +#include + +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 index 5243703a6..0815257ee 100644 --- 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 VPointCloud; /** @@ -54,7 +54,9 @@ 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; @@ -62,6 +64,9 @@ namespace velodyne_rawdata 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 index 73e5717c5..6d538da9a 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include @@ -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 index 1234280f8..090fc44f6 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -17,6 +17,7 @@ + @@ -33,6 +34,7 @@ + diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt index 8712d32db..0eaef824f 100644 --- 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 index d0b29cc0a..98887b4b0 100644 --- 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/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc similarity index 50% rename from velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc rename to velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc index 3720ac626..25fb7a4f7 100644 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc @@ -1,11 +1,13 @@ -#include +#include 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); diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index cc9288f90..176cac45c 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -305,7 +305,9 @@ inline float SQR(float val) { return val*val; } intensity = (intensity < min_intensity) ? min_intensity : intensity; intensity = (intensity > max_intensity) ? max_intensity : intensity; - data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, raw->blocks[i].rotation, distance, intensity); + data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, + raw->blocks[i].rotation, distance, intensity, + velodyne_pointcloud::ReturnType::UNKNOWN); } } } @@ -327,7 +329,9 @@ inline float SQR(float val) { return val*val; } float x, y, z; float intensity; - const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0]; + const raw_packet_vlp16_t *raw = (const raw_packet_vlp16_t *) &pkt.data[0]; + + bool is_dual_return = isDualReturnMode(raw); for (int block = 0; block < BLOCKS_PER_PACKET; block++) { @@ -344,24 +348,24 @@ inline float SQR(float val) { return val*val; } // Calculate difference between current and next block's azimuth angle. azimuth = (float)(raw->blocks[block].rotation); if (block < (BLOCKS_PER_PACKET-1)){ - raw_azimuth_diff = raw->blocks[block+1].rotation - raw->blocks[block].rotation; + raw_azimuth_diff = raw->blocks[block+1].rotation - raw->blocks[block].rotation; azimuth_diff = (float)((36000 + raw_azimuth_diff)%36000); - // some packets contain an angle overflow where azimuth_diff < 0 - if(raw_azimuth_diff < 0)//raw->blocks[block+1].rotation - raw->blocks[block].rotation < 0) - { - ROS_WARN_STREAM_THROTTLE(60, "Packet containing angle overflow, first angle: " << raw->blocks[block].rotation << " second angle: " << raw->blocks[block+1].rotation); - // if last_azimuth_diff was not zero, we can assume that the velodyne's speed did not change very much and use the same difference - if(last_azimuth_diff > 0){ - azimuth_diff = last_azimuth_diff; - } - // otherwise we are not able to use this data - // TODO: we might just not use the second 16 firings - else{ - continue; - } - } - last_azimuth_diff = azimuth_diff; - }else{ + // some packets contain an angle overflow where azimuth_diff < 0 + if(raw_azimuth_diff < 0)//raw->blocks[block+1].rotation - raw->blocks[block].rotation < 0) + { + ROS_WARN_STREAM_THROTTLE(60, "Packet containing angle overflow, first angle: " << raw->blocks[block].rotation << " second angle: " << raw->blocks[block+1].rotation); + // if last_azimuth_diff was not zero, we can assume that the velodyne's speed did not change very much and use the same difference + if(last_azimuth_diff > 0){ + azimuth_diff = last_azimuth_diff; + } + // otherwise we are not able to use this data + // TODO: we might just not use the second 16 firings + else{ + continue; + } + } + last_azimuth_diff = azimuth_diff; + } else { azimuth_diff = last_azimuth_diff; } @@ -373,6 +377,17 @@ inline float SQR(float val) { return val*val; } union two_bytes tmp; tmp.bytes[0] = raw->blocks[block].data[k]; tmp.bytes[1] = raw->blocks[block].data[k+1]; + // If we are in dual return mode, check if this channel data is + // equal to the previous block data to prevent duplication of + // points in the cloud + if (is_dual_return && (block % 2 == 1)) { + // TODO is 3 equalities slower than agregate bytes and subtraction? + if((raw->blocks[block].data[k] == raw->blocks[block-1].data[k]) && + (raw->blocks[block].data[k+1] == raw->blocks[block-1].data[k+1]) && + (raw->blocks[block].data[k+2] == raw->blocks[block-1].data[k+2])) { + continue; + } + } float distance = tmp.uint * calibration_.distance_resolution_m; distance += corrections.dist_correction; @@ -488,8 +503,15 @@ inline float SQR(float val) { return val*val; } SQR(1 - tmp.uint/65535))); intensity = (intensity < min_intensity) ? min_intensity : intensity; intensity = (intensity > max_intensity) ? max_intensity : intensity; - - data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity); + + velodyne_pointcloud::ReturnType return_type; + if(is_dual_return) { + return_type = blockIdxToReturnType(block); + } else { + return_type = returnModeToReturnType(raw); + } + data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, + azimuth_corrected, distance, intensity, return_type); } } }