APG-1816 Add lifecycle service to change state of camera pipelines/streaming (#9)

Author: SushiRaj99

orbbec_camera/CMakeLists.txt

@@ -32,6 +32,7 @@ set(dependencies
     camera_info_manager
     image_transport
     image_publisher
+    lifecycle_msgs
     OpenCV
     orbbec_camera_msgs
     rclcpp

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -37,6 +37,7 @@
 #include <std_srvs/srv/set_bool.hpp>
 #include <std_srvs/srv/empty.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
+#include <lifecycle_msgs/srv/change_state.hpp>
 
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <camera_info_manager/camera_info_manager.hpp>
@@ -315,7 +316,6 @@ 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 resetCaptureServiceVariables();
   void sendSoftwareTriggerCallback(const std::shared_ptr<CameraTrigger::Request>& request,
                                    std::shared_ptr<CameraTrigger::Response>& response);
@@ -339,6 +339,10 @@ class OBCameraNode {
 
   void setIRLongExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                                  std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
+  
+  void handleChangeStateRequest(
+      const std::shared_ptr<lifecycle_msgs::srv::ChangeState::Request> request,
+      std::shared_ptr<lifecycle_msgs::srv::ChangeState::Response> response);  
 
   void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
@@ -502,6 +506,8 @@ class OBCameraNode {
   rclcpp::Service<CameraTrigger>::SharedPtr send_service_trigger_srv_;
   rclcpp::Service<SetFilter>::SharedPtr set_filter_srv_;
   rclcpp::Service<CameraTrigger>::SharedPtr capture_camera_images_srv_;
+  // Lifecycle Service
+  rclcpp::Service<lifecycle_msgs::srv::ChangeState>::SharedPtr change_state_srv_;
 
 
   std::atomic_bool service_capture_started_{false};

orbbec_camera/include/orbbec_camera/ob_camera_node_driver.h

@@ -118,5 +118,6 @@ class OBCameraNodeDriver : public rclcpp::Node {
   std::string extension_path_;
   static backward::SignalHandling sh;  // for stack trace
   std::string upgrade_firmware_;
+  bool enable_streams_on_startup_ {true};
 };
 }  // namespace orbbec_camera

orbbec_camera/package.xml

@@ -17,6 +17,7 @@
   <depend>rclcpp_components</depend>
   <depend>cv_bridge</depend>
   <depend>camera_info_manager</depend>
+  <depend>lifecycle_msgs</depend>
   <depend>orbbec_camera_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>rclcpp</depend>

orbbec_camera/src/ob_camera_node.cpp

@@ -1387,6 +1387,7 @@ void OBCameraNode::stopStreams() {
         TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
                             interleave_frame_enable_);
       }
+      pipeline_started_.store(false);
     }
   } catch (const ob::Error &e) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());

orbbec_camera/src/ob_camera_node_driver.cpp

@@ -163,6 +163,7 @@ void OBCameraNodeDriver::init() {
   net_device_ip_ = declare_parameter<std::string>("net_device_ip", "");
   net_device_port_ = static_cast<int>(declare_parameter<int>("net_device_port", 0));
   enumerate_net_device_ = declare_parameter<bool>("enumerate_net_device", false);
+  enable_streams_on_startup_ = declare_parameter<bool>("enable_streams_on_startup", true);
   uvc_backend_ = declare_parameter<std::string>("uvc_backend", "libuvc");
   if (uvc_backend_ == "libuvc") {
     ctx_->setUvcBackendType(OB_UVC_BACKEND_TYPE_LIBUVC);
@@ -505,7 +506,9 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   }
   if (ob_camera_node_) {
     ob_camera_node_->startIMU();
-    ob_camera_node_->startStreams();
+    if (enable_streams_on_startup_) {
+      ob_camera_node_->startStreams();
+    }
   } else {
     RCLCPP_INFO_STREAM(logger_, "ob_camera_node_ is nullptr");
   }

orbbec_camera/src/ros_service.cpp

@@ -221,6 +221,9 @@ void OBCameraNode::setupCameraCtrlServices() {
                                        std::shared_ptr<SetString::Response> response) {
         setReadCustomerData(request, response);
       });
+  change_state_srv_ = node_->create_service<lifecycle_msgs::srv::ChangeState>(
+      "change_state", std::bind(&OBCameraNode::handleChangeStateRequest, this,
+                                std::placeholders::_1, std::placeholders::_2));
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -1069,6 +1072,63 @@ void OBCameraNode::resetCaptureServiceVariables() {
   number_of_depth_frames_captured_ = 0;
 }
 
+void OBCameraNode::handleChangeStateRequest(
+    const std::shared_ptr<lifecycle_msgs::srv::ChangeState::Request> request,
+        std::shared_ptr<lifecycle_msgs::srv::ChangeState::Response> response) {
+    
+  RCLCPP_INFO(logger_, "Received request to change state '%d' with label '%s'",
+                     request->transition.id, request->transition.label.c_str());
+  
+  response->success = true;
+  if(request->transition.id == lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE) {
+    RCLCPP_INFO_STREAM(logger_, "Recieved request to turn ON camera streams");
+    if (pipeline_started_){
+      RCLCPP_WARN_STREAM(logger_, "Camera streams already ON");
+      response->success = true;
+      return;
+    }
+    try {
+      setupProfiles();
+      startStreams();
+      RCLCPP_INFO_STREAM(logger_, "Camera streams are now ON");
+    } catch (const ob::Error& e) {
+      response->success = false;
+      RCLCPP_ERROR_STREAM(logger_, "Failed to start camera streams: " << e.getMessage());
+    } catch (const std::exception& e) {
+      response->success = false;
+      RCLCPP_ERROR_STREAM(logger_, "Failed to start camera streams: " << e.what());
+    } catch (...) {
+      response->success = false;
+      RCLCPP_ERROR_STREAM(logger_, "Failed to start camera streams: Unknown Error");
+    }
+  }
+  else if(request->transition.id == lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE) {
+    RCLCPP_INFO_STREAM(logger_, "Recieved request to turn OFF camera streams");
+    if (!pipeline_ || !pipeline_started_){
+      RCLCPP_WARN_STREAM(logger_, "Camera streams already OFF");
+      response->success = true;
+      return;
+    }
+    try {
+      stopStreams();
+      RCLCPP_INFO_STREAM(logger_, "Camera streams are now OFF");
+    } catch (const ob::Error& e) {
+      response->success = false;
+      RCLCPP_ERROR_STREAM(logger_, "Failed to stop camera streams: " << e.getMessage());
+    } catch (const std::exception& e) {
+      response->success = false;
+      RCLCPP_ERROR_STREAM(logger_, "Failed to stop camera streams: " << e.what());
+    } catch (...) {
+      response->success = false;
+      RCLCPP_ERROR_STREAM(logger_, "Failed to stop camera streams: Unknown Error");
+    }
+  }
+  else {
+    response->success = false;
+    RCLCPP_ERROR_STREAM(logger_, "Unsupported transition ID: " << request->transition.id);
+  }
+}
+
 void OBCameraNode::sendSoftwareTriggerCallback(
     const std::shared_ptr<CameraTrigger::Request>& request,
     std::shared_ptr<CameraTrigger::Response>& response) {