fixed crash

README.MD

@@ -68,6 +68,7 @@ ros2 launch orbbec_camera astra.launch.xml
 . ./install/setup.bash 
 rviz2
 ```
+
 Select the topic you want to display
 
 * List topics / services/ parameters ( on terminal 3)
@@ -226,7 +227,8 @@ The following are the launch parameters available:
 - `enable_color`: Enables the RGB camera.
 - `enable_depth`: Enables the depth camera.
 - `enable_ir`: Enables the IR camera.
-- `depth_registration`: Enables hardware alignment of depth and color. This requires the RGB point cloud to be open.
+- `depth_registration`: Enables hardware alignment the depth frame to color frame.
+  This field is required when the `enable_colored_point_cloud` is set to `true`.
 - `enable_publish_extrinsic`: Enables the publishing of camera extrinsic information.
 
 ## DDS Tuning

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -120,9 +120,13 @@ class OBCameraNode {
 
   void setupTopics();
 
+  void setupPipelineConfig();
+
   void setupCameraCtrlServices();
 
-  void startPipeline();
+  void startStreams();
+
+  void stopStreams();
 
   void setupDefaultImageFormat();
 
@@ -218,7 +222,8 @@ class OBCameraNode {
   rclcpp::Logger logger_;
   std::atomic_bool is_running_{false};
   std::unique_ptr<ob::Pipeline> pipeline_ = nullptr;
-  std::shared_ptr<ob::Config> config_ = nullptr;
+  std::atomic_bool pipeline_started_{false};
+  std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
   std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;
   std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_;
   std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> camera_infos_;
@@ -237,7 +242,8 @@ class OBCameraNode {
   std::map<stream_index_pair, std::string> format_str_;
   std::map<stream_index_pair, int> image_format_;
   std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>>
-      enabled_profiles_;
+      soupported_profiles_;
+  std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
   std::map<stream_index_pair, uint32_t> seq_;
   std::map<stream_index_pair, cv::Mat> images_;
   std::map<stream_index_pair, std::string> encoding_;

orbbec_camera/src/ob_camera_node.cpp

@@ -36,7 +36,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
   compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
   setupDefaultImageFormat();
   setupTopics();
-  startPipeline();
+  startStreams();
 }
 
 template <class T>
@@ -62,8 +62,8 @@ void OBCameraNode::clean() {
   if (tf_thread_->joinable()) {
     tf_thread_->join();
   }
-  RCLCPP_WARN_STREAM(logger_, "stop pipeline");
+  RCLCPP_WARN_STREAM(logger_, "stop streams");
-  pipeline_->stop();
+  stopStreams();
   RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
 }
 
@@ -93,15 +93,6 @@ void OBCameraNode::setupDevices() {
 }
 
 void OBCameraNode::setupProfiles() {
-  if (config_ != nullptr) {
-    config_.reset();
-  }
-  config_ = std::make_shared<ob::Config>();
-  if (depth_registration_) {
-    config_->setAlignMode(ALIGN_D2C_HW_MODE);
-  } else {
-    config_->setAlignMode(ALIGN_DISABLE);
-  }
   for (const auto& elem : IMAGE_STREAMS) {
     if (enable_stream_[elem]) {
       const auto& sensor = sensors_[elem];
@@ -113,7 +104,7 @@ void OBCameraNode::setupProfiles() {
                          << "stream_type: " << magic_enum::enum_name(profile->type())
                          << "Format: " << profile->format() << ", Width: " << profile->width()
                          << ", Height: " << profile->height() << ", FPS: " << profile->fps());
-        enabled_profiles_[elem].emplace_back(profile);
+        soupported_profiles_[elem].emplace_back(profile);
       }
 
       auto selected_profile =
@@ -140,7 +131,7 @@ void OBCameraNode::setupProfiles() {
         }
       }
       CHECK_NOTNULL(selected_profile);
-      config_->enableStream(selected_profile);
+      stream_profile_[elem] = selected_profile;
       images_[elem] =
           cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
       RCLCPP_INFO_STREAM(
@@ -151,14 +142,37 @@ void OBCameraNode::setupProfiles() {
   }
 }
 
-void OBCameraNode::startPipeline() {
+void OBCameraNode::startStreams() {
   if (pipeline_ != nullptr) {
     pipeline_.reset();
   }
   pipeline_ = std::make_unique<ob::Pipeline>(device_);
-  pipeline_->start(config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
+  try {
-    onNewFrameSetCallback(frame_set);
+    setupPipelineConfig();
-  });
+    pipeline_->start(pipeline_config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
+      onNewFrameSetCallback(frame_set);
+    });
+  } catch (const ob::Error& e) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to start pipeline: " << e.getMessage());
+    RCLCPP_INFO_STREAM(logger_, "try to disable ir stream and try again");
+    enable_stream_[INFRA0] = false;
+    setupPipelineConfig();
+    pipeline_->start(pipeline_config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
+      onNewFrameSetCallback(frame_set);
+    });
+  }
+  pipeline_started_.store(true);
+}
+
+void OBCameraNode::stopStreams() {
+  if (!pipeline_started_ || !pipeline_) {
+    return;
+  }
+  try {
+    pipeline_->stop();
+  } catch (const ob::Error& e) {
+    RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
+  }
 }
 
 void OBCameraNode::setupDefaultImageFormat() {
@@ -237,6 +251,21 @@ void OBCameraNode::setupTopics() {
   publishStaticTransforms();
 }
 
+void OBCameraNode::setupPipelineConfig() {
+  if (pipeline_config_) {
+    pipeline_config_.reset();
+  }
+  pipeline_config_ = std::make_shared<ob::Config>();
+  if (depth_registration_ && enable_stream_[COLOR] && enable_stream_[DEPTH]) {
+    pipeline_config_->setAlignMode(ALIGN_D2C_HW_MODE);
+  }
+  for (const auto& stream_index : IMAGE_STREAMS) {
+    if (enable_stream_[stream_index]) {
+      pipeline_config_->enableStream(stream_profile_[stream_index]);
+    }
+  }
+}
+
 void OBCameraNode::setupPublishers() {
   static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
   using PointCloud2 = sensor_msgs::msg::PointCloud2;

orbbec_camera/src/ros_service.cpp

@@ -523,7 +523,7 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
     pipeline_->stop();
     enable_stream_[stream_index] = enabled;
     setupProfiles();
-    startPipeline();
+    startStreams();
     return true;
   } catch (const ob::Error& e) {
     msg = e.getMessage();