diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/orbbec_camera/include/orbbec_camera/constants.h
index dc509624..5bdc6b67 100644
--- a/orbbec_camera/include/orbbec_camera/constants.h
+++ b/orbbec_camera/include/orbbec_camera/constants.h
@@ -23,7 +23,7 @@
 
 #define OB_ROS_MAJOR_VERSION 1
 #define OB_ROS_MINOR_VERSION 5
-#define OB_ROS_PATCH_VERSION 21
+#define OB_ROS_PATCH_VERSION 22
 
 #ifndef STRINGIFY
 #define STRINGIFY(arg) #arg
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index db3582b2..c9dfbb0f 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -19,6 +19,7 @@
 #include <nlohmann/json.hpp>
 
 #include <memory>
+#include <optional>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <unordered_map>
@@ -57,6 +58,7 @@
 #include "orbbec_camera_msgs/srv/set_int32.hpp"
 #include "orbbec_camera_msgs/srv/get_bool.hpp"
 #include "orbbec_camera_msgs/srv/set_string.hpp"
+#include "orbbec_camera_msgs/srv/set_stream_profile.hpp"
 #include "orbbec_camera/constants.h"
 #include "orbbec_camera/dynamic_params.h"
 #include "orbbec_camera/d2c_viewer.h"
@@ -64,6 +66,7 @@
 #include "orbbec_camera/image_publisher.h"
 #include "orbbec_camera/frame_timestamp_csv_logger.h"
 #include "jpeg_decoder.h"
+#include <std_msgs/msg/int32.hpp>
 #include <std_msgs/msg/string.hpp>
 
 #if __has_include(<cv_bridge/cv_bridge.hpp>)
@@ -105,6 +108,7 @@ using GetString = orbbec_camera_msgs::srv::GetString;
 using SetString = orbbec_camera_msgs::srv::SetString;
 using SetBool = std_srvs::srv::SetBool;
 using GetBool = orbbec_camera_msgs::srv::GetBool;
+using SetStreamProfile = orbbec_camera_msgs::srv::SetStreamProfile;
 
 typedef std::pair<ob_stream_type, int> stream_index_pair;
 
@@ -165,10 +169,38 @@ class OBCameraNode {
     double timestamp_ = -1;  // in nanoseconds
   };
 
+  struct PendingStreamProfile {
+    stream_index_pair stream_index;
+    int requested_width = 0;
+    int requested_height = 0;
+    int requested_fps = 0;
+    std::shared_ptr<ob::VideoStreamProfile> profile;
+  };
+
   void setupDevices();
 
   void setupProfiles();
 
+  void syncSoftwareAlignment();
+
+  std::shared_ptr<ob::VideoStreamProfile> selectVideoStreamProfile(
+      const stream_index_pair& stream_index, int width, int height, int fps, OBFormat format);
+
+  std::optional<stream_index_pair> getImageStreamByName(const std::string& stream_name) const;
+
+  bool validateStreamProfileRequest(const std::shared_ptr<SetStreamProfile::Request>& request,
+                                    std::vector<PendingStreamProfile>& pending_profiles,
+                                    std::string& message);
+
+  bool applyStreamProfiles(const std::vector<PendingStreamProfile>& pending_profiles,
+                           std::string& message);
+
+  void clearColorFrameQueue();
+
+  void stopColorFrameThread();
+
+  void setupImageBuffers();
+
   void updateImageConfig(const stream_index_pair& stream_index);
 
   void printSensorProfiles(const std::shared_ptr<ob::Sensor>& sensor);
@@ -179,12 +211,16 @@ class OBCameraNode {
 
   void setupTopics();
 
+  void setupImagePublisher(const stream_index_pair& stream_index);
+
   void setupPipelineConfig();
 
   void setupDiagnosticUpdater();
 
   void onTemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& status);
 
+  void publishLrmObstacleDistance();
+
   void setupCameraCtrlServices();
 
   void stopStreams();
@@ -269,6 +305,9 @@ class OBCameraNode {
                             std::shared_ptr<SetBool::Response>& response,
                             const stream_index_pair& stream_index);
 
+  void setImageRegistrationModeCallback(const std::shared_ptr<SetString::Request> request,
+                                        std::shared_ptr<SetString::Response> response);
+
   void setMirrorCallback(const std::shared_ptr<SetBool::Request>& request,
                          std::shared_ptr<SetBool::Response>& response,
                          const stream_index_pair& stream_index);
@@ -299,6 +338,9 @@ class OBCameraNode {
   void setIRLongExposureCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                                  std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
 
+  void setStreamProfileCallback(const std::shared_ptr<SetStreamProfile::Request>& request,
+                                std::shared_ptr<SetStreamProfile::Response>& response);
+
   void publishPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
 
   void publishDepthPointCloud(const std::shared_ptr<ob::FrameSet>& frame_set);
@@ -313,6 +355,8 @@ class OBCameraNode {
 
   std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame);
 
+  bool isColorFrameDecodeRequired(const std::shared_ptr<ob::Frame>& frame) const;
+
   bool decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame>& frame, uint8_t* buffer);
 
   std::shared_ptr<ob::Frame> decodeIRMJPGFrame(const std::shared_ptr<ob::Frame>& frame);
