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;