fix crash

orbbec_camera/CMakeLists.txt

@@ -7,7 +7,7 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -O3")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG}  -fPIC -g")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O3")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fPIC -g")
-set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_BUILD_TYPE "Debug")
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic -Werror)

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -147,6 +147,9 @@ class OBCameraNode {
 
   std::optional<OBCameraParam> findCameraParam(uint32_t color_width, uint32_t color_height,
                                                uint32_t depth_width, uint32_t depth_height);
+  std::optional<OBCameraParam> findDepthCameraParam(uint32_t width, uint32_t height);
+
+  std::optional<OBCameraParam> findColorCameraParam(uint32_t width, uint32_t height);
 
   void getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
                            std::shared_ptr<GetInt32::Response>& response,

orbbec_camera/params/ob_camera_params.yaml

@@ -3,27 +3,30 @@
     color_width: 640
     color_height: 480
     color_fps: 30.0
+    color_format: "RGB888"
     color_frame_id: "color_frame"
     color_optical_frame_id: "color_optical_frame"
     enable_color: true
     ir_width: 640
     ir_height: 480
     ir_fps: 30.0
+    ir_format: "Y16"
     ir_frame_id: "ir_frame"
     ir_optical_frame_id: "ir_optical_frame"
     enable_ir: true
-    depth_width: 640
+    depth_width: 1280
-    depth_height: 480
+    depth_height: 1024
     depth_fps: 30.0
+    depth_format: "Y16"
     depth_frame_id: "depth_frame"
     depth_optical_frame_id: "depth_optical_frame"
     enable_depth: true
     publish_tf: true
     tf_publish_rate: 10.0
-    publish_rgb_point_cloud: true
+    publish_rgb_point_cloud: false
     wait_for_device_timeout: 120.0
     reconnect_timeout: 6.0
-    d2c_mode: "hw"
+    d2c_mode: "none"
     serial_number: ""
     camera_link_frame_id: "camera_link"
     ob_log_level: "none"

orbbec_camera/src/ob_camera_node.cpp

@@ -116,6 +116,16 @@ void OBCameraNode::setupProfiles() {
     config_.reset();
   }
   config_ = std::make_shared<ob::Config>();
+  if (d2c_mode_ == "sw") {
+    config_->setAlignMode(ALIGN_D2C_SW_MODE);
+    align_depth_ = true;
+  } else if (d2c_mode_ == "hw") {
+    config_->setAlignMode(ALIGN_D2C_HW_MODE);
+    align_depth_ = true;
+  } else {
+    config_->setAlignMode(ALIGN_DISABLE);
+    align_depth_ = false;
+  }
   for (const auto& elem : IMAGE_STREAMS) {
     if (enable_[elem]) {
       const auto& sensor = sensors_[elem];
@@ -158,25 +168,14 @@ void OBCameraNode::setupProfiles() {
       images_[elem] =
           cv::Mat(height_[elem], width_[elem], image_format_[elem.first], cv::Scalar(0, 0, 0));
       RCLCPP_INFO_STREAM(
-          logger_, " stream is enabled - width: "
+          logger_, " stream " << stream_name_[elem.first] << " is enabled - width: " << width_[elem]
-                       << width_[elem] << ", height: " << height_[elem] << ", fps: " << fps_[elem]
+                              << ", height: " << height_[elem] << ", fps: " << fps_[elem] << ", "
-                       << ", "
                               << "Format: " << magic_enum::enum_name(selected_profile->format()));
     }
   }
 }
 
 void OBCameraNode::startPipeline() {
-  if (d2c_mode_ == "sw") {
-    config_->setAlignMode(ALIGN_D2C_SW_MODE);
-    align_depth_ = true;
-  } else if (d2c_mode_ == "hw") {
-    config_->setAlignMode(ALIGN_D2C_HW_MODE);
-    align_depth_ = true;
-  } else {
-    config_->setAlignMode(ALIGN_DISABLE);
-    align_depth_ = false;
-  }
   if (pipeline_ != nullptr) {
     pipeline_.reset();
   }
@@ -455,14 +454,40 @@ std::optional<OBCameraParam> OBCameraNode::findCameraParam(uint32_t color_width,
   return {};
 }
 
+std::optional<OBCameraParam> OBCameraNode::findDepthCameraParam(uint32_t width, uint32_t height) {
+  auto camera_params = device_->getCalibrationCameraParamList();
+  for (size_t i = 0; i < camera_params->count(); i++) {
+    auto param = camera_params->getCameraParam(i);
+    int depth_w = param.depthIntrinsic.width;
+    int depth_h = param.depthIntrinsic.height;
+    if (depth_w * height == depth_h * width) {
+      return param;
+    }
+  }
+  return {};
+}
+
+std::optional<OBCameraParam> OBCameraNode::findColorCameraParam(uint32_t width, uint32_t height) {
+  auto camera_params = device_->getCalibrationCameraParamList();
+  for (size_t i = 0; i < camera_params->count(); i++) {
+    auto param = camera_params->getCameraParam(i);
+    int color_w = param.rgbIntrinsic.width;
+    int color_h = param.rgbIntrinsic.height;
+    if (color_w * height == color_h * width) {
+      return param;
+    }
+  }
+  return {};
+}
 void OBCameraNode::setupDefaultStreamCalibData() {
   auto param = findDefaultCameraParam();
   if (!param.has_value()) {
     RCLCPP_WARN_STREAM(logger_, "Not Found default camera parameter");
     return;
-  }
+  } else {
     updateStreamCalibData(*param);
   }
+}
 
 void OBCameraNode::updateStreamCalibData(const OBCameraParam& param) {
   camera_infos_[DEPTH] = convertToCameraInfo(param.depthIntrinsic, param.depthDistortion);
@@ -489,16 +514,20 @@ void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const std::vector<floa
 }
 
 void OBCameraNode::calcAndPublishStaticTransform() {
-  tf2::Quaternion quaternion_optical, zero_rot;
+  tf2::Quaternion quaternion_optical, zero_rot, Q;
+  std::vector<float> trans(3, 0);
   zero_rot.setRPY(0.0, 0.0, 0.0);
   quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
   std::vector<float> zero_trans = {0, 0, 0};
   auto camera_param = findDefaultCameraParam();
-  CHECK(camera_param.has_value());
+  if (camera_param.has_value()) {
     auto ex = camera_param->transform;
-  auto Q = rotationMatrixToQuaternion(ex.rot);
+    Q = rotationMatrixToQuaternion(ex.rot);
     Q = quaternion_optical * Q * quaternion_optical.inverse();
-  std::vector<float> trans = {ex.trans[0], ex.trans[1], ex.trans[2]};
+    extrinsics_publisher_->publish(obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"));
+  } else {
+    Q.setRPY(0, 0, 0);
+  }
   rclcpp::Time tf_timestamp = node_->now();
 
   publishStaticTF(tf_timestamp, trans, Q, frame_id_[DEPTH], frame_id_[COLOR]);
@@ -508,7 +537,6 @@ void OBCameraNode::calcAndPublishStaticTransform() {
   publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[DEPTH],
                   optical_frame_id_[DEPTH]);
   publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[DEPTH]);
-  extrinsics_publisher_->publish(obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"));
 }
 
 void OBCameraNode::publishStaticTransforms() {
@@ -579,17 +607,18 @@ void OBCameraNode::publishColorFrame(std::shared_ptr<ob::ColorFrame> frame) {
     image.create(height, width, image.type());
   }
   image.data = (uint8_t*)frame->data();
-  auto& camera_info_publisher = camera_info_publishers_.at(stream);
+  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
-  auto& image_publisher = image_publishers_.at(stream);
+  if(camera_infos_.count(stream)) {
     auto& cam_info = camera_infos_.at(stream);
     if (cam_info.width != width || cam_info.height != height) {
       updateStreamCalibData(pipeline_->getCameraParam());
       cam_info.height = height;
       cam_info.width = width;
     }
-  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
     cam_info.header.stamp = timestamp;
+    auto& camera_info_publisher = camera_info_publishers_.at(stream);
     camera_info_publisher->publish(cam_info);
+  }
   sensor_msgs::msg::Image::SharedPtr img;
   img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding_.at(stream), image).toImageMsg();
 
@@ -599,6 +628,7 @@ void OBCameraNode::publishColorFrame(std::shared_ptr<ob::ColorFrame> frame) {
   img->step = width * unit_step_size_[stream];
   img->header.frame_id = optical_frame_id_[COLOR];
   img->header.stamp = timestamp;
+  auto& image_publisher = image_publishers_.at(stream);
   image_publisher.publish(img);
 }
 
@@ -611,17 +641,18 @@ void OBCameraNode::publishDepthFrame(std::shared_ptr<ob::DepthFrame> frame) {
     image.create(height, width, image.type());
   }
   image.data = (uint8_t*)frame->data();
-  auto& camera_info_publisher = camera_info_publishers_.at(stream);
+  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
-  auto& image_publisher = image_publishers_.at(stream);
+  if (camera_infos_.count(stream)) {
     auto& cam_info = camera_infos_.at(stream);
     if (cam_info.width != width || cam_info.height != height) {
       updateStreamCalibData(pipeline_->getCameraParam());
       cam_info.height = height;
       cam_info.width = width;
     }
-  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
     cam_info.header.stamp = timestamp;
+    auto& camera_info_publisher = camera_info_publishers_.at(stream);
     camera_info_publisher->publish(cam_info);
+  }
   sensor_msgs::msg::Image::SharedPtr img;
   img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding_.at(stream), image).toImageMsg();
 
@@ -635,6 +666,7 @@ void OBCameraNode::publishDepthFrame(std::shared_ptr<ob::DepthFrame> frame) {
     img->header.frame_id = optical_frame_id_[DEPTH];
   }
   img->header.stamp = timestamp;
+  auto& image_publisher = image_publishers_.at(stream);
   image_publisher.publish(img);
 }
 