@@ -424,7 +468,9 @@ class OBCameraNode {
   rclcpp::Service<SetBool>::SharedPtr set_auto_white_balance_srv_;
   rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
   rclcpp::Service<SetString>::SharedPtr switch_ir_camera_srv_;
+  rclcpp::Service<SetString>::SharedPtr set_image_registration_mode_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ir_long_exposure_srv_;
+  rclcpp::Service<SetStreamProfile>::SharedPtr set_stream_profile_srv_;
   std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
       set_auto_exposure_srv_;
   rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
@@ -437,6 +483,8 @@ class OBCameraNode {
   rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_ldp_measure_distance_srv_;
+  rclcpp::TimerBase::SharedPtr lrm_obstacle_distance_timer_;
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr lrm_obstacle_distance_pub_;
 
   bool enable_sync_output_accel_gyro_ = false;
   bool publish_tf_ = false;
@@ -530,11 +578,13 @@ class OBCameraNode {
   // mjpeg decoder
   std::shared_ptr<JPEGDecoder> jpeg_decoder_ = nullptr;
   uint8_t* rgb_buffer_ = nullptr;
+  size_t rgb_buffer_size_ = 0;
   bool is_color_frame_decoded_ = false;
   std::mutex device_lock_;
   // For color
   std::queue<std::shared_ptr<ob::FrameSet>> color_frame_queue_;
   std::shared_ptr<std::thread> colorFrameThread_ = nullptr;
+  std::atomic_bool stop_color_frame_thread_{false};
   std::mutex color_frame_queue_lock_;
   std::condition_variable color_frame_queue_cv_;
 
@@ -598,6 +648,8 @@ class OBCameraNode {
   // soft ware trigger
   rclcpp::TimerBase::SharedPtr software_trigger_timer_;
   std::chrono::milliseconds software_trigger_period_{33};
+  bool enable_lrm_obstacle_distance_publish_ = false;
+  double lrm_obstacle_distance_publish_rate_ = 10.0;
   bool enable_heartbeat_ = false;
   std::string industry_mode_ = "";
   bool enable_color_undistortion_ = false;
diff --git a/orbbec_camera/launch/gemini_330_series.launch.py b/orbbec_camera/launch/gemini_330_series.launch.py
index 1e00062a..4ed727db 100644
--- a/orbbec_camera/launch/gemini_330_series.launch.py
+++ b/orbbec_camera/launch/gemini_330_series.launch.py
@@ -155,6 +155,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_publish_extrinsic', default_value='false'),
         DeclareLaunchArgument('enable_d2c_viewer', default_value='false'),
         DeclareLaunchArgument('enable_ldp', default_value='true'),
+        DeclareLaunchArgument('enable_lrm_obstacle_distance_publish', default_value='false'),
+        DeclareLaunchArgument('lrm_obstacle_distance_publish_rate', default_value='10.0'),
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
diff --git a/orbbec_camera/package.xml b/orbbec_camera/package.xml
index 93f530ad..a21da61a 100644
--- a/orbbec_camera/package.xml
+++ b/orbbec_camera/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_camera</name>
-  <version>1.5.21</version>
+  <version>1.5.22</version>
   <description>Orbbec Camera package</description>
   <maintainer email="mocun@orbbec.com">Joe Dong</maintainer>
   <license>Apache-2.0</license>
diff --git a/orbbec_camera/scripts/99-obsensor-libusb.rules b/orbbec_camera/scripts/99-obsensor-libusb.rules
index 6ea8f08f..d3a4f835 100644
--- a/orbbec_camera/scripts/99-obsensor-libusb.rules
+++ b/orbbec_camera/scripts/99-obsensor-libusb.rules
@@ -1,4 +1,4 @@
-SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0501", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Bootloader Device"
+SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0501", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Bootloader_Device"
 
 # UVC Modules
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0635", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Femto"
@@ -12,9 +12,9 @@ SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0669", MODE:="066
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="066b", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Femto_Bolt"
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0660", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Astra_2"
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0670", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2"
-SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0671", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2 XL"
-SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0673", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2 L"
-SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0675", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2 VL"
+SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0671", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2_XL"
+SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0673", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2_L"
+SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0675", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_2_VL"
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0800", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_335"
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0801", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_330"
 SUBSYSTEMS=="usb", ATTRS{idVendor}=="2bc5", ATTRS{idProduct}=="0802", MODE:="0666",  OWNER:="root", GROUP:="video", SYMLINK+="Gemini_dm330"
diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
index 80466bf7..69ba0dce 100644
--- a/orbbec_camera/src/ob_camera_node.cpp
+++ b/orbbec_camera/src/ob_camera_node.cpp
@@ -15,6 +15,8 @@
  *******************************************************************************/
 
 #include "orbbec_camera/ob_camera_node.h"
+#include <algorithm>
+#include <cctype>
 #include <rclcpp/rclcpp.hpp>
 #include <thread>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -81,28 +83,67 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
       frame_timestamp_csv_logger_.reset();
     }
   }
-#if defined(USE_RK_HW_DECODER)
-  jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
-#elif defined(USE_NV_HW_DECODER)
-  jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
-#endif
   if (enable_d2c_viewer_) {
     auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]);
     auto depth_qos = getRMWQosProfileFromString(image_qos_[DEPTH]);
     d2c_viewer_ = std::make_unique<D2CViewer>(node_, rgb_qos, depth_qos);
   }
-  if (enable_stream_[COLOR]) {
-    rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 3];
-  }
-  if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
-    rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
-    rgb_point_cloud_buffer_ = new uint8_t[rgb_point_cloud_buffer_size_];
-    xy_table_data_size_ = width_[DEPTH] * height_[DEPTH] * 2;
-    xy_table_data_ = new float[xy_table_data_size_];
-  }
+  setupImageBuffers();
   is_camera_node_initialized_ = true;
 }
 
