ROS 2: Add option to use the RGB image timestamp for the registered d…

…epth image

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

depth_image_proc/src/register.cpp

@@ -82,6 +82,7 @@ class RegisterNode : public rclcpp::Node
 
   // Parameters
   bool fill_upsampling_holes_;
+  bool use_rgb_timestamp_;  // use source time stamp from RGB camera
 
   void imageCb(
     const Image::ConstSharedPtr & depth_image_msg,
@@ -105,6 +106,7 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
   // Read parameters
   int queue_size = this->declare_parameter<int>("queue_size", 5);
   fill_upsampling_holes_ = this->declare_parameter<bool>("fill_upsampling_holes", false);
+  use_rgb_timestamp_ = this->declare_parameter<bool>("use_rgb_timestamp", false);
 
   // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
   sync_ = std::make_shared<Synchronizer>(
@@ -169,7 +171,8 @@ void RegisterNode::imageCb(
   }
 
   auto registered_msg = std::make_shared<Image>();
-  registered_msg->header.stamp = depth_image_msg->header.stamp;
+  registered_msg->header.stamp =
+    use_rgb_timestamp_ ? rgb_info_msg->header.stamp : depth_image_msg->header.stamp;
   registered_msg->header.frame_id = rgb_info_msg->header.frame_id;
   registered_msg->encoding = depth_image_msg->encoding;