Prevent negative sec and nano in TimeToROSStamp when time is large,

replaced Time functions with standard methods.

ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp

@@ -54,19 +54,7 @@ namespace {
 using milliseconds = std::chrono::milliseconds;
 using seconds = std::chrono::duration<long double>;
 using std::chrono::duration_cast;
-
-// Convert to ros time
-auto TimeToROSStamp(const double &time) {
-    builtin_interfaces::msg::Time stamp;
-    stamp.sec = static_cast<int32_t>(std::floor(time));
-    stamp.nanosec = static_cast<uint32_t>((time - stamp.sec) * 1e9);
-    return stamp;
-};
-double ROSStampToTime(const builtin_interfaces::msg::Time stamp) {
-    double time = static_cast<double>(stamp.sec);
-    time += static_cast<double>(stamp.nanosec) * 1e-9;
-    return time;
-};
+using Time = builtin_interfaces::msg::Time;
 }  // namespace
 
 namespace kinematic_icp_ros {
@@ -199,9 +187,9 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con
     const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
     // Update what is the current stamp of this iteration
     const auto begin_scan_stamp =
-        min_it != timestamps.cend() ? TimeToROSStamp(*min_it) : last_stamp;
+        min_it != timestamps.cend() ? Time{rclcpp::Time(*min_it)} : last_stamp;
     const auto end_scan_stamp =
-        max_it != timestamps.cend() ? TimeToROSStamp(*max_it) : msg->header.stamp;
+        max_it != timestamps.cend() ? Time{rclcpp::Time(*max_it)} : msg->header.stamp;
     current_stamp_ = end_scan_stamp;
     // Get the initial guess from the wheel odometry
     const auto delta =
@@ -219,7 +207,7 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con
     }
 
     // Compute velocities, use the elapsed time between the current msg and the last received
-    const double elapsed_time = ROSStampToTime(end_scan_stamp) - ROSStampToTime(begin_scan_stamp);
+    const double elapsed_time = current_stamp_.nanosec - last_stamp.nanosec;
     const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log();
     const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time;