+void OBCameraNode::clearColorFrameQueue() {
+  std::lock_guard<std::mutex> color_queue_lock(color_frame_queue_lock_);
+  std::queue<std::shared_ptr<ob::FrameSet>> empty_queue;
+  color_frame_queue_.swap(empty_queue);
+  is_color_frame_decoded_ = false;
+}
+
+void OBCameraNode::stopColorFrameThread() {
+  if (!colorFrameThread_) {
+    return;
+  }
+  stop_color_frame_thread_.store(true);
+  color_frame_queue_cv_.notify_all();
+  if (colorFrameThread_->joinable()) {
+    colorFrameThread_->join();
+  }
+  colorFrameThread_.reset();
+  stop_color_frame_thread_.store(false);
+}
+
+void OBCameraNode::setupImageBuffers() {
+  delete[] rgb_buffer_;
+  rgb_buffer_ = nullptr;
+  jpeg_decoder_.reset();
+
+#if defined(USE_RK_HW_DECODER)
+  if (enable_stream_[COLOR] && width_[COLOR] > 0 && height_[COLOR] > 0) {
+    jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
+  }
+#elif defined(USE_NV_HW_DECODER)
+  if (enable_stream_[COLOR] && width_[COLOR] > 0 && height_[COLOR] > 0) {
+    jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
+  }
+#endif
+
+  if (enable_stream_[COLOR]) {
+    CHECK(width_[COLOR] > 0 && height_[COLOR] > 0);
+    rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 4];
+  }
+
+  if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
+    xy_tables_.reset();
+    const uint32_t point_cloud_buffer_size = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
+    if (point_cloud_buffer_size > rgb_point_cloud_buffer_size_) {
+      delete[] rgb_point_cloud_buffer_;
+      rgb_point_cloud_buffer_ = new uint8_t[point_cloud_buffer_size];
+      rgb_point_cloud_buffer_size_ = point_cloud_buffer_size;
+    }
+  }
+  is_color_frame_decoded_ = false;
+}
+
 template <class T>
 void OBCameraNode::setAndGetNodeParameter(
     T &param, const std::string &param_name, const T &default_value,
@@ -137,15 +178,20 @@ void OBCameraNode::clean() noexcept {
     frame_timestamp_csv_logger_->shutdown();
     frame_timestamp_csv_logger_.reset();
   }
+  if (software_trigger_timer_) {
+    software_trigger_timer_->cancel();
+    software_trigger_timer_.reset();
+  }
+  if (lrm_obstacle_distance_timer_) {
+    lrm_obstacle_distance_timer_->cancel();
+    lrm_obstacle_distance_timer_.reset();
+  }
   RCLCPP_WARN_STREAM(logger_, "Stop tf thread");
   if (tf_thread_ && tf_thread_->joinable()) {
     tf_thread_->join();
   }
   RCLCPP_WARN_STREAM(logger_, "Stop color frame thread");
-  if (colorFrameThread_ && colorFrameThread_->joinable()) {
-    color_frame_queue_cv_.notify_all();
-    colorFrameThread_->join();
-  }
+  stopColorFrameThread();
 
   RCLCPP_WARN_STREAM(logger_, "stop streams");
   stopStreams();
@@ -153,6 +199,7 @@ void OBCameraNode::clean() noexcept {
   if (rgb_buffer_) {
     delete[] rgb_buffer_;
     rgb_buffer_ = nullptr;
+    rgb_buffer_size_ = 0;
   }
   if (rgb_point_cloud_buffer_) {
     delete[] rgb_point_cloud_buffer_;
@@ -278,9 +325,12 @@ void OBCameraNode::setupDevices() {
                          "Laser energy level set to " << new_laser_energy_level << " (new value)");
     }
   }
-  if (depth_registration_) {
+  if (depth_registration_ && (align_mode_ == "SW" || isGemini335PID(pid))) {
     RCLCPP_INFO_STREAM(logger_, "Create align filter");
     align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
+    if (align_mode_ == "SW") {
+      RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
+    }
   }
   if (device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
     TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DISPARITY_TO_DEPTH_BOOL, enable_hardware_d2d_);
@@ -885,26 +935,263 @@ void OBCameraNode::setupProfiles() {
     }
   }
 }
