add ordered point cloud param

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -433,5 +433,6 @@ class OBCameraNode {
   std::mutex colorFrameMtx_;
   std::condition_variable colorFrameCV_;
 
+  bool ordered_pc_ = false;
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/astra.launch.py

@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/astra2.launch.py

@@ -69,6 +69,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/astra_adv.launch.py

@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/astra_embedded_s.launch.py

@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/astra_stereo_u3.launch.py

@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai.launch.py

@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai_d1.launch.py

@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai_dcw.launch.py

@@ -60,7 +60,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
-        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1')
+        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai_dcw2.launch.py

@@ -60,7 +60,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_ldp', default_value='true'),
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
-        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1')
+        DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai_dw.launch.py

@@ -49,8 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
-        DeclareLaunchArgument('jpeg_decoder', default_value='avdec_mjpeg'),
-        DeclareLaunchArgument('video_convert', default_value='videoconvert'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai_max.launch.py

@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/dabai_pro.launch.py

@@ -49,6 +49,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/deeya.launch.py

@@ -60,6 +60,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/femto.launch.py

@@ -70,6 +70,7 @@ def generate_launch_description():
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
         DeclareLaunchArgument('enable_frame_sync', default_value='false'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/femto_bolt.launch.py

@@ -75,6 +75,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger2image_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/femto_mega.launch.py

@@ -77,6 +77,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/gemini2.launch.py

@@ -81,6 +81,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/gemini2L.launch.py

@@ -81,6 +81,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/gemini2VL.launch.py

@@ -74,6 +74,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/gemini2XL.launch.py

@@ -90,6 +90,7 @@ def generate_launch_description():
         DeclareLaunchArgument('trigger_out_delay_us', default_value='0'),
         DeclareLaunchArgument('trigger_out_enabled', default_value='false'),
         DeclareLaunchArgument('enable_frame_sync', default_value='true'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/gemini_e.launch.py

@@ -61,6 +61,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/launch/gemini_e_lite.launch.py

@@ -50,6 +50,7 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_soft_filter', default_value='true'),
         DeclareLaunchArgument('soft_filter_max_diff', default_value='-1'),
         DeclareLaunchArgument('soft_filter_speckle_size', default_value='-1'),
+        DeclareLaunchArgument('ordered_pc', default_value='false'),
     ]
 
     # Node configuration

orbbec_camera/src/ob_camera_node.cpp

@@ -496,6 +496,7 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(soft_filter_speckle_size_, "soft_filter_speckle_size", -1);
   setAndGetNodeParameter<double>(liner_accel_cov_, "linear_accel_cov", 0.0003);
   setAndGetNodeParameter<double>(angular_vel_cov_, "angular_vel_cov", 0.02);
+  setAndGetNodeParameter<bool>(ordered_pc_, "ordered_pc", false);
 }
 
 void OBCameraNode::setupTopics() {
@@ -639,6 +640,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
   modifier.resize(width * height);
   point_cloud_msg_.width = depth_frame->width();
   point_cloud_msg_.height = depth_frame->height();
+  point_cloud_msg_.is_dense = false;
   point_cloud_msg_.row_step = point_cloud_msg_.width * point_cloud_msg_.point_step;
   point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
   sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
@@ -652,26 +654,31 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
   const static float max_depth = MAX_DISTANCE / depth_scale;
   for (uint32_t y = 0; y < height; y++) {
     for (uint32_t x = 0; x < width; x++) {
+      bool vaild_point = true;
       if (depth_data[y * width + x] < min_depth || depth_data[y * width + x] > max_depth) {
-        continue;
+        vaild_point = false;
+      }
+      if(vaild_point || ordered_pc_) {
+        float xf = (x - u0) * fdx;
+        float yf = (y - v0) * fdy;
+        float zf = depth_data[y * width + x] * depth_scale;
+        *iter_x = zf * xf / 1000.0;
+        *iter_y = zf * yf / 1000.0;
+        *iter_z = zf / 1000.0;
+        ++iter_x, ++iter_y, ++iter_z;
+        valid_count++;
       }
-      float xf = (x - u0) * fdx;
-      float yf = (y - v0) * fdy;
-      float zf = depth_data[y * width + x] * depth_scale;
-      *iter_x = zf * xf / 1000.0;
-      *iter_y = zf * yf / 1000.0;
-      *iter_z = zf / 1000.0;
-      ++iter_x, ++iter_y, ++iter_z;
-      valid_count++;
     }
   }
   auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
+  if(!ordered_pc_){
+    point_cloud_msg_.is_dense = true;
+    point_cloud_msg_.width = valid_count;
+    point_cloud_msg_.height = 1;
+    modifier.resize(valid_count);
+  }
   point_cloud_msg_.header.stamp = timestamp;
   point_cloud_msg_.header.frame_id = optical_frame_id_[DEPTH];
-  point_cloud_msg_.is_dense = true;
-  point_cloud_msg_.width = valid_count;
-  point_cloud_msg_.height = 1;
-  modifier.resize(valid_count);
   depth_cloud_pub_->publish(point_cloud_msg_);
 
   if (save_point_cloud_) {
@@ -734,6 +741,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
   modifier.resize(color_width * color_height);
   point_cloud_msg_.width = color_frame->width();
   point_cloud_msg_.height = color_frame->height();
+  point_cloud_msg_.is_dense = false;
   std::string format_str = "rgb";
   point_cloud_msg_.point_step =
       addPointField(point_cloud_msg_, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
@@ -755,34 +763,39 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
   for (uint32_t y = 0; y < color_height; y++) {
     for (uint32_t x = 0; x < color_width; x++) {
       float depth = depth_data[y * depth_width + x];
+      bool vaild_point = true;
       if (depth < min_depth || depth > max_depth) {
-        continue;
+        vaild_point= false;
+      }
+      if(vaild_point || ordered_pc_) {
+        float xf = (x - u0) * fdx;
+        float yf = (y - v0) * fdy;
+        float zf = depth * depth_scale;
+        *iter_x = zf * xf / 1000.0;
+        *iter_y = zf * yf / 1000.0;
+        *iter_z = zf / 1000.0;
+        *iter_r = color_data[(y * color_width + x) * 3];
+        *iter_g = color_data[(y * color_width + x) * 3 + 1];
+        *iter_b = color_data[(y * color_width + x) * 3 + 2];
+        ++iter_x;
+        ++iter_y;
+        ++iter_z;
+        ++iter_r;
+        ++iter_g;
+        ++iter_b;
+        ++valid_count;
       }
-      float xf = (x - u0) * fdx;
-      float yf = (y - v0) * fdy;
-      float zf = depth * depth_scale;
-      *iter_x = zf * xf / 1000.0;
-      *iter_y = zf * yf / 1000.0;
-      *iter_z = zf / 1000.0;
-      *iter_r = color_data[(y * color_width + x) * 3];
-      *iter_g = color_data[(y * color_width + x) * 3 + 1];
-      *iter_b = color_data[(y * color_width + x) * 3 + 2];
-      ++iter_x;
-      ++iter_y;
-      ++iter_z;
-      ++iter_r;
-      ++iter_g;
-      ++iter_b;
-      ++valid_count;
     }
   }
   auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp());
+  if(!ordered_pc_){
+    point_cloud_msg_.is_dense = true;
+    point_cloud_msg_.width = valid_count;
+    point_cloud_msg_.height = 1;
+    modifier.resize(valid_count);
+  }
   point_cloud_msg_.header.stamp = timestamp;
   point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
-  point_cloud_msg_.is_dense = true;
-  point_cloud_msg_.width = valid_count;
-  point_cloud_msg_.height = 1;
-  modifier.resize(valid_count);
   depth_registration_cloud_pub_->publish(point_cloud_msg_);
   if (save_colored_point_cloud_) {
     save_colored_point_cloud_ = false;