Fix point cloud direction

orbbec_camera/src/ob_camera_node.cpp

@@ -158,10 +158,13 @@ void OBCameraNode::setupProfiles() {
 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;
   }
   pipeline_ = std::make_unique<ob::Pipeline>(device_);
   pipeline_->start(config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
@@ -189,17 +192,17 @@ void OBCameraNode::getParameters() {
     depth_aligned_frame_id_[stream_index] = stream_name_[OB_STREAM_COLOR] + "_optical_frame";
   }
   setAndGetNodeParameter(publish_tf_, "publish_tf", true);
-  setAndGetNodeParameter(align_depth_, "align_depth", true);
   setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 10.0);
   setAndGetNodeParameter(publish_rgb_point_cloud_, "publish_rgb_point_cloud", false);
   setAndGetNodeParameter(d2c_mode_, "d2c_mode_", DEFAULT_D2C_MODE);
+  setAndGetNodeParameter(camera_link_frame_id_, "camera_link_frame_id", DEFAULT_BASE_FRAME_ID);
 }
 
 void OBCameraNode::setupTopics() {
   getParameters();
   setupDevices();
-  updateStreamCalibData();
   setupProfiles();
+  setupDefaultStreamCalibData();
   setupCameraCtrlServices();
   setupPublishers();
   publishStaticTransforms();
@@ -228,7 +231,7 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
     return;
   }
   try {
-    if(publish_rgb_point_cloud_) {
+    if (publish_rgb_point_cloud_) {
       if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
         publishColorPointCloud(frame_set);
       }
@@ -268,7 +271,7 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
     bool valid_pixel(points->z > 0);
     if (valid_pixel) {
       *iter_x = static_cast<float>(points->x / 1000.0);
-      *iter_y = static_cast<float>(points->y / 1000.0);
+      *iter_y = -static_cast<float>(points->y / 1000.0);
       *iter_z = static_cast<float>(points->z / 1000.0);
       ++iter_x;
       ++iter_y;
@@ -316,7 +319,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
     bool valid_pixel(points->z > 0);
     if (valid_pixel) {
       *iter_x = static_cast<float>(points->x / 1000.0);
-      *iter_y = static_cast<float>(points->y / 1000.0);
+      *iter_y = -static_cast<float>(points->y / 1000.0);
       *iter_z = static_cast<float>(points->z / 1000.0);
       *iter_r = static_cast<uint8_t>(points->r);
       *iter_g = static_cast<uint8_t>(points->g);
@@ -336,6 +339,7 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
   point_cloud_publisher_->publish(point_cloud_msg_);
 }
+
 void OBCameraNode::frameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
   auto color_frame = frame_set->colorFrame();
   auto depth_frame = frame_set->depthFrame();
@@ -351,6 +355,7 @@ void OBCameraNode::frameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
   }
   publishPointCloud(frame_set);
 }
+
 std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
   auto camera_params = device_->getCalibrationCameraParamList();
   for (size_t i = 0; i < camera_params->count(); i++) {
@@ -428,11 +433,18 @@ std::optional<OBCameraParam> OBCameraNode::findCameraParam(uint32_t color_width,
   return {};
 }
 
-void OBCameraNode::updateStreamCalibData() {
+void OBCameraNode::setupDefaultStreamCalibData() {
   auto param = findDefaultCameraParam();
-  CHECK(param.has_value());
-  camera_infos_[DEPTH] = convertToCameraInfo(param->depthIntrinsic, param->depthDistortion);
-  camera_infos_[COLOR] = convertToCameraInfo(param->rgbIntrinsic, param->rgbDistortion);
+  if (!param.has_value()) {
+    RCLCPP_WARN_STREAM(logger_, "Not Found default camera parameter");
+    return;
+  }
+  updateStreamCalibData(*param);
+}
+
+void OBCameraNode::updateStreamCalibData(const OBCameraParam& param) {
+  camera_infos_[DEPTH] = convertToCameraInfo(param.depthIntrinsic, param.depthDistortion);
+  camera_infos_[COLOR] = convertToCameraInfo(param.rgbIntrinsic, param.rgbDistortion);
   camera_infos_[INFRA0] = camera_infos_[DEPTH];
 }
 
@@ -468,12 +480,12 @@ void OBCameraNode::calcAndPublishStaticTransform() {
   rclcpp::Time tf_timestamp = node_->now();
 
   publishStaticTF(tf_timestamp, trans, Q, frame_id_[DEPTH], frame_id_[COLOR]);
-  publishStaticTF(tf_timestamp, trans, Q, "camera_link", frame_id_[COLOR]);
+  publishStaticTF(tf_timestamp, trans, Q, camera_link_frame_id_, frame_id_[COLOR]);
   publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[COLOR],
                   optical_frame_id_[COLOR]);
   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_[DEPTH]);
+  publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, frame_id_[DEPTH]);
   extrinsics_publisher_->publish(obExtrinsicsToMsg(ex, "depth_to_color_extrinsics"));
 }
 