-void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
-  if (format_[stream_index] == OB_FORMAT_Y8) {
-    image_format_[stream_index] = CV_8UC1;
-    encoding_[stream_index] = stream_index.first == OB_STREAM_DEPTH
-                                  ? sensor_msgs::image_encodings::TYPE_8UC1
-                                  : sensor_msgs::image_encodings::MONO8;
-    unit_step_size_[stream_index] = sizeof(uint8_t);
+
+void OBCameraNode::syncSoftwareAlignment() {
+  bool use_software_alignment = depth_registration_ && align_mode_ == "SW";
+  if (depth_registration_ && align_mode_ == "HW" && device_) {
+    auto device_info = device_->getDeviceInfo();
+    CHECK_NOTNULL(device_info.get());
+    use_software_alignment = isGemini335PID(device_info->pid());
   }
-  if (format_[stream_index] == OB_FORMAT_MJPG) {
-    if (stream_index.first == OB_STREAM_IR || stream_index.first == OB_STREAM_IR_LEFT ||
-        stream_index.first == OB_STREAM_IR_RIGHT) {
+  if (use_software_alignment) {
+    if (!align_filter_) {
+      align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
+      if (align_mode_ == "SW") {
+        RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
+      }
+    }
+    return;
+  }
+  align_filter_.reset();
+}
+
+std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
+    const stream_index_pair &stream_index, int width, int height, int fps, OBFormat format) {
+  auto sensor_it = sensors_.find(stream_index);
+  if (sensor_it == sensors_.end() || !sensor_it->second) {
+    throw std::runtime_error("Sensor is not available for stream " + stream_name_[stream_index]);
+  }
+  auto profiles = sensor_it->second->getStreamProfileList();
+  if (!profiles || profiles->count() == 0) {
+    throw std::runtime_error("No stream profiles available for stream " +
+                             stream_name_[stream_index]);
+  }
+
+  std::shared_ptr<ob::VideoStreamProfile> selected_profile;
+  if (width == 0 && height == 0 && fps == 0) {
+    selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
+  } else {
+    selected_profile = profiles->getVideoStreamProfile(width, height, format, fps);
+  }
+
+  if (!selected_profile) {
+    throw std::runtime_error("Requested stream profile is not supported");
+  }
+  return selected_profile;
+}
+
+std::optional<stream_index_pair> OBCameraNode::getImageStreamByName(
+    const std::string &stream_name) const {
+  if (stream_name == "color") {
+    return COLOR;
+  }
+  if (stream_name == "depth") {
+    return DEPTH;
+  }
+  if (stream_name == "ir") {
+    return INFRA0;
+  }
+  if (stream_name == "left_ir") {
+    return INFRA1;
+  }
+  if (stream_name == "right_ir") {
+    return INFRA2;
+  }
+  return std::nullopt;
+}
+
+bool OBCameraNode::validateStreamProfileRequest(
+    const std::shared_ptr<SetStreamProfile::Request> &request,
+    std::vector<PendingStreamProfile> &pending_profiles, std::string &message) {
+  pending_profiles.clear();
+  if (!request || request->profiles.empty()) {
+    message = "profiles is empty";
+    return false;
+  }
+
+  std::unordered_set<std::string> requested_streams;
+  bool has_changes = false;
+  for (const auto &profile : request->profiles) {
+    const auto stream_index = getImageStreamByName(profile.stream_name);
+    if (!stream_index) {
+      message = "Unsupported stream_name: " + profile.stream_name +
+                ". Supported stream_name values: color, depth, ir, left_ir, right_ir";
+      return false;
+    }
+    if (!requested_streams.insert(profile.stream_name).second) {
+      message = "Duplicated stream_name: " + profile.stream_name;
+      return false;
+    }
+    if (!enable_stream_[*stream_index]) {
+      message = "Stream is not enabled: " + profile.stream_name;
+      return false;
+    }
+
+    if (profile.width < 0 || profile.height < 0 || profile.fps < 0) {
+      message = profile.stream_name + " width, height and fps must be non-negative";
+      return false;
+    }
+    if ((profile.width > 0) != (profile.height > 0)) {
+      message = profile.stream_name + " width and height must be provided together";
+      return false;
+    }
+    if (profile.width <= 0 && profile.fps <= 0 && profile.format.empty()) {
+      message = profile.stream_name + " must provide resolution, fps or format";
+      return false;
+    }
+
+    const int requested_width = profile.width > 0 ? profile.width : width_[*stream_index];
+    const int requested_height = profile.height > 0 ? profile.height : height_[*stream_index];
+    const int requested_fps = profile.fps > 0 ? profile.fps : fps_[*stream_index];
+    if (requested_width <= 0 || requested_height <= 0 || requested_fps <= 0) {
+      message = profile.stream_name + " current width, height and fps must be positive";
+      return false;
+    }
+
+    OBFormat requested_format = format_[*stream_index];
+    if (!profile.format.empty()) {
+      std::string format_name;
+      format_name.reserve(profile.format.size());
+      std::transform(profile.format.begin(), profile.format.end(), std::back_inserter(format_name),
+                     [](unsigned char ch) { return static_cast<char>(std::toupper(ch)); });
+      if (format_name == "ANY") {
+        requested_format = OB_FORMAT_UNKNOWN;
+      } else {
+        requested_format = OBFormatFromString(format_name);
+        if (requested_format == OB_FORMAT_UNKNOWN) {
+          message = "Unsupported format: " + profile.format;
+          return false;
+        }
+      }
+    }
+
+    try {
+      auto selected_profile = selectVideoStreamProfile(
+          *stream_index, requested_width, requested_height, requested_fps, requested_format);
+      has_changes = has_changes ||
+                    static_cast<int>(selected_profile->width()) != width_[*stream_index] ||
+                    static_cast<int>(selected_profile->height()) != height_[*stream_index] ||
+                    static_cast<int>(selected_profile->fps()) != fps_[*stream_index] ||
+                    selected_profile->format() != format_[*stream_index];
+      pending_profiles.push_back(
+          {*stream_index, requested_width, requested_height, requested_fps, selected_profile});
+    } catch (const ob::Error &e) {
+      message = "Unsupported profile for " + profile.stream_name + ": " + e.getMessage();
+      return false;
+    } catch (const std::exception &e) {
+      message = "Unsupported profile for " + profile.stream_name + ": " + e.what();
+      return false;
+    }
+  }
+  if (!has_changes) {
+    message = "requested stream profiles are already active";
+    return false;
+  }
+  return true;
+}
+
+bool OBCameraNode::applyStreamProfiles(const std::vector<PendingStreamProfile> &pending_profiles,
+                                       std::string &message) {
+  if (pending_profiles.empty()) {
+    message = "profiles is empty";
+    return false;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  try {
+    const bool restart_pipeline = pipeline_started_.load();
+    if (restart_pipeline) {
+      stopStreams();
+    }
+    stopColorFrameThread();
+    clearColorFrameQueue();
+
+    for (const auto &pending_profile : pending_profiles) {
+      const auto &stream_index = pending_profile.stream_index;
+      auto selected_profile = pending_profile.profile;
+      const auto old_format = format_[stream_index];
+      stream_profile_[stream_index] = selected_profile;
+      height_[stream_index] = static_cast<int>(selected_profile->height());
+      width_[stream_index] = static_cast<int>(selected_profile->width());
+      fps_[stream_index] = static_cast<int>(selected_profile->fps());
+      format_[stream_index] = selected_profile->format();
+      format_str_[stream_index] = OBFormatToString(format_[stream_index]);
+      updateImageConfig(stream_index);
+      if (old_format != format_[stream_index]) {
+        setupImagePublisher(stream_index);
+      }
+      images_[stream_index] = cv::Mat(height_[stream_index], width_[stream_index],
+                                      image_format_[stream_index], cv::Scalar(0, 0, 0));
+      RCLCPP_INFO_STREAM(
+          logger_, "Updated stream profile for "
+                       << stream_name_[stream_index] << " - width: " << width_[stream_index]
+                       << ", height: " << height_[stream_index] << ", fps: " << fps_[stream_index]
+                       << ", format: " << magic_enum::enum_name(format_[stream_index]));
+    }
+
+    setupImageBuffers();
+    clearColorFrameQueue();
+    if (restart_pipeline) {
+      startStreams();
+    }
+    message = "success";
+    return true;
+  } catch (const ob::Error &e) {
+    message = e.getMessage();
+  } catch (const std::exception &e) {
+    message = e.what();
+  } catch (...) {
+    message = "unknown error";
+  }
+  return false;
+}
+
+void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
+  const auto format = format_[stream_index];
+  const bool is_depth_stream = stream_index.first == OB_STREAM_DEPTH;
+  const bool is_ir_stream = stream_index.first == OB_STREAM_IR ||
+                            stream_index.first == OB_STREAM_IR_LEFT ||
+                            stream_index.first == OB_STREAM_IR_RIGHT;
+  const bool is_color_stream = stream_index == COLOR;
+
+  if (format == OB_FORMAT_Y8 || format == OB_FORMAT_GRAY) {
+    image_format_[stream_index] = CV_8UC1;
+    encoding_[stream_index] = is_depth_stream ? sensor_msgs::image_encodings::TYPE_8UC1
+                                              : sensor_msgs::image_encodings::MONO8;
+    unit_step_size_[stream_index] = sizeof(uint8_t);
+  } else if (format == OB_FORMAT_Y10 || format == OB_FORMAT_Y11 || format == OB_FORMAT_Y12 ||
+             format == OB_FORMAT_Y14 || format == OB_FORMAT_Y16 || format == OB_FORMAT_Z16 ||
+             format == OB_FORMAT_RW16) {
+    image_format_[stream_index] = CV_16UC1;
+    encoding_[stream_index] = is_depth_stream ? sensor_msgs::image_encodings::TYPE_16UC1
+                                              : sensor_msgs::image_encodings::MONO16;
+    unit_step_size_[stream_index] = sizeof(uint16_t);
+  } else if (format == OB_FORMAT_MJPG || format == OB_FORMAT_MJPEG) {
+    if (is_ir_stream) {
       image_format_[stream_index] = CV_8UC1;
       encoding_[stream_index] = sensor_msgs::image_encodings::MONO8;
       unit_step_size_[stream_index] = sizeof(uint8_t);
+    } else if (is_color_stream) {
+      image_format_[stream_index] = CV_8UC3;
+      encoding_[stream_index] = sensor_msgs::image_encodings::RGB8;
+      unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
     }
-  }
-  if (format_[stream_index] == OB_FORMAT_Y16 && stream_index == COLOR) {
-    image_format_[stream_index] = CV_16UC1;
-    encoding_[stream_index] = sensor_msgs::image_encodings::MONO16;
-    unit_step_size_[stream_index] = sizeof(uint16_t);
+  } else if (format == OB_FORMAT_BGR) {
+    image_format_[stream_index] = CV_8UC3;
+    encoding_[stream_index] = sensor_msgs::image_encodings::BGR8;
+    unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
+  } else if (format == OB_FORMAT_RGB || format == OB_FORMAT_RGB888) {
+    image_format_[stream_index] = CV_8UC3;
+    encoding_[stream_index] = sensor_msgs::image_encodings::RGB8;
+    unit_step_size_[stream_index] = 3 * sizeof(uint8_t);
+  } else if (format == OB_FORMAT_BGRA) {
+    image_format_[stream_index] = CV_8UC4;
+    encoding_[stream_index] = sensor_msgs::image_encodings::BGRA8;
+    unit_step_size_[stream_index] = 4 * sizeof(uint8_t);
+  } else if (format == OB_FORMAT_RGBA) {
+    image_format_[stream_index] = CV_8UC4;
+    encoding_[stream_index] = sensor_msgs::image_encodings::RGBA8;
+    unit_step_size_[stream_index] = 4 * sizeof(uint8_t);
   }
 }
 
@@ -1226,6 +1513,19 @@ void OBCameraNode::getParameters() {
     depth_registration_ = false;
   }
   setAndGetNodeParameter<bool>(enable_ldp_, "enable_ldp", true);
+  setAndGetNodeParameter<bool>(enable_lrm_obstacle_distance_publish_,
+                               "enable_lrm_obstacle_distance_publish", false);
+  setAndGetNodeParameter<double>(lrm_obstacle_distance_publish_rate_,
+                                 "lrm_obstacle_distance_publish_rate", 10.0);
+  if (enable_lrm_obstacle_distance_publish_ && !enable_ldp_) {
+    RCLCPP_INFO_STREAM(logger_, "enable_lrm_obstacle_distance_publish is true, enabling LDP");
+    enable_ldp_ = true;
+  }
+  if (lrm_obstacle_distance_publish_rate_ <= 0.0) {
+    RCLCPP_WARN_STREAM(logger_, "Invalid lrm_obstacle_distance_publish_rate "
+                                    << lrm_obstacle_distance_publish_rate_ << ", reset to 10.0");
+    lrm_obstacle_distance_publish_rate_ = 10.0;
+  }
   setAndGetNodeParameter<int>(soft_filter_max_diff_, "soft_filter_max_diff", -1);
   setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
   setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
@@ -1374,9 +1674,9 @@ void OBCameraNode::setupPipelineConfig() {
   CHECK_NOTNULL(device_info.get());
   auto pid = device_info->pid();
   if (depth_registration_ && enable_stream_[COLOR] && enable_stream_[DEPTH] &&
-      !isGemini335PID(pid)) {
-    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));
+      !isGemini335PID(pid) && align_mode_ == "HW") {
+    OBAlignMode align_mode = ALIGN_D2C_HW_MODE;
+    RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
     pipeline_config_->setAlignMode(align_mode);
     RCLCPP_INFO_STREAM(logger_, "enable depth scale " << (enable_depth_scale_ ? "ON" : "OFF"));
     pipeline_config_->setDepthScaleRequire(enable_depth_scale_);
@@ -1417,6 +1717,39 @@ void OBCameraNode::setupCameraInfo() {
   }
 }
 
+void OBCameraNode::setupImagePublisher(const stream_index_pair &stream_index) {
+  if (!enable_stream_[stream_index]) {
+    image_publishers_.erase(stream_index);
+    compressed_image_publishers_.erase(stream_index);
+    return;
+  }
+
+  const std::string topic = stream_name_[stream_index] + "/image_raw";
+  auto image_qos_profile = getRMWQosProfileFromString(image_qos_[stream_index]);
+  if (use_intra_process_) {
+    image_qos_profile = rmw_qos_profile_default;
+  }
+
+  const bool is_mjpg_color_stream =
+      stream_index == COLOR && format_[stream_index] == OB_FORMAT_MJPG;
+  if (use_intra_process_ || is_mjpg_color_stream) {
+    image_publishers_[stream_index] =
+        std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
+  } else {
+    image_publishers_[stream_index] =
+        std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
+  }
+
+  if (is_mjpg_color_stream) {
+    compressed_image_publishers_[stream_index] =
+        node_->create_publisher<sensor_msgs::msg::CompressedImage>(
+            topic + "/compressed",
+            rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile), image_qos_profile));
+  } else {
+    compressed_image_publishers_.erase(stream_index);
+  }
+}
+
 void OBCameraNode::setupPublishers() {
   using PointCloud2 = sensor_msgs::msg::PointCloud2;
   using CameraInfo = sensor_msgs::msg::CameraInfo;
@@ -1443,30 +1776,9 @@ void OBCameraNode::setupPublishers() {
       continue;
     }
     std::string name = stream_name_[stream_index];
-    std::string topic = name + "/image_raw";
-    auto image_qos = image_qos_[stream_index];
-    auto image_qos_profile = getRMWQosProfileFromString(image_qos);
-    if (use_intra_process_) {
-      image_qos_profile = rmw_qos_profile_default;
-    }
-    const bool is_mjpg_color_stream =
-        stream_index == COLOR && format_[stream_index] == OB_FORMAT_MJPG;
-    if (use_intra_process_ || is_mjpg_color_stream) {
-      image_publishers_[stream_index] =
-          std::make_shared<image_rcl_publisher>(*node_, topic, image_qos_profile);
-    } else {
-      image_publishers_[stream_index] =
-          std::make_shared<image_transport_publisher>(*node_, topic, image_qos_profile);
-    }
-    if (is_mjpg_color_stream) {
-      compressed_image_publishers_[stream_index] =
-          node_->create_publisher<sensor_msgs::msg::CompressedImage>(
-              topic + "/compressed",
-              rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(image_qos_profile),
-                          image_qos_profile));
-    }
+    setupImagePublisher(stream_index);
 
