fixed save point cloud

Author: jian-dong

orbbec_camera/include/orbbec_camera/utils.h

@@ -34,7 +34,9 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
 
 void saveRGBPointsToPly(const std::shared_ptr<ob::Frame>& frame, const std::string& fileName);
 
-void soavePointCloudMsgToPly(const sensor_msgs::msg::PointCloud2& msg, const std::string& fileName);
+void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2& msg, const std::string& fileName);
+
+void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2& msg, const std::string& fileName);
 
 void savePointsToPly(const std::shared_ptr<ob::Frame>& frame, const std::string& fileName);
 

orbbec_camera/launch/astra.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
         DeclareLaunchArgument('connection_delay', default_value='100'),
         DeclareLaunchArgument('color_width', default_value='640'),
         DeclareLaunchArgument('color_height', default_value='480'),
-        DeclareLaunchArgument('color_fps', default_value='30'),
+        DeclareLaunchArgument('color_fps', default_value='10'),
         DeclareLaunchArgument('color_format', default_value='RGB'),
         DeclareLaunchArgument('enable_color', default_value='true'),
         DeclareLaunchArgument('flip_color', default_value='false'),
@@ -32,15 +32,15 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
         DeclareLaunchArgument('depth_width', default_value='640'),
         DeclareLaunchArgument('depth_height', default_value='480'),
-        DeclareLaunchArgument('depth_fps', default_value='30'),
+        DeclareLaunchArgument('depth_fps', default_value='10'),
         DeclareLaunchArgument('depth_format', default_value='Y11'),
         DeclareLaunchArgument('enable_depth', default_value='true'),
         DeclareLaunchArgument('flip_depth', default_value='false'),
         DeclareLaunchArgument('depth_qos', default_value='default'),
         DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
         DeclareLaunchArgument('ir_width', default_value='640'),
         DeclareLaunchArgument('ir_height', default_value='480'),
-        DeclareLaunchArgument('ir_fps', default_value='30'),
+        DeclareLaunchArgument('ir_fps', default_value='10'),
         DeclareLaunchArgument('ir_format', default_value='Y10'),
         DeclareLaunchArgument('enable_ir', default_value='true'),
         DeclareLaunchArgument('flip_ir', default_value='false'),

orbbec_camera/launch/multi_camera.launch.py

@@ -12,22 +12,22 @@ def generate_launch_description():
     launch_file_dir = os.path.join(package_dir, 'launch')
     launch1_include = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'dabai_dcw.launch.py')
+            os.path.join(launch_file_dir, 'astra.launch.py')
         ),
         launch_arguments={
             'camera_name': 'camera_01',
-            'usb_port': '5-3.4.4.3.1',
+            'usb_port': '5-3.4.4.3',
             'device_num': '2'
         }.items()
     )
 
     launch2_include = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'dabai_dcw.launch.py')
+            os.path.join(launch_file_dir, 'astra.launch.py')
         ),
         launch_arguments={
             'camera_name': 'camera_02',
-            'usb_port': '5-3.4.4.1.1',
+            'usb_port': '5-3.4.4.1',
             'device_num': '2'
         }.items()
     )

orbbec_camera/src/ob_camera_node.cpp

@@ -623,7 +623,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
       std::filesystem::create_directory(current_path + "/point_cloud");
     }
     RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
-    soavePointCloudMsgToPly(point_cloud_msg_, filename);
+    saveDepthPointsToPly(point_cloud_msg_, filename);
   }
 }
 
@@ -733,7 +733,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
       std::filesystem::create_directory(current_path + "/point_cloud");
     }
     RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
-    soavePointCloudMsgToPly(point_cloud_msg_, filename);
+    saveRGBPointCloudMsgToPly(point_cloud_msg_, filename);
   }
 }
 

orbbec_camera/src/utils.cpp

@@ -87,8 +87,8 @@ void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::stri
   }
 }
 
-void soavePointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
-                             const std::string &fileName) {
+void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
+                               const std::string &fileName) {
   FILE *fp = fopen(fileName.c_str(), "wb+");
   CHECK_NOTNULL(fp);
 
@@ -134,6 +134,45 @@ void soavePointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
   fclose(fp);
 }
 
+void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2 &msg, const std::string &fileName) {
+  FILE *fp = fopen(fileName.c_str(), "wb+");
+  CHECK_NOTNULL(fp);
+
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(msg, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(msg, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(msg, "z");
+
+  // First, count the actual number of valid points
+  size_t valid_points = 0;
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
+      ++valid_points;
+    }
+  }
+
+  // Reset the iterators
+  iter_x = sensor_msgs::PointCloud2ConstIterator<float>(msg, "x");
+  iter_y = sensor_msgs::PointCloud2ConstIterator<float>(msg, "y");
+  iter_z = sensor_msgs::PointCloud2ConstIterator<float>(msg, "z");
+
+  fprintf(fp, "ply\n");
+  fprintf(fp, "format ascii 1.0\n");
+  fprintf(fp, "element vertex %zu\n", valid_points);
+  fprintf(fp, "property float x\n");
+  fprintf(fp, "property float y\n");
+  fprintf(fp, "property float z\n");
+  fprintf(fp, "end_header\n");
+
+  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+    if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
+      fprintf(fp, "%.3f %.3f %.3f\n", *iter_x, *iter_y, *iter_z);
+    }
+  }
+
+  fflush(fp);
+  fclose(fp);
+}
+
 void savePointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string &fileName) {
   size_t point_size = frame->dataSize() / sizeof(OBPoint);
   FILE *fp = fopen(fileName.c_str(), "wb+");