fix timestamps

realsense2_camera/include/base_realsense_node.h

@@ -192,6 +192,7 @@ namespace realsense2_camera
         void enable_devices();
         void setupFilters();
         bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain);
+        uint64_t millisecondsToNanoseconds(double timestamp_ms);
         rclcpp::Time frameSystemTimeSec(rs2::frame frame);
         cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image);
         void clip_depth(rs2::depth_frame depth_frame, float clipping_dist);

realsense2_camera/src/base_realsense_node.cpp

@@ -708,17 +708,35 @@ bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time
     return false;
 }
 
+uint64_t BaseRealSenseNode::millisecondsToNanoseconds(double timestamp_ms)
+{
+        // modf breaks input into an integral and fractional part
+        double int_part_ms, fract_part_ms;
+        fract_part_ms = modf(timestamp_ms, &int_part_ms);
+
+        //convert to ns
+        uint64_t int_part_ns = static_cast<uint64_t>(int_part_ms) * 1000000;
+        uint64_t fract_part_ns = static_cast<uint64_t>(fract_part_ms * 10000000);
+
+        // Convert fract_parts_ns into 6 digits and round it
+        // to be aligned with librealsense get_timestamp API
+        fract_part_ns = (fract_part_ns % 10 > 4) ? (fract_part_ns/10 + 1) : (fract_part_ns/10);
+
+        return int_part_ns + fract_part_ns;
+}
+
 rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame)
 {
+    double timestamp_ms = frame.get_timestamp();
     if (frame.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK)
     {
-        double elapsed_camera_ns = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) * 1e6;
+        double elapsed_camera_ns = millisecondsToNanoseconds(timestamp_ms - _camera_time_base);
 
         /*
         Fixing deprecated-declarations compilation warning.
         Duration(rcl_duration_value_t) is deprecated in favor of 
         static Duration::from_nanoseconds(rcl_duration_value_t)
-        starting from GALAXY.
+        starting from GALACTIC.
         */
 #if defined(FOXY) || defined(ELOQUENT) || defined(DASHING)
         auto duration = rclcpp::Duration(elapsed_camera_ns);
@@ -730,7 +748,7 @@ rclcpp::Time BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame)
     }
     else
     {
-        return rclcpp::Time(frame.get_timestamp() * 1e6);
+        return rclcpp::Time(millisecondsToNanoseconds(timestamp_ms));
     }
 }