-    topic = name + "/camera_info";
+    std::string topic = name + "/camera_info";
     auto camera_info_qos = camera_info_qos_[stream_index];
     auto camera_info_qos_profile = getRMWQosProfileFromString(camera_info_qos);
     if (use_intra_process_) {
@@ -1483,6 +1795,10 @@ void OBCameraNode::setupPublishers() {
                           camera_info_qos_profile));
     }
     if (stream_index == COLOR && enable_color_undistortion_) {
+      auto image_qos_profile = getRMWQosProfileFromString(image_qos_[stream_index]);
+      if (use_intra_process_) {
+        image_qos_profile = rmw_qos_profile_default;
+      }
       if (use_intra_process_) {
         color_undistortion_publisher_ = std::make_shared<image_rcl_publisher>(
             *node_, "color/image_undistorted", image_qos_profile);
@@ -1566,6 +1882,46 @@ void OBCameraNode::setupPublishers() {
   std_msgs::msg::String msg;
   msg.data = filter_status_.dump(2);
   filter_status_pub_->publish(msg);
+
+  if (enable_lrm_obstacle_distance_publish_) {
+    lrm_obstacle_distance_pub_ =
+        node_->create_publisher<std_msgs::msg::Int32>("lrm/obstacle_distance", rclcpp::QoS(10));
+    RCLCPP_INFO_STREAM(logger_, "Publishing LRM obstacle distance on lrm/obstacle_distance at "
+                                    << lrm_obstacle_distance_publish_rate_ << " Hz");
+    auto publish_period = std::chrono::duration_cast<std::chrono::milliseconds>(
+        std::chrono::duration<double>(1.0 / lrm_obstacle_distance_publish_rate_));
+    if (publish_period < std::chrono::milliseconds(1)) {
+      publish_period = std::chrono::milliseconds(1);
+    }
+    lrm_obstacle_distance_timer_ =
+        node_->create_wall_timer(publish_period, [this]() { publishLrmObstacleDistance(); });
+  }
+}
+
+void OBCameraNode::publishLrmObstacleDistance() {
+  if (!lrm_obstacle_distance_pub_) {
+    return;
+  }
+  if (lrm_obstacle_distance_pub_->get_subscription_count() == 0) {
+    return;
+  }
+  try {
+    std_msgs::msg::Int32 msg;
+    {
+      std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+      msg.data = device_->getIntProperty(OB_PROP_LDP_MEASURE_DISTANCE_INT);
+    }
+    lrm_obstacle_distance_pub_->publish(msg);
+  } catch (const ob::Error &e) {
+    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
+                         "Failed to publish LRM obstacle distance: %s", e.getMessage());
+  } catch (const std::exception &e) {
+    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
+                         "Failed to publish LRM obstacle distance: %s", e.what());
+  } catch (...) {
+    RCLCPP_WARN_THROTTLE(logger_, *node_->get_clock(), 5000,
+                         "Failed to publish LRM obstacle distance: unknown error");
+  }
 }
 
 void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
