Publish messages using frame timestamp

realsense_ros/CMakeLists.txt

@@ -65,6 +65,7 @@ set(CMAKE_CXX_FLAGS "-fPIE -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector -Wformat
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -100,6 +101,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 
 ament_target_dependencies(${PROJECT_NAME}
+  rcl
   rclcpp
   rclcpp_components
   nav_msgs
@@ -133,6 +135,7 @@ install(
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(rcl REQUIRED)
   find_package(rclcpp REQUIRED)
   find_package(nav_msgs REQUIRED)
   find_package(sensor_msgs REQUIRED)
@@ -162,6 +165,7 @@ if(BUILD_TESTING)
         realsense2
 	${PROJECT_NAME})
       ament_target_dependencies(${target}
+        "rcl"
         "rclcpp"
         "nav_msgs"
         "sensor_msgs"

realsense_ros/include/realsense/rs_base.hpp

@@ -80,6 +80,7 @@ class RealSenseBase
   Result toggleStream(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeResolution(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeFPS(const stream_index_pair & stream, const rclcpp::Parameter & param);
+  rclcpp::Time frameToTime(const rs2::frame & frame);
 
   typedef struct VideoStreamInfo
   {

realsense_ros/package.xml

@@ -21,6 +21,8 @@
   <depend>realsense_msgs</depend>
   <depend>image_transport</depend>
 
+  <build_depend>rcl</build_depend>
+
   <exec_depend>launch_ros</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

realsense_ros/src/rs_base.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <sstream>
+#include "rcl/time.h"
 #include "realsense/rs_base.hpp"
 
 namespace realsense
@@ -457,4 +458,8 @@ Result RealSenseBase::changeFPS(const stream_index_pair & stream, const rclcpp::
   }
   return result;
 }
+
+rclcpp::Time RealSenseBase::frameToTime(const rs2::frame & frame) {
+  return rclcpp::Time(RCL_MS_TO_NS(frame.get_timestamp()));
+}
 }  // namespace realsense

realsense_ros/src/rs_d435.cpp

@@ -60,7 +60,7 @@ RealSenseD435::RealSenseD435(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 void RealSenseD435::publishTopicsCallback(const rs2::frame & frame)
 {
   rs2::frameset frameset = frame.as<rs2::frameset>();
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
   if (enable_[COLOR] && (image_pub_[COLOR]->get_subscription_count() > 0 || camera_info_pub_[COLOR]->get_subscription_count() > 0)){
     auto frame = frameset.get_color_frame();
     publishImageTopic(frame, t);

realsense_ros/src/rs_d435i.cpp

@@ -35,7 +35,7 @@ RealSenseD435I::RealSenseD435I(rs2::context ctx, rs2::device dev, rclcpp::Node &
 
 void RealSenseD435I::publishTopicsCallback(const rs2::frame & frame)
 {
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
   if (frame.is<rs2::frameset>()) {
     RealSenseD435::publishTopicsCallback(frame);
   } else if (frame.is<rs2::motion_frame>()) {

realsense_ros/src/rs_t265.cpp

@@ -42,7 +42,7 @@ RealSenseT265::RealSenseT265(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 
 void RealSenseT265::publishTopicsCallback(const rs2::frame & frame)
 {
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
 
   if (frame.is<rs2::frameset>()) {
     auto frameset = frame.as<rs2::frameset>();