diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index 3e04715d3..86617601a 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h @@ -88,6 +88,12 @@ class Convert Config config_; bool first_rcfg_call; + // Additional parameters for checking azimuth differences between consecutive data blocks + velodyne_msgs::VelodyneScanPtr adjusted_vel_msg_ptr {new velodyne_msgs::VelodyneScan}; + int last_azimuth_; + int current_azimuth_diff_; + int prev_azimuth_diff_; + // diagnostics updater diagnostic_updater::Updater diagnostics_; double diag_min_freq_; diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index df387bf30..ed5471a5c 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -39,6 +39,7 @@ #include #include #include +#include namespace velodyne_rawdata { @@ -113,6 +114,7 @@ class DataContainerBase cloud.width = config_.init_width; cloud.height = config_.init_height; cloud.is_dense = static_cast(config_.is_dense); + is_newscan = false; } virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance, @@ -150,6 +152,49 @@ class DataContainerBase sensor_msgs::PointCloud2 cloud; + bool is_newscan; + int last_azimuth_corrected; + int current_corrected_azimuth_diff; + int prev_corrected_azimuth_diff; + + // Struct for residual points + struct RPoint + { + float x; + float y; + float z; + uint16_t ring; + uint16_t azimuth; + float distance; + float intensity; + }; + + std::vector residual_cloud_; + + void addResidualPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance, + const float intensity) + { + RPoint point = {x, y, z, ring, azimuth, distance, intensity}; + residual_cloud_.push_back(point); + } + + void offloadResidualPoint() + { + // Add residual points from the previous scan + for (size_t p = 0; p < residual_cloud_.size(); ++p) + { + addPoint(residual_cloud_[p].x, residual_cloud_[p].y, residual_cloud_[p].z, + residual_cloud_[p].ring, residual_cloud_[p].azimuth, residual_cloud_[p].distance, + residual_cloud_[p].intensity); + } + residual_cloud_.clear(); + } + + void addOffsetDuration(ros::Duration offset) + { + cloud.header.stamp = cloud.header.stamp + offset; + } + inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3f& eigen_vec) { eigen_vec(0) = tf_vec[0]; diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index 73616b607..ec1d5ce6a 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -51,6 +51,14 @@ namespace velodyne_pointcloud data_->scansPerPacket())); } + // Initilize all parameters used for data block azimuth difference calculation + last_azimuth_ = -1; + current_azimuth_diff_ = -1; + prev_azimuth_diff_ = -1; + + container_ptr_->last_azimuth_corrected = -1; + container_ptr_->current_corrected_azimuth_diff = -1; + container_ptr_->prev_corrected_azimuth_diff = -1; // advertise output point cloud (before subscribing to input data) output_ = @@ -131,11 +139,56 @@ namespace velodyne_pointcloud // process each packet provided by the driver for (size_t i = 0; i < scanMsg->packets.size(); ++i) { - data_->unpack(scanMsg->packets[i], *container_ptr_); + velodyne_msgs::VelodynePacket tmp_packet = scanMsg->packets[i]; + + // Extract base rotation of first block in packet + std::size_t azimuth_data_pos = 100*0+2; + int azimuth = *( (u_int16_t*) (&tmp_packet.data[azimuth_data_pos])); + + //if first packet in scan, there is no "valid" last_azimuth_ + if (last_azimuth_ == -1) + { + last_azimuth_ = azimuth; + continue; + } + if (current_azimuth_diff_ == -1) + { + current_azimuth_diff_ = azimuth - last_azimuth_; + continue; + } + if (prev_azimuth_diff_ == -1) + { + prev_azimuth_diff_ = current_azimuth_diff_; + continue; + } + current_azimuth_diff_ = azimuth - last_azimuth_; + + adjusted_vel_msg_ptr->packets.push_back(tmp_packet); + + if ((current_azimuth_diff_ < 0) && (prev_azimuth_diff_ >= 0)) + { + // Current and previous azimuth differences have the same sign indicating the current packet was taken at 0 deg + adjusted_vel_msg_ptr->header.stamp = tmp_packet.stamp; + + // allocate a point cloud with same time and frame ID as raw data + container_ptr_->setup(adjusted_vel_msg_ptr); + container_ptr_->offloadResidualPoint(); + + // process each packet + for (size_t j = 0; j < adjusted_vel_msg_ptr->packets.size(); ++j) + { + data_->unpack(adjusted_vel_msg_ptr->packets[j], *container_ptr_); + } + + // Clear all the converted packets + adjusted_vel_msg_ptr->packets.clear(); + } + last_azimuth_ = azimuth; + prev_azimuth_diff_ = current_azimuth_diff_; } // publish the accumulated cloud message - diag_topic_->tick(scanMsg->header.stamp); + diag_topic_->tick(adjusted_vel_msg_ptr->header.stamp); diagnostics_.update(); output_.publish(container_ptr_->finishCloud()); } diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index d8a561973..487e76124 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -379,7 +379,23 @@ inline float SQR(float val) { return val*val; } /** correct for the laser rotation as a function of timing during the firings **/ azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION); azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000; - + + // Calculate azimuth difference of three consecutive scan points + if (data.last_azimuth_corrected == -1) + { + data.last_azimuth_corrected = azimuth_corrected; + } + else if (data.current_corrected_azimuth_diff == -1) + { + data.current_corrected_azimuth_diff = azimuth_corrected - data.last_azimuth_corrected; + } + else if (data.prev_corrected_azimuth_diff == -1) + { + data.prev_corrected_azimuth_diff = data.current_corrected_azimuth_diff; + } + + data.current_corrected_azimuth_diff = azimuth_corrected - data.last_azimuth_corrected; + /*condition added to avoid calculating points which are not in the interesting defined area (min_angle < area < max_angle)*/ if ((azimuth_corrected >= config_.min_angle @@ -484,8 +500,26 @@ 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); + + if ((data.current_corrected_azimuth_diff < 0) && (data.prev_corrected_azimuth_diff) >= 0) + { + data.is_newscan = true; + } + data.last_azimuth_corrected = azimuth_corrected; + data.prev_corrected_azimuth_diff = data.current_corrected_azimuth_diff; + + if (data.is_newscan) + { + data.addResidualPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity); + } + else + { + data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity); + // Update timestamp + ros::Duration first_block_time_offset(0, block*((dsr * VLP16_DSR_TOFFSET) + + (firing * VLP16_FIRING_TOFFSET))); // [µs] + data.addOffsetDuration(first_block_time_offset); + } } } data.newLine();