PR #2952 from Nir-Az: Support 2 res for LPC

IntelRealSense · Dec 25, 2023 · 554f45d · 554f45d
2 parents 18a1149 + b929cb3
commit 554f45d
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 9 deletions.
diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
@@ -204,7 +204,7 @@ namespace realsense2_camera
         void startDynamicTf();
         void publishDynamicTransforms();
         void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);
-        void publishLabeledPointCloud(rs2::labeled_points points, const rclcpp::Time& t);
+        void publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t);
         bool shouldPublishCameraInfo(const stream_index_pair& sip);
         Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;
         IMUInfo getImuInfo(const rs2::stream_profile& profile);

diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
@@ -841,7 +841,7 @@ bool BaseRealSenseNode::shouldPublishCameraInfo(const stream_index_pair& sip)
     return (stream != RS2_STREAM_SAFETY && stream != RS2_STREAM_OCCUPANCY && stream != RS2_STREAM_LABELED_POINT_CLOUD);
 }
 
-void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const rclcpp::Time& t){
+void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t){
     if(!_labeled_pointcloud_publisher || 0 == _labeled_pointcloud_publisher->get_subscription_count())
         return;
 
@@ -857,22 +857,23 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r
                                 "y", 1, sensor_msgs::msg::PointField::FLOAT32,
                                 "z", 1, sensor_msgs::msg::PointField::FLOAT32,
                                 "label", 1, sensor_msgs::msg::PointField::UINT8);
-    modifier.resize(pc.size());
+    modifier.resize(lpc.size());
 
     // Fill the PointCloud message with data
     sensor_msgs::PointCloud2Iterator<float> iter_x(*msg_pointcloud, "x");
     sensor_msgs::PointCloud2Iterator<float> iter_y(*msg_pointcloud, "y");
     sensor_msgs::PointCloud2Iterator<float> iter_z(*msg_pointcloud, "z");
     sensor_msgs::PointCloud2Iterator<uint8_t> iter_label(*msg_pointcloud, "label");
-    const rs2::vertex* vertex = pc.get_vertices();
-    const uint8_t* label = pc.get_labels();
-    msg_pointcloud->width = 320;
-    msg_pointcloud->height = 180;
-    msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t);
+    const rs2::vertex* vertex = lpc.get_vertices();
+    const uint8_t* label = lpc.get_labels();
+
+    msg_pointcloud->width = lpc.get_width();
+    msg_pointcloud->height = lpc.get_height();
+    msg_pointcloud->point_step = lpc.get_bits_per_pixel() / 8;
     msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step;
     msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step);
 
-    for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, label++)
+    for (size_t point_idx=0; point_idx < lpc.size(); point_idx++, vertex++, label++)
     {
         *iter_x = vertex->x;
         *iter_y = vertex->y;