@@ -1893,21 +2249,51 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       if (depth_frame) {
         ob::FrameHelper::pushFrame(frame_set, OB_FRAME_DEPTH, depth_frame);
       }
-      if (depth_registration_ && align_filter_ && depth_frame && has_first_color_frame_) {
+    }
+    if (depth_registration_ && align_filter_ && depth_frame && color_frame) {
+      auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
+      if (frame_set->getFrame(target_frame_type)) {
+        if (align_target_stream_ == OB_STREAM_DEPTH) {
+          ob::FormatConvertFilter align_color_format_convert_filter;
+          bool need_convert = true;
+          switch (color_frame->format()) {
+            case OB_FORMAT_YUYV:
+              align_color_format_convert_filter.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
+              break;
+            case OB_FORMAT_UYVY:
+              align_color_format_convert_filter.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
+              break;
+            case OB_FORMAT_MJPG:
+              align_color_format_convert_filter.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
+              break;
+            default:
+              need_convert = false;
+              break;
+          }
+          if (need_convert) {
+            auto converted_color_frame = align_color_format_convert_filter.process(color_frame);
+            if (converted_color_frame) {
+              ob::FrameHelper::pushFrame(frame_set, OB_FRAME_COLOR, converted_color_frame);
+              color_frame = converted_color_frame;
+            } else {
+              RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *(node_->get_clock()), 1000,
+                                              "Failed to convert color frame for C2D alignment");
+            }
+          }
+        }
         auto new_frame = align_filter_->process(frame_set);
         if (new_frame) {
           auto new_frame_set = new_frame->as<ob::FrameSet>();
           CHECK_NOTNULL(new_frame_set.get());
           frame_set = new_frame_set;
+          depth_frame = frame_set->getFrame(OB_FRAME_DEPTH);
           color_frame = frame_set->getFrame(OB_FRAME_COLOR);
+          has_first_color_frame_ = has_first_color_frame_ || color_frame;
         } else {
-          RCLCPP_ERROR(logger_, "Failed to align depth frame to color frame");
-          return;
+          RCLCPP_ERROR(logger_, "Failed to align frame set");
         }
       } else {
-        RCLCPP_DEBUG(logger_,
-                     "Depth registration is disabled or align filter is null or depth frame is "
-                     "null or color frame is null");
+        RCLCPP_DEBUG(logger_, "Depth registration target frame is null, skip software alignment");
       }
     }
     if (enable_stream_[COLOR] && color_frame) {
@@ -1946,12 +2332,15 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
 }
 
 void OBCameraNode::onNewColorFrameCallback() {
-  while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load()) {
+  while (enable_stream_[COLOR] && rclcpp::ok() && is_running_.load() &&
+         !stop_color_frame_thread_.load()) {
     std::unique_lock<std::mutex> lock(color_frame_queue_lock_);
-    color_frame_queue_cv_.wait(
-        lock, [this]() { return !color_frame_queue_.empty() || !(is_running_.load()); });
+    color_frame_queue_cv_.wait(lock, [this]() {
+      return !color_frame_queue_.empty() || !(is_running_.load()) ||
+             stop_color_frame_thread_.load();
+    });
 
-    if (!rclcpp::ok() || !is_running_.load()) {
+    if (!rclcpp::ok() || !is_running_.load() || stop_color_frame_thread_.load()) {
       break;
     }
 
@@ -1973,10 +2362,12 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
   if (frame->format() == OB_FORMAT_RGB || frame->format() == OB_FORMAT_BGR) {
     return frame;
   }
-  if (frame->format() == OB_FORMAT_RGB || frame->format() == OB_FORMAT_BGR) {
-    return frame;
-  }
-  if (frame->format() == OB_FORMAT_Y16 || frame->format() == OB_FORMAT_Y8) {
+  if (frame->format() == OB_FORMAT_RGB888 || frame->format() == OB_FORMAT_RGBA ||
+      frame->format() == OB_FORMAT_BGRA || frame->format() == OB_FORMAT_Y8 ||
+      frame->format() == OB_FORMAT_GRAY || frame->format() == OB_FORMAT_Y10 ||
+      frame->format() == OB_FORMAT_Y11 || frame->format() == OB_FORMAT_Y12 ||
+      frame->format() == OB_FORMAT_Y14 || frame->format() == OB_FORMAT_Y16 ||
+      frame->format() == OB_FORMAT_Z16 || frame->format() == OB_FORMAT_RW16) {
     return frame;
   }
   if (!setupFormatConvertType(frame->format())) {
@@ -1992,14 +2383,32 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
   return color_frame;
 }
 
+bool OBCameraNode::isColorFrameDecodeRequired(const std::shared_ptr<ob::Frame> &frame) const {
+  if (frame == nullptr) {
+    return false;
+  }
+  const auto format = frame->format();
+  if (format == OB_FORMAT_RGB || format == OB_FORMAT_BGR || format == OB_FORMAT_RGB888 ||
+      format == OB_FORMAT_RGBA || format == OB_FORMAT_BGRA || format == OB_FORMAT_Y8 ||
+      format == OB_FORMAT_GRAY || format == OB_FORMAT_Y10 || format == OB_FORMAT_Y11 ||
+      format == OB_FORMAT_Y12 || format == OB_FORMAT_Y14 || format == OB_FORMAT_Y16 ||
+      format == OB_FORMAT_Z16 || format == OB_FORMAT_RW16) {
+    return false;
+  }
+  return true;
+}
+
 bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &frame,
                                             uint8_t *buffer) {
   if (frame == nullptr) {
     return false;
   }
-  if (!rgb_buffer_) {
+  if (!buffer) {
     return false;
   }
+  if (!isColorFrameDecodeRequired(frame)) {
+    return true;
+  }
   CHECK_NOTNULL(image_publishers_[COLOR]);
   bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
   if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
@@ -2044,6 +2453,12 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
       RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
       return false;
     }
+    if (video_frame->dataSize() > rgb_buffer_size_) {
+      delete[] rgb_buffer_;
+      rgb_buffer_size_ = video_frame->dataSize();
+      rgb_buffer_ = new uint8_t[rgb_buffer_size_];
+      buffer = rgb_buffer_;
+    }
     CHECK_NOTNULL(buffer);
     memcpy(buffer, video_frame->data(), video_frame->dataSize());
     return true;
@@ -2247,12 +2662,15 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   if (image.empty() || image.cols != width || image.rows != height) {
     image.create(height, width, image_format_[stream_index]);
   }
-  if (frame->type() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
+  const bool use_decoded_color_buffer =
+      frame->type() == OB_FRAME_COLOR && isColorFrameDecodeRequired(frame);
+  if (use_decoded_color_buffer && !is_color_frame_decoded_) {
     RCLCPP_ERROR(logger_, "color frame is not decoded");
     return;
   }
-  if (frame->type() == OB_FRAME_COLOR) {
-    memcpy(image.data, rgb_buffer_, video_frame->width() * video_frame->height() * 3);
+  if (use_decoded_color_buffer) {
+    memcpy(image.data, rgb_buffer_,
+           video_frame->width() * video_frame->height() * unit_step_size_[stream_index]);
   } else {
     memcpy(image.data, video_frame->data(), video_frame->dataSize());
   }
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index e675f936..699af089 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -15,6 +15,8 @@
  *******************************************************************************/
 
 #include "orbbec_camera/ob_camera_node.h"
+#include <algorithm>
+#include <cctype>
 #include <rclcpp/rclcpp.hpp>
 #include <nlohmann/json.hpp>
 #include <thread>
@@ -172,11 +174,21 @@ void OBCameraNode::setupCameraCtrlServices() {
                           std::shared_ptr<SetString::Response> response) {
         switchIRCameraCallback(request, response);
       });
+  set_image_registration_mode_srv_ = node_->create_service<SetString>(
+      "set_image_registration_mode", [this](const std::shared_ptr<SetString::Request> request,
+                                            std::shared_ptr<SetString::Response> response) {
+        setImageRegistrationModeCallback(request, response);
+      });
   set_ir_long_exposure_srv_ = node_->create_service<SetBool>(
       "set_ir_long_exposure", [this](const std::shared_ptr<SetBool::Request> request,
                                      std::shared_ptr<SetBool::Response> response) {
         setIRLongExposureCallback(request, response);
       });
+  set_stream_profile_srv_ = node_->create_service<SetStreamProfile>(
+      "set_stream_profile", [this](const std::shared_ptr<SetStreamProfile::Request> request,
+                                   std::shared_ptr<SetStreamProfile::Response> response) {
+        setStreamProfileCallback(request, response);
+      });
   get_ldp_measure_distance_srv_ = node_->create_service<GetInt32>(
       "get_ldp_measure_distance", [this](const std::shared_ptr<GetInt32::Request> request,
                                          std::shared_ptr<GetInt32::Response> response) {
@@ -756,6 +768,117 @@ bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enab
   }
 }
 
+void OBCameraNode::setImageRegistrationModeCallback(
+    const std::shared_ptr<SetString::Request> request,
+    std::shared_ptr<SetString::Response> response) {
+  auto mode = request->data;
+  std::transform(mode.begin(), mode.end(), mode.begin(),
+                 [](unsigned char ch) { return static_cast<char>(std::toupper(ch)); });
+  if (mode != "OFF" && mode != "HW_D2C" && mode != "SW_D2C" && mode != "SW_C2D") {
+    response->success = false;
+    response->message = "Invalid image registration mode '" + request->data +
+                        "'. Valid values: OFF, HW_D2C, SW_D2C, SW_C2D";
+    return;
+  }
+
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+
+  if (mode != "OFF" && (!enable_stream_[COLOR] || !enable_stream_[DEPTH])) {
+    response->success = false;
+    response->message =
+        "Image registration mode " + mode + " requires both color and depth streams to be enabled";
+    return;
+  }
+
+  const bool old_depth_registration = depth_registration_;
+  const std::string old_align_mode = align_mode_;
+  const OBStreamType old_align_target_stream = align_target_stream_;
+  const bool was_running = pipeline_started_.load();
+
+  auto mode_from_state = [](bool depth_registration, const std::string& align_mode,
+                            OBStreamType align_target_stream) {
+    if (!depth_registration) {
+      return std::string("OFF");
+    }
+    if (align_mode == "HW") {
+      return std::string("HW_D2C");
+    }
+    return align_target_stream == OB_STREAM_DEPTH ? std::string("SW_C2D") : std::string("SW_D2C");
+  };
+  const auto old_mode =
+      mode_from_state(old_depth_registration, old_align_mode, old_align_target_stream);
+
+  auto apply_image_registration_mode = [this](const std::string& mode) {
+    if (mode == "OFF") {
+      depth_registration_ = false;
+      align_mode_ = "HW";
+      align_target_stream_ = OB_STREAM_COLOR;
+    } else if (mode == "HW_D2C") {
+      depth_registration_ = true;
+      align_mode_ = "HW";
+      align_target_stream_ = OB_STREAM_COLOR;
+    } else {
+      depth_registration_ = true;
+      align_mode_ = "SW";
+      align_target_stream_ = mode == "SW_C2D" ? OB_STREAM_DEPTH : OB_STREAM_COLOR;
+    }
+    align_filter_.reset();
+    syncSoftwareAlignment();
+  };
+
+  auto restore_old_mode = [this, old_depth_registration, old_align_mode,
+                           old_align_target_stream]() {
+    depth_registration_ = old_depth_registration;
+    align_mode_ = old_align_mode;
+    align_target_stream_ = old_align_target_stream;
+    align_filter_.reset();
+    syncSoftwareAlignment();
+  };
+
+  auto rollback_after_error = [&](const std::string& error_message) {
+    try {
+      restore_old_mode();
+      if (was_running && !pipeline_started_.load()) {
+        startStreams();
+      }
+      response->message = "Failed to set image registration mode to " + mode + ": " +
+                          error_message + ". Rolled back to " + old_mode;
+    } catch (const std::exception& rollback_error) {
+      response->message = "Failed to set image registration mode to " + mode + ": " +
+                          error_message + ". Rollback to " + old_mode +
+                          " also failed: " + rollback_error.what();
+    } catch (...) {
+      response->message = "Failed to set image registration mode to " + mode + ": " +
+                          error_message + ". Rollback to " + old_mode + " also failed";
+    }
+    response->success = false;
+  };
+
+  try {
+    if (was_running) {
+      stopStreams();
+      pipeline_started_.store(false);
+    }
+
+    apply_image_registration_mode(mode);
+
+    if (was_running) {
+      startStreams();
+      response->message = "Image registration mode changed from " + old_mode + " to " + mode +
+                          "; streams restarted";
+    } else {
+      response->message = "Image registration mode set to " + mode + "; streams remain stopped";
+    }
+    response->success = true;
+  } catch (const ob::Error& e) {
+    rollback_after_error(e.getMessage());
+  } catch (const std::exception& e) {
+    rollback_after_error(e.what());
+  } catch (...) {
+    rollback_after_error("unknown error");
+  }
+}
+
 void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
                                      std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
   (void)request;
@@ -821,4 +944,34 @@ void OBCameraNode::setIRLongExposureCallback(
     response->success = false;
   }
 }
+
+void OBCameraNode::setStreamProfileCallback(
+    const std::shared_ptr<SetStreamProfile::Request>& request,
+    std::shared_ptr<SetStreamProfile::Response>& response) {
+  try {
+    std::vector<PendingStreamProfile> pending_profiles;
+    std::string message;
+    if (!validateStreamProfileRequest(request, pending_profiles, message)) {
+      response->success = false;
+      response->message = message;
+      return;
+    }
+    if (!applyStreamProfiles(pending_profiles, message)) {
+      response->success = false;
+      response->message = message;
+      return;
+    }
+    response->success = true;
+    response->message = message;
+  } catch (const ob::Error& e) {
+    response->success = false;
+    response->message = e.getMessage();
+  } catch (const std::exception& e) {
+    response->success = false;
+    response->message = e.what();
+  } catch (...) {
+    response->success = false;
+    response->message = "unknown error";
+  }
+}
 }  // namespace orbbec_camera
diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt
index 22a1b05e..e462cc66 100644
--- a/orbbec_camera_msgs/CMakeLists.txt
+++ b/orbbec_camera_msgs/CMakeLists.txt
@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Metadata.msg"
   "msg/IMUInfo.msg"
   "msg/RGBD.msg"
+  "msg/StreamProfile.msg"
   "srv/GetBool.srv"
   "srv/GetDeviceInfo.srv"
   "srv/GetCameraInfo.srv"
@@ -27,6 +28,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/GetString.srv"
   "srv/SetInt32.srv"
   "srv/SetString.srv"
+  "srv/SetStreamProfile.srv"
   DEPENDENCIES
   sensor_msgs
   std_msgs)
diff --git a/orbbec_camera_msgs/msg/StreamProfile.msg b/orbbec_camera_msgs/msg/StreamProfile.msg
new file mode 100644
index 00000000..64026a72
--- /dev/null
+++ b/orbbec_camera_msgs/msg/StreamProfile.msg
@@ -0,0 +1,5 @@
+string stream_name
+int32 width
+int32 height
+int32 fps
+string format
diff --git a/orbbec_camera_msgs/package.xml b/orbbec_camera_msgs/package.xml
index 136e5ad4..f3c7fdc9 100644
--- a/orbbec_camera_msgs/package.xml
+++ b/orbbec_camera_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_camera_msgs</name>
-  <version>1.5.21</version>
+  <version>1.5.22</version>
   <description>A package containing orbbec camera messages definitions.</description>
   <maintainer email="mocun@orbbec.com">Joe Dong</maintainer>
   <license>Apache-2.0</license>
diff --git a/orbbec_camera_msgs/srv/SetStreamProfile.srv b/orbbec_camera_msgs/srv/SetStreamProfile.srv
new file mode 100644
index 00000000..e204d2fc
--- /dev/null
+++ b/orbbec_camera_msgs/srv/SetStreamProfile.srv
@@ -0,0 +1,4 @@
+StreamProfile[] profiles
+---
+bool success
+string message
diff --git a/orbbec_description/package.xml b/orbbec_description/package.xml
index fb7978cc..7227e7ff 100644
--- a/orbbec_description/package.xml
+++ b/orbbec_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_description</name>
-  <version>1.5.21</version>
+  <version>1.5.22</version>
   <description>TODO: Package description</description>
   <maintainer email="jian.dong@pobox.com">toosimple</maintainer>
   <license>Apache-2.0</license>