diff --git a/velodyne_driver/include/velodyne_driver/driver.h b/velodyne_driver/include/velodyne_driver/driver.h index 07e21b8c8..59e0370b7 100644 --- 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 index 35a6213aa..b6579fece 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -15,6 +15,7 @@ + @@ -33,6 +34,7 @@ + diff --git a/velodyne_driver/src/driver/driver.cc b/velodyne_driver/src/driver/driver.cc index bed356818..3f0f38ef6 100644 --- 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 index c87770573..bf1e08525 100644 --- 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 VPointCloud; class RingColors diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index 8010f3a42..cd0810559 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h @@ -45,7 +45,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 efccfec46..13e550fd3 100644 --- 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 +#include 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 index 29fb99727..01ea3d349 100644 --- 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/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h similarity index 77% rename from velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h rename to velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIRR.h index 3c8aa24d6..0ffd8c514 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/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 +#include 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 \ No newline at end of file diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 180e6a0a6..7c86f7363 100644 --- 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 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 index ed7a3f4c8..bcf3ee470 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include @@ -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 \ No newline at end of file diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index 30949aa53..f799b3701 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -18,6 +18,7 @@ + @@ -35,6 +36,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 b060ecaa4..a0db5fdc6 100644 --- 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 deleted file mode 100644 index 3720ac626..000000000 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ /dev/null @@ -1,23 +0,0 @@ - - - -#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) - { - // 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; - - // append this point to the cloud - pc->points.push_back(point); - ++pc->width; - } -} - diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc new file mode 100644 index 000000000..4de807857 --- /dev/null +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIRR.cc @@ -0,0 +1,27 @@ + + + +#include +#include + +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; + } +} + diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index e40f71366..2e62e28d5 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -310,7 +310,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::RETURN_UNKNOWN); } } } @@ -332,7 +334,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++) { @@ -349,24 +353,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; } @@ -378,6 +382,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; @@ -493,8 +508,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); } } }