add rest timestamp and syncImmediately service

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -289,6 +289,10 @@ class OBCameraNode {
   void getLdpMeasureDistanceCallback(const std::shared_ptr<GetInt32::Request>& request,
                                      std::shared_ptr<GetInt32::Response>& response);
 
+  void setSYNCImmediatelyCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                                  std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
+  void setRESETTimestampCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                                 std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
   void saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
@@ -433,6 +437,8 @@ class OBCameraNode {
   rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_ldp_measure_distance_srv_;
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_immediately_srv_;
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_reset_timestamp_srv_;
 
   bool enable_sync_output_accel_gyro_ = false;
   bool publish_tf_ = false;

orbbec_camera/src/ob_camera_node.cpp

@@ -2077,6 +2077,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       .toImageMsg(*image_msg);
   CHECK_NOTNULL(image_msg.get());
   image_msg->header.stamp = timestamp;
+  RCLCPP_INFO_STREAM(logger_, "timestamp " << frame_timestamp);
   image_msg->is_bigendian = false;
   image_msg->step = width * unit_step_size_[stream_index];
   image_msg->header.frame_id = frame_id;

orbbec_camera/src/ros_service.cpp

@@ -169,6 +169,16 @@ void OBCameraNode::setupCameraCtrlServices() {
                                          std::shared_ptr<GetInt32::Response> response) {
         getLdpMeasureDistanceCallback(request, response);
       });
+  set_sync_immediately_srv_ = node_->create_service<SetBool>(
+      "set_sync_immediately", [this](const std::shared_ptr<SetBool::Request> request,
+                                     std::shared_ptr<SetBool::Response> response) {
+        setSYNCImmediatelyCallback(request, response);
+      });
+  set_reset_timestamp_srv_ = node_->create_service<SetBool>(
+      "set_reset_timestamp", [this](const std::shared_ptr<SetBool::Request> request,
+                                    std::shared_ptr<SetBool::Response> response) {
+        setRESETTimestampCallback(request, response);
+      });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -767,4 +777,43 @@ void OBCameraNode::setIRLongExposureCallback(
     response->success = false;
   }
 }
+
+void OBCameraNode::setSYNCImmediatelyCallback(
+    const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+    std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
+  (void)request;
+  try {
+    TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
+    response->success = true;
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+    response->success = false;
+  } catch (const std::exception& e) {
+    response->message = e.what();
+    response->success = false;
+  } catch (...) {
+    response->message = "unknown error";
+    response->success = false;
+  }
+}
+
+void OBCameraNode::setRESETTimestampCallback(
+    const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+    std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
+  (void)request;
+  try {
+    device_->setBoolProperty(OB_PROP_TIMER_RESET_SIGNAL_BOOL, true);
+    response->success = true;
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+    response->success = false;
+  } catch (const std::exception& e) {
+    response->message = e.what();
+    response->success = false;
+  } catch (...) {
+    response->message = "unknown error";
+    response->success = false;
+  }
+}
+
 }  // namespace orbbec_camera