Fix conversions

PRBonn · Nov 15, 2024 · 2807f97 · 2807f97
1 parent 944856e
commit 2807f97
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
@@ -192,9 +192,10 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con
     // Get scan duration and current stamp
     const auto begin_scan_time = min_it != timestamps.cend() ? *min_it : toTime(last_stamp);
     const auto end_scan_time = max_it != timestamps.cend() ? *max_it : toTime(msg->header.stamp);
-    const double scan_duration = end_scan_stamp - begin_scan_stamp;
-    current_stamp_ = begin_scan_time < toTime(last_stamp) ? toStamp(last_stamp_) + scan_duration
-                                                          : toStamp(end_scan_stamp);
+    const double scan_duration = std::abs(end_scan_stamp - begin_scan_stamp);
+    current_stamp_ = begin_scan_time < toTime(last_stamp)
+                         ? toStamp(toTime(last_stamp_) + scan_duration)
+                         : end_scan_stamp;
 
     // Get the initial guess from the wheel odometry
     const auto delta = LookupDeltaTransform(base_frame_, last_stamp_, base_frame_, current_stamp_,