Merge pull request #2236 from doronhi/ros2-beta-fix_metadata_timestamp

Ros2-beta: fix metadata timestamp

README.md

@@ -3,11 +3,6 @@ These are packages for using Intel RealSense cameras (D400 and L500 series, SR30
 
 LibRealSense supported version: v2.50.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases))
 
-## Beta Version
-This version is meant to ne more dynamic than the 3.x.x versions in controlling the camera parameters. It allows starting and stopping separate sensors and separate postprocessing blocks in runtime. This makes it much simpler to find out the best configuration. All parameters can also be defined at start-up by modifying the launch files, as is in the 3.x.x version.
-While many of the parameter names remain as they were, many were slightly modified to match the new hirarchy. For instance, in the D400 series, the depth, infrared1 and infrared2 streams belong to one stereo sensor. Therefore, there are no longer `infra1_width`, `infra2_width` and `depth_width` parameters, which should have always been set to the same value, but a single, `depth_module.profile` parameter that combines width, height and fps and can also be changed in runtime.
-The parameters `enable_infra1`, `enable_infra2` and `enable_depth` can now be set in runtime.
-
 ## Installation Instructions
 This version supports ROS2 Dashing, Foxy and Rolling.
 
@@ -78,7 +73,7 @@ ros2 run realsense2_camera realsense2_camera_node --ros-args -p enable_color:=fa
 or, with a launch file:
 ```bash
 ros2 launch realsense2_camera rs_launch.py
-ros2 launch realsense2_camera rs_launch.py pointcloud.enable:=true
+ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true
 ```
 
 This will stream all camera sensors and publish on the appropriate ROS topics.

realsense2_camera/include/base_realsense_node.h

@@ -201,7 +201,7 @@ namespace realsense2_camera
                           const std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>& info_publishers,
                           const std::map<stream_index_pair, image_transport::Publisher>& image_publishers,
                           const bool is_publishMetadata = true);
-        void publishMetadata(rs2::frame f, const std::string& frame_id);
+        void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id);
 
         sensor_msgs::msg::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data);
 

realsense2_camera/scripts/echo_metadada.py

@@ -10,6 +10,7 @@
 def metadata_cb(msg):
     aa = json.loads(msg.json_data)
     os.system('clear')
+    print('header:\nstamp:\n  secs:', msg.header.stamp.sec, '\n  nsecs:',  msg.header.stamp.nanosec)
     print('\n'.join(['%10s:%-10s' % (key, str(value)) for key, value in aa.items()]))
 
 def main():

realsense2_camera/src/base_realsense_node.cpp

@@ -370,10 +370,9 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame)
                 rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain()));
 
     auto stream_index = (stream == GYRO.first)?GYRO:ACCEL;
+    rclcpp::Time t(frameSystemTimeSec(frame));
     if (0 != _imu_publishers[stream_index]->get_subscription_count())
     {
-        rclcpp::Time t(frameSystemTimeSec(frame));
-
         auto imu_msg = sensor_msgs::msg::Imu();
         ImuMessage_AddDefaultValues(imu_msg);
         imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index);
@@ -395,7 +394,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame)
         _imu_publishers[stream_index]->publish(imu_msg);
         ROS_DEBUG("Publish %s stream", ros_stream_to_string(frame.get_profile().stream_type()));
     }
-    publishMetadata(frame, OPTICAL_FRAME_ID(stream_index));
+    publishMetadata(frame, t, OPTICAL_FRAME_ID(stream_index));
 }
 
 void BaseRealSenseNode::pose_callback(rs2::frame frame)
@@ -1040,22 +1039,21 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t,
     }
     if (is_publishMetadata)
     {
-        publishMetadata(f, OPTICAL_FRAME_ID(stream));
+        publishMetadata(f, t, OPTICAL_FRAME_ID(stream));
     }
 }
 
-void BaseRealSenseNode::publishMetadata(rs2::frame f, const std::string& frame_id)
+void BaseRealSenseNode::publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id)
 {
     stream_index_pair stream = {f.get_profile().stream_type(), f.get_profile().stream_index()};    
     if (_metadata_publishers.find(stream) != _metadata_publishers.end())
     {
-        rclcpp::Time t(frameSystemTimeSec(f));    
         auto& md_publisher = _metadata_publishers.at(stream);
         if (0 != md_publisher->get_subscription_count())
         {
             realsense2_camera_msgs::msg::Metadata msg;
             msg.header.frame_id = frame_id;
-            msg.header.stamp = t;
+            msg.header.stamp = header_time;
             std::stringstream json_data;
             const char* separator = ",";
             json_data << "{";