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);
}
}
}