fixed depth align

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -283,7 +283,7 @@ class OBCameraNode {
 
   std::shared_ptr<ob::Frame> processDepthFrameFilter(std::shared_ptr<ob::Frame>& frame);
 
-  void onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet>& frame_set);
+  void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
 
   std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame);
 
@@ -513,5 +513,7 @@ class OBCameraNode {
   double diagnostic_period_ = 1.0;
   bool enable_laser_ = false;
   int laser_on_off_mode_ = 0;
+  std::unique_ptr<ob::Align> align_filter_ = nullptr;
+  OBStreamType align_target_stream_ = OB_STREAM_COLOR;
 };
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/utils.h

@@ -65,7 +65,7 @@ bool isOpenNIDevice(int pid);
 OB_DEPTH_PRECISION_LEVEL depthPrecisionLevelFromString(
     const std::string& depth_precision_level_str);
 
-float depthPrecisionFromString(const std::string &depth_precision_level_str) ;
+float depthPrecisionFromString(const std::string& depth_precision_level_str);
 
 OBMultiDeviceSyncMode OBSyncModeFromString(const std::string& mode);
 
@@ -91,4 +91,6 @@ OBHoleFillingMode holeFillingModeFromString(const std::string& hole_filling_mode
 
 bool isGemini2R(int pid);
 
+OBStreamType obStreamTypeFromString(const std::string& stream_type);
+
 }  // namespace orbbec_camera

orbbec_camera/src/ob_camera_node.cpp

