diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
index e3fe768a..cf0a3748 100644
--- a/orbbec_camera/src/ob_camera_node.cpp
+++ b/orbbec_camera/src/ob_camera_node.cpp
@@ -6546,8 +6546,9 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       if (align_target_stream_ == OB_STREAM_COLOR) {
         publishRawDepthImage(depth_frame);
       }
-      if (align_target_stream_ == OB_STREAM_DEPTH && !color_frame) {
-        RCLCPP_DEBUG_STREAM(logger_, "C2D alignment requires a color frame, skip alignment");
+      if (!color_frame) {
+        RCLCPP_DEBUG_STREAM(logger_, "Software alignment requires a color frame, skip frame set");
+        return;
       } else {
         auto align_color_frame = color_frame;
         if (align_target_stream_ == OB_STREAM_DEPTH) {
@@ -6570,6 +6571,7 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
           }
           if (!align_color_frame) {
             RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment");
+            return;
           } else if (align_color_frame != color_frame) {
             color_frame = align_color_frame;
             frame_set->pushFrame(color_frame);
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index 95ee5d86..9b80665f 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -903,6 +903,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
 
   auto rollback_after_error = [&](const std::string& error_message) {
     try {
+      stopColorFrameThreads();
+      clearColorFrameQueues();
       restore_old_mode();
       if (was_running && !pipeline_started_.load()) {
         startStreams();
@@ -924,6 +926,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
     if (was_running) {
       stopStreams();
     }
+    stopColorFrameThreads();
+    clearColorFrameQueues();
 
     apply_image_registration_mode(mode);