fix: Update velodyne point stamp calculation (#47)

* Update nebula_common.cpp * Update nebula_common.cpp * Update velodyne_decoder_ros_wrapper.cpp * match AWF hecta degrees in the azimuth
tier4 · Aug 8, 2023 · cb25896 · cb25896
1 parent 0c40a1d
commit cb25896
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 3 deletions.
diff --git a/nebula_common/src/nebula_common.cpp b/nebula_common/src/nebula_common.cpp
@@ -58,9 +58,9 @@ pcl::PointCloud<PointXYZIRADT>::Ptr convertPointXYZIRCAEDTToPointXYZIRADT(
     point.z = p.z;
     point.intensity = p.intensity;
     point.ring = p.channel;
-    point.azimuth = rad2deg(p.azimuth);
+    point.azimuth = rad2deg(p.azimuth) * 100.0;
     point.distance = p.distance;
-    point.time_stamp = static_cast<double>(stamp) + p.time_stamp*10e-9;
+    point.time_stamp = stamp + static_cast<double>(p.time_stamp)*1e-9;
     output_pointcloud->points.emplace_back(point);
   }
 

diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp
@@ -49,6 +49,7 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback(
   std::tuple<nebula::drivers::NebulaPointCloudPtr, double> pointcloud_ts =
     driver_ptr_->ConvertScanToPointcloud(scan_msg);
   nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts);
+  double cloud_stamp = std::get<1>(pointcloud_ts);
   if (pointcloud == nullptr) {
     RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed.");
     return;
@@ -77,7 +78,7 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback(
     aw_points_ex_pub_->get_subscription_count() > 0 ||
     aw_points_ex_pub_->get_intra_process_subscription_count() > 0) {
     const auto autoware_ex_cloud =
-      nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, 0.0);//velodyne drivers originally doesn't use absolute time
+      nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, cloud_stamp);
     auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
     pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr);
     ros_pc_msg_ptr->header.stamp =