@@ -134,6 +134,9 @@ void OBCameraNode::setupDevices() {
   }
   auto info = device_->getDeviceInfo();
   try {
+    if (depth_registration_) {
+      align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
+    }
     if (enable_hardware_d2d_ &&
         device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
       device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, true);
@@ -144,7 +147,6 @@ void OBCameraNode::setupDevices() {
     device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, enable_laser_);
     device_->setIntProperty(OB_PROP_LASER_ON_OFF_MODE_INT, laser_on_off_mode_);
     device_->loadPreset(device_preset_.c_str());
-    return;
     auto depth_sensor = device_->getSensor(OB_SENSOR_DEPTH);
     // set depth sensor to filter
     auto filter_list = depth_sensor->getRecommendedFilters();
@@ -219,15 +221,16 @@ void OBCameraNode::setupDevices() {
       sync_config.triggerOutEnable = trigger_out_enabled_;
       device_->setMultiDeviceSyncConfig(sync_config);
     }
-    if (device_->isPropertySupported(OB_PROP_DEPTH_PRECISION_LEVEL_INT, OB_PERMISSION_READ_WRITE)) {
+    if (device_->isPropertySupported(OB_PROP_DEPTH_PRECISION_LEVEL_INT, OB_PERMISSION_READ_WRITE) &&
+        !depth_precision_str_.empty()) {
       auto default_precision_level = device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT);
       if (default_precision_level != depth_precision_) {
         device_->setIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT, depth_precision_);
         RCLCPP_INFO_STREAM(logger_, "set depth precision to " << depth_precision_str_);
       }
-    }
+    } else if (device_->isPropertySupported(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
-    if (device_->isPropertySupported(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT,
+                                            OB_PERMISSION_READ_WRITE) &&
-                                     OB_PERMISSION_READ_WRITE)) {
+               !depth_precision_str_.empty()) {
       auto depth_unit_flexible_adjustment = depthPrecisionFromString(depth_precision_str_);
       auto range = device_->getFloatPropertyRange(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT);
       RCLCPP_INFO_STREAM(
@@ -683,10 +686,12 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter(trigger2image_delay_us_, "trigger2image_delay_us", 0);
   setAndGetNodeParameter(trigger_out_delay_us_, "trigger_out_delay_us", 0);
   setAndGetNodeParameter(trigger_out_enabled_, "trigger_out_enabled", false);
-  setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "1mm");
+  setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
-  std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper);
+  if (!depth_precision_str_.empty()) {
-  sync_mode_ = OBSyncModeFromString(sync_mode_str_);
+    std::transform(sync_mode_str_.begin(), sync_mode_str_.end(), sync_mode_str_.begin(), ::toupper);
-  depth_precision_ = depthPrecisionLevelFromString(depth_precision_str_);
+    sync_mode_ = OBSyncModeFromString(sync_mode_str_);
+    depth_precision_ = depthPrecisionLevelFromString(depth_precision_str_);
+  }
   if (enable_colored_point_cloud_) {
     depth_registration_ = true;
   }
@@ -727,6 +732,9 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<double>(diagnostic_period_, "diagnostic_period", 1.0);
   setAndGetNodeParameter<bool>(enable_laser_, "enable_laser", false);
   setAndGetNodeParameter<int>(laser_on_off_mode_, "laser_on_off_mode", 0);
+  std::string align_target_stream_str_;
+  setAndGetNodeParameter<std::string>(align_target_stream_str_, "align_target_stream", "COLOR");
+  align_target_stream_ = obStreamTypeFromString(align_target_stream_str_);
 }
 
 void OBCameraNode::setupTopics() {
@@ -780,8 +788,8 @@ void OBCameraNode::setupPipelineConfig() {
   pipeline_config_ = std::make_shared<ob::Config>();
   pipeline_config_->setDepthScaleRequire(enable_depth_scale_);
   if (depth_registration_ && enable_stream_[COLOR] && enable_stream_[DEPTH]) {
-    RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
     OBAlignMode align_mode = align_mode_ == "HW" ? ALIGN_D2C_HW_MODE : ALIGN_D2C_SW_MODE;
+    RCLCPP_INFO_STREAM(logger_, "set align mode to " << magic_enum::enum_name(align_mode));
     pipeline_config_->setAlignMode(align_mode);
   }
   for (const auto &stream_index : IMAGE_STREAMS) {
@@ -1144,7 +1152,7 @@ std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
   return frame;
 }
 
-void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &frame_set) {
+void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
   if (!is_running_.load()) {
     return;
   }
@@ -1161,7 +1169,15 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
     // is_color_frame_decoded_ = decodeColorFrameToBuffer(frame_set->colorFrame(), rgb_buffer_);
     std::shared_ptr<ob::ColorFrame> colorFrame = frame_set->colorFrame();
     depth_frame_ = frame_set->getFrame(OB_FRAME_DEPTH);
+    if (depth_registration_ && align_filter_) {
+      auto new_frame = align_filter_->process(frame_set);
+      CHECK_NOTNULL(new_frame.get());
+      auto new_frame_set = new_frame->as<ob::FrameSet>();
+      CHECK_NOTNULL(new_frame_set.get());
+      depth_frame_ = new_frame_set->getFrame(OB_FRAME_DEPTH);
+    }
     depth_frame_ = processDepthFrameFilter(depth_frame_);
+
     if (enable_stream_[COLOR] && colorFrame) {
       std::lock_guard<std::mutex> colorLock(colorFrameMtx_);
       colorFrameQueue_.push(frame_set);
@@ -1181,9 +1197,6 @@ void OBCameraNode::onNewFrameSetCallback(const std::shared_ptr<ob::FrameSet> &fr
         if (frame == nullptr) {
           continue;
         }
-        if (frame_type == OB_FRAME_DEPTH) {
-          frame = depth_frame_;
-        }
 
         std::shared_ptr<ob::Frame> irFrame = decodeIRMJPGFrame(frame);
         if (irFrame) {

orbbec_camera/src/utils.cpp

@@ -386,7 +386,7 @@ float depthPrecisionFromString(const std::string &depth_precision_level_str) {
   if (depth_precision_level_str.size() < 2) {
     RCLCPP_WARN_STREAM(rclcpp::get_logger("utils"),
                        "Invalid depth precision level: " << depth_precision_level_str
-                                                          << ". Using default precision level 1mm");
+                                                         << ". Using default precision level 1mm");
     return 1.0;
   }
   std::string depth_precision_level_str_num =
@@ -697,4 +697,31 @@ bool isGemini2R(int pid) {
   }
   return false;
 }
+
+OBStreamType obStreamTypeFromString(const std::string &stream_type) {
+  std::string upper_stream_type = stream_type;
+  std::transform(upper_stream_type.begin(), upper_stream_type.end(), upper_stream_type.begin(),
+                 ::toupper);
+  if (upper_stream_type == "VIDEO") {
+    return OB_STREAM_VIDEO;
+  } else if (upper_stream_type == "IR") {
+    return OB_STREAM_IR;
+  } else if (upper_stream_type == "COLOR") {
+    return OB_STREAM_COLOR;
+  } else if (upper_stream_type == "DEPTH") {
+    return OB_STREAM_DEPTH;
+  } else if (upper_stream_type == "ACCEL") {
+    return OB_STREAM_ACCEL;
+  } else if (upper_stream_type == "GYRO") {
+    return OB_STREAM_GYRO;
+  } else if (upper_stream_type == "IR_LEFT") {
+    return OB_STREAM_IR_LEFT;
+  } else if (upper_stream_type == "IR_RIGHT") {
+    return OB_STREAM_IR_RIGHT;
+  } else if (upper_stream_type == "RAW_PHASE") {
+    return OB_STREAM_RAW_PHASE;
+  } else {
+    return OB_STREAM_UNKNOWN;
+  }
+}
 }  // namespace orbbec_camera