@@ -485,6 +497,7 @@ void OBCameraNode::publishStaticTransforms() {
     static_tf_broadcaster_->sendTransform(static_tf_msgs_);
   }
 }
+
 void OBCameraNode::publishDynamicTransforms() {
   RCLCPP_WARN(logger_, "Publishing dynamic camera transforms (/tf) at %g Hz", tf_publish_rate_);
   std::mutex mu;
@@ -545,8 +558,9 @@ void OBCameraNode::publishColorFrame(std::shared_ptr<ob::ColorFrame> frame) {
   auto& camera_info_publisher = camera_info_publishers_.at(stream);
   auto& image_publisher = image_publishers_.at(stream);
   auto& cam_info = camera_infos_.at(stream);
-  if (cam_info.width != width) {
+  if (cam_info.width != width || cam_info.height != height) {
     RCLCPP_ERROR(logger_, "cam info error");
+    updateStreamCalibData(pipeline_->getCameraParam());
     cam_info.height = height;
     cam_info.width = width;
   }
@@ -577,8 +591,9 @@ void OBCameraNode::publishDepthFrame(std::shared_ptr<ob::DepthFrame> frame) {
   auto& camera_info_publisher = camera_info_publishers_.at(stream);
   auto& image_publisher = image_publishers_.at(stream);
   auto& cam_info = camera_infos_.at(stream);
-  if (cam_info.width != width) {
+  if (cam_info.width != width || cam_info.height != height) {
     RCLCPP_ERROR(logger_, "cam info error");
+    updateStreamCalibData(pipeline_->getCameraParam());
     cam_info.height = height;
     cam_info.width = width;
   }
@@ -592,7 +607,11 @@ void OBCameraNode::publishDepthFrame(std::shared_ptr<ob::DepthFrame> frame) {
   img->height = height;
   img->is_bigendian = false;
   img->step = width * unit_step_size_[stream];
-  img->header.frame_id = optical_frame_id_[COLOR];
+  if (align_depth_) {
+    img->header.frame_id = optical_frame_id_[COLOR];
+  } else {
+    img->header.frame_id = optical_frame_id_[DEPTH];
+  }
   img->header.stamp = timestamp;
   image_publisher.publish(img);
 }
@@ -609,8 +628,9 @@ void OBCameraNode::publishIRFrame(std::shared_ptr<ob::IRFrame> frame) {
   auto& camera_info_publisher = camera_info_publishers_.at(stream);
   auto& image_publisher = image_publishers_.at(stream);
   auto& cam_info = camera_infos_.at(stream);
-  if (cam_info.width != width) {
+  if (cam_info.width != width || cam_info.height != height) {
     RCLCPP_ERROR(logger_, "cam info error");
+    updateStreamCalibData(pipeline_->getCameraParam());
     cam_info.height = height;
     cam_info.width = width;
   }
@@ -624,7 +644,11 @@ void OBCameraNode::publishIRFrame(std::shared_ptr<ob::IRFrame> frame) {
   img->height = height;
   img->is_bigendian = false;
   img->step = width * unit_step_size_[stream];
-  img->header.frame_id = optical_frame_id_[COLOR];
+  if (align_depth_) {
+    img->header.frame_id = optical_frame_id_[COLOR];
+  } else {
+    img->header.frame_id = optical_frame_id_[DEPTH];
+  }
   img->header.stamp = timestamp;
   image_publisher.publish(img);
 }

orbbec_camera/src/ob_camera_node_factory.cpp

@@ -18,6 +18,7 @@ OBCameraNodeFactory::OBCameraNodeFactory(const rclcpp::NodeOptions &node_options
       logger_(this->get_logger()) {
   init();
 }
+
 OBCameraNodeFactory::OBCameraNodeFactory(const std::string &node_name, const std::string &ns,
                                          const rclcpp::NodeOptions &node_options)
     : Node(node_name, ns, node_options),
@@ -32,8 +33,10 @@ OBCameraNodeFactory::~OBCameraNodeFactory() {
     query_thread_.join();
   }
 }
+
 void OBCameraNodeFactory::init() {
-  ctx_->setLoggerSeverity(OB_LOG_SEVERITY_NONE);
+  ob_log_level_ = declare_parameter<std::string>("ob_log_level", "none");
+  ctx_->setLoggerSeverity(obLogSeverityFromString(ob_log_level_));
   is_alive_.store(true);
   parameters_ = std::make_shared<Parameters>(this);
   serial_number_ = declare_parameter<std::string>("serial_number", "");
@@ -123,6 +126,21 @@ void OBCameraNodeFactory::printDeviceInfo(const std::shared_ptr<ob::DeviceInfo>
   RCLCPP_INFO_STREAM(logger_, "hardware version " << device_info->hardwareVersion());
 }
 
+OBLogSeverity OBCameraNodeFactory::obLogSeverityFromString(const std::string &log_level) {
+  if (log_level == "debug") {
+    return OB_LOG_SEVERITY_DEBUG;
+  } else if (log_level == "info") {
+    return OB_LOG_SEVERITY_INFO;
+  } else if (log_level == "warn" || log_level == "warning") {
+    return OB_LOG_SEVERITY_WARN;
+  } else if (log_level == "fatal" || log_level == "error") {
+    return OB_LOG_SEVERITY_FATAL;
+  } else {
+    // Default None
+    return OB_LOG_SEVERITY_NONE;
+  }
+}
+
 void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list) {
   if (device_) {
     return;

orbbec_camera/src/ros_service.cpp

@@ -25,55 +25,49 @@ void OBCameraNode::setupCameraCtrlServices() {
     if (enable_[stream_index]) {
       std::string service_name = "get_" + stream_name + "_exposure";
       get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
-          service_name, [this, stream_index = stream_index](
-                            const std::shared_ptr<rmw_request_id_t> request_header,
-                            const std::shared_ptr<GetInt32::Request> request,
-                            std::shared_ptr<GetInt32::Response> response) {
+          service_name,
+          [this, stream_index = stream_index](const std::shared_ptr<GetInt32::Request> request,
+                                              std::shared_ptr<GetInt32::Response> response) {
             getExposureCallback(request, response, stream_index);
           });
 
       service_name = "set_" + stream_name + "_exposure";
       set_exposure_srv_[stream_index] = node_->create_service<SetInt32>(
-          service_name, [this, stream_index = stream_index](
-                            const std::shared_ptr<rmw_request_id_t> request_header,
-                            const std::shared_ptr<SetInt32::Request> request,
-                            std::shared_ptr<SetInt32::Response> response) {
+          service_name,
+          [this, stream_index = stream_index](const std::shared_ptr<SetInt32::Request> request,
+                                              std::shared_ptr<SetInt32::Response> response) {
             setExposureCallback(request, response, stream_index);
           });
       service_name = "get_" + stream_name + "_gain";
       get_gain_srv_[stream_index] = node_->create_service<GetInt32>(
-          service_name, [this, stream_index = stream_index](
-                            const std::shared_ptr<rmw_request_id_t> request_header,
-                            const std::shared_ptr<GetInt32::Request> request,
-                            std::shared_ptr<GetInt32::Response> response) {
+          service_name,
+          [this, stream_index = stream_index](const std::shared_ptr<GetInt32::Request> request,
+                                              std::shared_ptr<GetInt32::Response> response) {
             getGainCallback(request, response, stream_index);
           });
 
       service_name = "set_" + stream_name + "_gain";
       set_gain_srv_[stream_index] = node_->create_service<SetInt32>(
-          service_name, [this, stream_index = stream_index](
-                            const std::shared_ptr<rmw_request_id_t> request_header,
-                            const std::shared_ptr<SetInt32::Request> request,
-                            std::shared_ptr<SetInt32::Response> response) {
+          service_name,
+          [this, stream_index = stream_index](const std::shared_ptr<SetInt32::Request> request,
+                                              std::shared_ptr<SetInt32::Response> response) {
             setGainCallback(request, response, stream_index);
           });
       if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
         service_name = "set_" + stream_name + "_auto_exposure";
         set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
-            service_name, [this, stream_index = stream_index](
-                              const std::shared_ptr<rmw_request_id_t> request_header,
-                              const std::shared_ptr<SetBool::Request> request,
-                              std::shared_ptr<SetBool::Response> response) {
+            service_name,
+            [this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
+                                                std::shared_ptr<SetBool::Response> response) {
               setAutoExposureCallback(request, response, stream_index);
             });
       }
     }
   }
   set_fan_mode_srv_ = node_->create_service<SetInt32>(
-      "set_fan_mode", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                             const std::shared_ptr<SetInt32::Request> request,
+      "set_fan_mode", [this](const std::shared_ptr<SetInt32::Request> request,
                              std::shared_ptr<SetInt32::Response> response) {
-        setFanModeCallback(request_header, request, response);
+        setFanModeCallback(request, response);
       });
   set_floor_enable_srv_ = node_->create_service<SetBool>(
       "set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
@@ -95,30 +89,25 @@ void OBCameraNode::setupCameraCtrlServices() {
       });
 
   get_white_balance_srv_ = node_->create_service<GetInt32>(
-      "get_white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                  const std::shared_ptr<GetInt32::Request> request,
+      "get_white_balance", [this](const std::shared_ptr<GetInt32::Request> request,
                                   std::shared_ptr<GetInt32::Response> response) {
-        getWhiteBalanceCallback(request_header, request, response);
+        getWhiteBalanceCallback(request, response);
       });
 
   set_white_balance_srv_ = node_->create_service<SetInt32>(
-      "set_white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                  const std::shared_ptr<SetInt32::Request> request,
+      "set_white_balance", [this](const std::shared_ptr<SetInt32::Request> request,
                                   std::shared_ptr<SetInt32::Response> response) {
-        setWhiteBalanceCallback(request_header, request, response);
+        setWhiteBalanceCallback(request, response);
       });
   get_device_srv_ = node_->create_service<GetDeviceInfo>(
-      "get_device_info", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                const std::shared_ptr<GetDeviceInfo::Request> request,
+      "get_device_info", [this](const std::shared_ptr<GetDeviceInfo::Request> request,
                                 std::shared_ptr<GetDeviceInfo::Response> response) {
-        getDeviceInfoCallback(request_header, request, response);
-      });
-  get_api_version_srv_ = node_->create_service<GetString>(
-      "get_sdk_version", [this](const std::shared_ptr<rmw_request_id_t> request_header,
-                                const std::shared_ptr<GetString::Request> request,
-                                std::shared_ptr<GetString::Response> response) {
-        getSDKVersion(request_header, request, response);
+        getDeviceInfoCallback(request, response);
       });
+  get_sdk_version_srv_ = node_->create_service<GetString>(
+      "get_sdk_version",
+      [this](const std::shared_ptr<GetString::Request> request,
+             std::shared_ptr<GetString::Response> response) { getSDKVersion(request, response); });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -157,6 +146,7 @@ void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>&
 void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& request,
                                    std::shared_ptr<GetInt32::Response>& response,
                                    const stream_index_pair& stream_index) {
+  (void)request;
   auto stream = stream_index.first;
   try {
     switch (stream) {
@@ -218,9 +208,9 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
   }
 }
 
-void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
-                                           const std::shared_ptr<GetInt32::Request>& request,
+void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
                                            std::shared_ptr<GetInt32::Response>& response) {
+  (void)request;
   try {
     response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
     response->success = true;
@@ -236,8 +226,7 @@ void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_
   }
 }
 
-void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
-                                           const std::shared_ptr<SetInt32 ::Request>& request,
+void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Request>& request,
                                            std::shared_ptr<SetInt32 ::Response>& response) {
   try {
     device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
@@ -289,10 +278,8 @@ void OBCameraNode::setAutoExposureCallback(
   }
 }
 
-void OBCameraNode::setFanModeCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
-                                      const std::shared_ptr<SetInt32::Request>& request,
+void OBCameraNode::setFanModeCallback(const std::shared_ptr<SetInt32::Request>& request,
                                       std::shared_ptr<SetInt32::Response>& response) {
-  (void)request_header;
   (void)response;
   bool fan_mode = request->data;
   try {
@@ -379,6 +366,7 @@ void OBCameraNode::setLdpEnableCallback(
 void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
                                        std::shared_ptr<GetInt32 ::Response>& response,
                                        const stream_index_pair& stream_index) {
+  (void)request;
   auto stream = stream_index.first;
   try {
     switch (stream) {
@@ -408,9 +396,9 @@ void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>&
   }
 }
 
-void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
-                                         const std::shared_ptr<GetDeviceInfo::Request>& request,
+void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Request>& request,
                                          std::shared_ptr<GetDeviceInfo::Response>& response) {
+  (void)request;
   try {
     auto device_info = device_->getDeviceInfo();
     response->info.name = device_info->name();
@@ -432,9 +420,9 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<rmw_request_id_t>
   }
 }
 
-void OBCameraNode::getSDKVersion(const std::shared_ptr<rmw_request_id_t>& request_header,
-                                 const std::shared_ptr<GetString::Request>& request,
+void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& request,
                                  std::shared_ptr<GetString::Response>& response) {
+  (void)request;
   try {
     auto device_info = device_->getDeviceInfo();
     nlohmann::json data;