Locus service trigger (#4)

I wanted to setup this PR to start getting some feedback. Need a bit
more cleanup and corner cases check, but I can trigger and get images in
service response.

I have few more things to add on thread safety but any suggestions are
welcome.

---------

Co-authored-by: Bence Magyar <[email protected]>

Author: asainilocus

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -48,6 +48,7 @@
 
 #include "orbbec_camera_msgs/msg/device_info.hpp"
 #include "orbbec_camera_msgs/srv/get_device_info.hpp"
+#include "orbbec_camera_msgs/srv/camera_trigger.hpp"
 #include "orbbec_camera_msgs/msg/extrinsics.hpp"
 #include "orbbec_camera_msgs/msg/metadata.hpp"
 #include "orbbec_camera_msgs/msg/imu_info.hpp"
@@ -111,6 +112,7 @@ using SetBool = std_srvs::srv::SetBool;
 using GetBool = orbbec_camera_msgs::srv::GetBool;
 using SetFilter = orbbec_camera_msgs::srv::SetFilter;
 using SetArrays = orbbec_camera_msgs::srv::SetArrays;
+using CameraTrigger = orbbec_camera_msgs::srv::CameraTrigger;
 
 typedef std::pair<ob_stream_type, int> stream_index_pair;
 
@@ -313,8 +315,10 @@ class OBCameraNode {
                          std::shared_ptr<SetFilter ::Response>& response);
   void setSYNCHostimeCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                               std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
-  void sendSoftwareTriggerCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
-                                   std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
+
+  void resetCaptureServiceVariables();
+  void sendSoftwareTriggerCallback(const std::shared_ptr<CameraTrigger::Request>& request,
+                                   std::shared_ptr<CameraTrigger::Response>& response);
 
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
@@ -495,8 +499,24 @@ class OBCameraNode {
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_reset_timestamp_srv_;
   rclcpp::Service<SetInt32>::SharedPtr set_interleaver_laser_sync_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_host_time_srv_;
-  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr send_software_trigger_srv_;
+  rclcpp::Service<CameraTrigger>::SharedPtr send_service_trigger_srv_;
   rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
+  rclcpp::Service<CameraTrigger>::SharedPtr capture_camera_images_srv_;
+
+
+  std::atomic_bool service_capture_started_{false};
+  std::atomic_int number_of_rgb_frames_captured_{0};
+  std::atomic_int number_of_depth_frames_captured_{0};
+  std::mutex service_capture_lock_;
+  std::condition_variable service_capture_cv_;
+  sensor_msgs::msg::CameraInfo color_image_camera_info_;
+  sensor_msgs::msg::CameraInfo depth_image_camera_info_;
+  sensor_msgs::msg::Image::UniquePtr color_image_;
+  sensor_msgs::msg::Image::UniquePtr depth_image_;
+  sensor_msgs::msg::Image::UniquePtr undistorted_color_images_;
+
+  // timeout in ms
+  float service_capture_time_out_ = 1000;
 
   bool enable_sync_output_accel_gyro_ = false;
   bool publish_tf_ = false;
@@ -585,6 +605,7 @@ class OBCameraNode {
   int trigger2image_delay_us_ = 0;
   int trigger_out_delay_us_ = 0;
   bool trigger_out_enabled_ = false;
+  bool service_trigger_enabled_ = false;
   bool software_trigger_enabled_ = false;
   int frames_per_trigger_ = 2;
   bool enable_ptp_config_ = false;

orbbec_camera/launch/gemini_330_series.launch.py

@@ -85,6 +85,7 @@ def generate_launch_description():
         DeclareLaunchArgument('color_ae_roi_top', default_value='-1'),
         DeclareLaunchArgument('color_ae_roi_bottom', default_value='-1'),
         DeclareLaunchArgument('color_exposure', default_value='-1'),
+        DeclareLaunchArgument('trigger_mode', default_value='false'),
         DeclareLaunchArgument('color_gain', default_value='-1'),
         DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
         DeclareLaunchArgument('color_white_balance', default_value='-1'),
@@ -180,7 +181,8 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='true'),
-        DeclareLaunchArgument('software_trigger_enabled', default_value='true'),
+        DeclareLaunchArgument('software_trigger_enabled', default_value='false'),
+        DeclareLaunchArgument('service_trigger_enabled', default_value='false'),
         DeclareLaunchArgument('frames_per_trigger', default_value='2'),
         DeclareLaunchArgument('software_trigger_period', default_value='33'),  # ms
         DeclareLaunchArgument('enable_ptp_config', default_value='false'),  # Only for Gemini 335Le

orbbec_camera/src/ob_camera_node.cpp

@@ -300,7 +300,7 @@ void OBCameraNode::setupDevices() {
       RCLCPP_INFO_STREAM(logger_,
                          "Software trigger period " << software_trigger_period_.count() << " ms");
       software_trigger_timer_ = node_->create_wall_timer(software_trigger_period_, [this]() {
-        if (software_trigger_enabled_) {
+        if (!service_trigger_enabled_ and software_trigger_enabled_) {
           TRY_EXECUTE_BLOCK(device_->triggerCapture());
         }
       });
@@ -1655,7 +1655,8 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(trigger2image_delay_us_, "trigger2image_delay_us", 0);
   setAndGetNodeParameter<int>(trigger_out_delay_us_, "trigger_out_delay_us", 0);
   setAndGetNodeParameter<bool>(trigger_out_enabled_, "trigger_out_enabled", true);
-  setAndGetNodeParameter<bool>(software_trigger_enabled_, "software_trigger_enabled", true);
+  setAndGetNodeParameter<bool>(software_trigger_enabled_, "software_trigger_enabled", false);
+  setAndGetNodeParameter<bool>(service_trigger_enabled_, "service_trigger_enabled", false);
   setAndGetNodeParameter<bool>(enable_ptp_config_, "enable_ptp_config", false);
   setAndGetNodeParameter<std::string>(cloud_frame_id_, "cloud_frame_id", "");
   if (enable_colored_point_cloud_ || enable_d2c_viewer_) {
@@ -2660,7 +2661,7 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
     has_subscriber = true;
   }
-  if (!has_subscriber) {
+  if (!has_subscriber && !service_capture_started_) {
     return false;
   }
   if (metadata_publishers_.count(COLOR) &&
@@ -2753,7 +2754,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   has_subscriber =
       has_subscriber || (metadata_publishers_.count(stream_index) &&
                          metadata_publishers_[stream_index]->get_subscription_count() > 0);
-  if (!has_subscriber) {
+  if (!has_subscriber && !service_capture_started_) {
     return;
   }
   std::shared_ptr<ob::VideoFrame> video_frame;
@@ -2835,9 +2836,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     publishMetadata(frame, stream_index, camera_info.header);
   }
   CHECK_NOTNULL(image_publishers_[stream_index]);
-  if (image_publishers_[stream_index]->get_subscription_count() == 0) {
-    return;
-  }
   auto &image = images_[stream_index];
   if (image.empty() || image.cols != width || image.rows != height) {
     image.create(height, width, image_format_[stream_index]);
@@ -2848,7 +2846,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   }
   if (frame->getType() == OB_FRAME_COLOR && frame->format() != OB_FORMAT_Y8 &&
       frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
-      frame->format() != OB_FORMAT_RGBA && image_publishers_[COLOR]->get_subscription_count() > 0) {
+      frame->format() != OB_FORMAT_RGBA && (image_publishers_[COLOR]->get_subscription_count() > 0 || service_capture_started_)) {
     memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
   } else {
     memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
@@ -2868,10 +2866,32 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   image_msg->step = width * unit_step_size_[stream_index];
   image_msg->header.frame_id = frame_id;
   CHECK(image_publishers_.count(stream_index) > 0);
-  saveImageToFile(stream_index, image, *image_msg);
-  image_publishers_[stream_index]->publish(std::move(image_msg));
-  if (stream_index == COLOR && enable_color_undistortion_ &&
-      color_undistortion_publisher_->get_subscription_count() > 0) {
+
+  if (image_publishers_[stream_index]->get_subscription_count() > 0) {
+    // saveImageToFile(stream_index, image, *image_msg);
+    image_publishers_[stream_index]->publish(std::move(image_msg));
+  }
+
+  if (service_trigger_enabled_ and service_capture_started_) {
+    std::unique_lock<std::mutex> lock(service_capture_lock_);
+    if (stream_index == COLOR) {
+      if (image_msg) {
+        number_of_rgb_frames_captured_++;
+        color_image_ = std::move(image_msg);
+	color_image_camera_info_ = camera_info;
+      }
+    }
+    else if (stream_index == DEPTH) {
+      if (image_msg) {
+        number_of_depth_frames_captured_++;
+        depth_image_ = std::move(image_msg);
+	depth_image_camera_info_ = camera_info;
+      }
+    }
+    service_capture_cv_.notify_all();
+  }
+
+  if (stream_index == COLOR && enable_color_undistortion_) {
     auto undistorted_image = undistortImage(image, intrinsic, distortion);
     sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
     cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
@@ -2881,7 +2901,9 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     undistorted_image_msg->is_bigendian = false;
     undistorted_image_msg->step = width * unit_step_size_[stream_index];
     undistorted_image_msg->header.frame_id = frame_id;
-    color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
+    if (color_undistortion_publisher_->get_subscription_count() > 0) {
+      color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
+    }
   }
 }
 

orbbec_camera/src/ros_service.cpp

@@ -206,9 +206,9 @@ void OBCameraNode::setupCameraCtrlServices() {
                                   std::shared_ptr<SetBool::Response> response) {
         setSYNCHostimeCallback(request, response);
       });
-  send_software_trigger_srv_ = node_->create_service<SetBool>(
-      "send_software_trigger", [this](const std::shared_ptr<SetBool::Request> request,
-                                      std::shared_ptr<SetBool::Response> response) {
+  send_service_trigger_srv_ = node_->create_service<CameraTrigger>(
+      "send_service_trigger", [this](const std::shared_ptr<CameraTrigger::Request> request,
+                                     std::shared_ptr<CameraTrigger::Response> response) {
         sendSoftwareTriggerCallback(request, response);
       });
   set_write_customerdata_srv_ = node_->create_service<SetString>(
@@ -1063,23 +1063,58 @@ void OBCameraNode::setSYNCHostimeCallback(
   }
 }
 
+void OBCameraNode::resetCaptureServiceVariables() {
+  service_capture_started_ = false;
+  number_of_rgb_frames_captured_ = 0;
+  number_of_depth_frames_captured_ = 0;
+}
+
 void OBCameraNode::sendSoftwareTriggerCallback(
-    const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
-    std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
-  try {
-    if (request->data) {
+    const std::shared_ptr<CameraTrigger::Request>& request,
+    std::shared_ptr<CameraTrigger::Response>& response) {
+  (void)request;
+  if (service_trigger_enabled_ and !software_trigger_enabled_) {
+    service_capture_started_ = true;
+    try {
+      RCLCPP_DEBUG_STREAM(logger_, "Triggering");
       device_->triggerCapture();
+
+      RCLCPP_DEBUG_STREAM(logger_, "Capturing images");
+      std::unique_lock<std::mutex> lock(service_capture_lock_);
+      service_capture_cv_.wait_until(
+          lock, std::chrono::system_clock::now() + std::chrono::seconds(1), [this]() {
+            return (number_of_rgb_frames_captured_ >= frames_per_trigger_ &&
+                    number_of_depth_frames_captured_ >= frames_per_trigger_);
+          });
+      RCLCPP_DEBUG_STREAM(logger_, "Captured " << number_of_rgb_frames_captured_ << " "
+                                               << number_of_depth_frames_captured_ << " "
+                                               << (color_image_ == nullptr) << " "
+                                               << (depth_image_ == nullptr));
+
+      if (number_of_rgb_frames_captured_ >= frames_per_trigger_ &&
+          number_of_depth_frames_captured_ >= frames_per_trigger_ && color_image_ && depth_image_) {
+        response->success = true;
+        response->rgb_image = *(color_image_);
+        response->depth_image = *(depth_image_);
+        response->rgb_camera_info = color_image_camera_info_;
+        response->depth_camera_info = depth_image_camera_info_;
+      } else {
+        response->success = false;
+        response->message = "Failed to capture images";
+      }
+    } 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;
     }
-    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;
+    resetCaptureServiceVariables();
+  } else {
+    RCLCPP_INFO_STREAM(logger_, "Service not enabled");
   }
 }
 

orbbec_camera_msgs/CMakeLists.txt

@@ -21,6 +21,7 @@ rosidl_generate_interfaces(
     "msg/Metadata.msg"
     "msg/IMUInfo.msg"
     "msg/RGBD.msg"
+    "srv/CameraTrigger.srv"
     "srv/GetBool.srv"
     "srv/GetDeviceInfo.srv"
     "srv/GetCameraInfo.srv"

orbbec_camera_msgs/srv/CameraTrigger.srv

@@ -0,0 +1,11 @@
+# Copyright (C) 2024, Locus Robotics. All rights reserved.
+# Unauthorized copying of this file, via any medium, is strictly prohibited
+# Proprietary and confidential
+
+---
+bool success
+string message
+sensor_msgs/Image rgb_image
+sensor_msgs/Image depth_image
+sensor_msgs/CameraInfo rgb_camera_info
+sensor_msgs/CameraInfo depth_camera_info

orbbec_camera_py/launch/orbbec_camera_trigger.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+
+  <arg name="use_tote_pose" default="true" />
+
+  <node pkg="orbbec_camera_py" exec="run_orbbec_trigger" output="both">
+    <param name="camera_name" value="/inward_a_right_camera"/>
+    <param name="trigger_period" value="5.0"/>
+  </node>
+
+</launch>

orbbec_camera_py/orbbec_camera_py/__init__.py

@@ -0,0 +1,66 @@
+# Copyright (C) 2023, Locus Robotics. All rights reserved.
+# Unauthorized copying of this file, via any medium, is strictly prohibited
+# Proprietary and confidential
+
+from typing import cast
+
+import time
+import rclpy
+from rclpy.node import Node
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image, CameraInfo
+from orbbec_camera_msgs.srv import CameraTrigger
+
+
+class OrbbecTriggerNode(Node):
+    def __init__(self):
+        super().__init__("orbbec_camera_trigger")
+
+        self.declare_parameter("camera_name", rclpy.Parameter.Type.STRING)
+        self.declare_parameter("trigger_period", rclpy.Parameter.Type.DOUBLE)
+
+        self.img_publisher = self.create_publisher(
+            Image, "~/trigger_rgb_image", 1
+        )
+        self.bridge = CvBridge()
+
+        self.camera_name = cast(str, self.get_parameter("camera_name").value)
+        self.trigger_period = cast(float, self.get_parameter("trigger_period").value)
+
+        self.client = self.create_client(CameraTrigger, self.camera_name + "/send_service_trigger")
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Service not available, waiting...")
+        self.get_logger().info("Service available")
+        self.request = CameraTrigger.Request()
+
+    def spin(self):
+        time.sleep(self.trigger_period)
+        self.get_logger().info("Calling service...")
+        self.future = self.client.call_async(self.request)
+        rclpy.spin_until_future_complete(self, self.future)
+
+        if self.future.result() is not None and self.future.result().success:
+            response = self.future.result()
+
+            self.img_publisher.publish(self.future.result().rgb_image)
+            self.get_logger().error("Received Images!")
+        else:
+            self.get_logger().error("Service call failed!")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OrbbecTriggerNode()
+
+    while rclpy.ok():
+        try:
+            node.spin()
+        except KeyboardInterrupt:
+            pass
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

orbbec_camera_py/orbbec_camera_py/run_orbbec_trigger.py

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>orbbec_camera_py</name>
+  <version>0.1.0</version>
+  <description>Test python nodes for orbbec.</description>
+  <maintainer email="[email protected]">Amritpal Saini</maintainer>
+  <license>Proprietary</license>
+
+  <depend>orbbec_camera_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>sensor_msgs_py</depend>
+
+  <exec_depend>message_filters</exec_depend>
+  <exec_depend>rosidl_runtime_py</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>