@@ -647,17 +679,20 @@ void OBCameraNode::publishIRFrame(std::shared_ptr<ob::IRFrame> frame) {
     image.create(height, width, image.type());
   }
   image.data = (uint8_t*)frame->data();
-  auto& camera_info_publisher = camera_info_publishers_.at(stream);
+  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
   auto& image_publisher = image_publishers_.at(stream);
+  if (camera_infos_.count(stream)) {
     auto& cam_info = camera_infos_.at(stream);
+    auto& camera_info_publisher = camera_info_publishers_.at(stream);
+
     if (cam_info.width != width || cam_info.height != height) {
       updateStreamCalibData(pipeline_->getCameraParam());
       cam_info.height = height;
       cam_info.width = width;
     }
-  auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp());
     cam_info.header.stamp = timestamp;
     camera_info_publisher->publish(cam_info);
+  }
   sensor_msgs::msg::Image::SharedPtr img;
   img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding_.at(stream), image).toImageMsg();
 

orbbec_camera/src/ob_camera_node_factory.cpp

@@ -145,9 +145,7 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list)
     if (device_ == nullptr) {
       std::string lower_sn;
       std::transform(serial_number_.begin(), serial_number_.end(), std::back_inserter(lower_sn),
-                     [](auto ch) {
+                     [](auto ch) { return isalpha(ch) ? tolower(ch) : static_cast<int>(ch); });
-                       return isalpha(ch) ? tolower(ch) : static_cast<int>(ch);
-                     });
       device_ = list->getDeviceBySN(lower_sn.c_str());
     }
     if (device_ == nullptr) {

orbbec_camera/src/ros_service.cpp

@@ -105,12 +105,12 @@ void OBCameraNode::setupCameraCtrlServices() {
         setWhiteBalanceCallback(request, response);
       });
   get_auto_white_balance_srv_ = node_->create_service<GetInt32>(
-      "get_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
+      "get_auto_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
                                   std::shared_ptr<GetInt32::Response> response) {
         getAutoWhiteBalanceCallback(request, response);
       });
   set_auto_white_balance_srv_ = node_->create_service<SetBool>(
-      "set_white_balance", [this](const std::shared_ptr<SetBool::Request> request,
+      "set_auto_white_balance", [this](const std::shared_ptr<SetBool::Request> request,
                                   std::shared_ptr<SetBool::Response> response) {
         setAutoWhiteBalanceCallback(request, response);
       });