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

@@ -34,7 +34,6 @@
 #include "kinematic_icp_ros/utils/RosUtils.hpp"
 
 // ROS 2 headers
-#include <tf2/time.h>
 #include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_broadcaster.h>
 
@@ -54,19 +53,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 +186,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 +206,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 = rclcpp::Time(current_stamp_).nanoseconds() - rclcpp::Time(last_stamp).nanoseconds();
     const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log();
     const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time;