add OBFormatFromString

orbbec_camera/include/orbbec_camera/utils.h

@@ -42,4 +42,6 @@ rclcpp::Time frameTimeStampToROSTime(uint64_t ms);
 
 std::string getObSDKVersion();
 
+OBFormat OBFormatFromString(const std::string& format);
+
 }  // namespace orbbec_camera

orbbec_camera/params/ob_camera_params.yaml

@@ -2,19 +2,19 @@
   ros__parameters:
     color_width: 640
     color_height: 480
-    color_fps: 30
+    color_fps: 30.0
     color_frame_id: "color_frame"
     color_optical_frame_id: "color_optical_frame"
     enable_color: true
     ir_width: 640
     ir_height: 480
-    ir_fps: 30
+    ir_fps: 30.0
     ir_frame_id: "ir_frame"
     ir_optical_frame_id: "ir_optical_frame"
-    enable_ir: false
+    enable_ir: true
     depth_width: 640
     depth_height: 480
-    depth_fps: 30
+    depth_fps: 30.0
     depth_frame_id: "depth_frame"
     depth_optical_frame_id: "depth_optical_frame"
     enable_depth: true

orbbec_camera/src/ob_camera_node.cpp

@@ -40,7 +40,7 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
   if (device_pid == FEMTO_PID || device_pid == FEMTO_LIVE_PID || device_pid == FEMTO_OW_PID) {
     format_[COLOR] = OB_FORMAT_I420;
   } else if (device_pid == ASTRA_PLUS_PID || device_pid == ASTRA_PLUS_S_PID) {
-    format_[COLOR] = OB_FORMAT_MJPG;
+    format_[COLOR] = OB_FORMAT_YUYV;
   } else {
     // default RGB888
     format_[COLOR] = OB_FORMAT_RGB888;
@@ -307,9 +307,8 @@ void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_se
 void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
   auto depth_frame = frame_set->depthFrame();
   auto color_frame = frame_set->colorFrame();
-  auto camera_param = findCameraParam(color_frame->width(), color_frame->height(),
+  auto camera_param = pipeline_->getCameraParam();
-                                      depth_frame->width(), depth_frame->height());
+  point_cloud_filter_.setCameraParam(camera_param);
-  point_cloud_filter_.setCameraParam(*camera_param);
   point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
   auto frame = point_cloud_filter_.process(frame_set);
   size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
@@ -321,7 +320,6 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
   point_cloud_msg_.width = color_frame->width();
   point_cloud_msg_.height = color_frame->height();
   std::string format_str = "rgb";
-
   point_cloud_msg_.point_step =
       addPointField(point_cloud_msg_, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32,
                     point_cloud_msg_.point_step);

orbbec_camera/src/utils.cpp

@@ -150,5 +150,59 @@ std::string getObSDKVersion() {
   std::string version = major + "." + minor + "." + patch;
   return version;
 }
+OBFormat OBFormatFromString(const std::string &format) {
+  std::string fixed_format;
+  std::transform(format.begin(), format.end(), std::back_inserter(fixed_format),
+                 [](const auto ch) { return std::isalpha(ch) ? toupper(ch) : ch; });
+  if (fixed_format == "YUYV") {
+    return OB_FORMAT_YUYV;
+  } else if (fixed_format == "YUYV2") {
+    return OB_FORMAT_YUY2;
+  } else if (fixed_format == "UYVY") {
+    return OB_FORMAT_UYVY;
+  } else if (fixed_format == "NV12") {
+    return OB_FORMAT_NV12;
+  } else if (fixed_format == "NV21") {
+    return OB_FORMAT_NV21;
+  } else if (fixed_format == "H264") {
+    return OB_FORMAT_H264;
+  } else if (fixed_format == "H265") {
+    return OB_FORMAT_H265;
+  } else if (fixed_format == "Y16") {
+    return OB_FORMAT_Y16;
+  } else if (fixed_format == "Y8") {
+    return OB_FORMAT_Y8;
+  } else if (fixed_format == "Y10") {
+    return OB_FORMAT_Y10;
+  } else if (fixed_format == "Y11") {
+    return OB_FORMAT_Y11;
+  } else if (fixed_format == "Y12") {
+    return OB_FORMAT_Y12;
+  } else if (fixed_format == "GRAY") {
+    return OB_FORMAT_GRAY;
+  } else if (fixed_format == "HEVC") {
+    return OB_FORMAT_HEVC;
+  } else if (fixed_format == "I420") {
+    return OB_FORMAT_I420;
+  } else if (fixed_format == "ACCEL") {
+    return OB_FORMAT_ACCEL;
+  } else if (fixed_format == "GYRO") {
+    return OB_FORMAT_GYRO;
+  } else if (fixed_format == "POINT") {
+    return OB_FORMAT_POINT;
+  } else if (fixed_format == "RGB_POINT") {
+    return OB_FORMAT_RGB_POINT;
+  } else if (fixed_format == "REL") {
+    return OB_FORMAT_RLE;
+  } else if (fixed_format == "RGB888") {
+    return OB_FORMAT_RGB888;
+  } else if (fixed_format == "BGR") {
+    return OB_FORMAT_BGR;
+  } else if (fixed_format == "Y14") {
+    return OB_FORMAT_Y14;
+  } else {
+    return OB_FORMAT_UNKNOWN;
+  }
+}
 
 }  // namespace orbbec_camera