Always copy fill all fields for organized point clouds. (#419)

Author: Samahu

CHANGELOG.rst

@@ -11,6 +11,7 @@ Changelog
   of sensor TF transforms.
 * Introduced a new topic ``/ouster/telemetry`` that publishes ``ouster_ros::Telemetry`` messages,
   the topic can be turned on/off by including the token ``TLM`` in the flag ``proc_mask`` launch arg.
+* [BUGFIX]: NEAR_IR data is not populated with data for organized point clouds that have no range.
 
 
 ouster_ros v0.13.0

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.13.3</version>
+  <version>0.13.4</version>
   <description>Ouster ROS driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

src/point_cloud_compose.h

@@ -144,21 +144,13 @@ void scan_to_cloud_f(ouster_ros::Cloud<PointT>& cloud, PointS& staging_point,
             auto ts =
                 timestamp[ts_idx] > scan_ts ? timestamp[ts_idx] - scan_ts : 0UL;
 
-            if (xyz.isNaN().any()) {
-                if (organized) {
-                    cloud.is_dense = false;
-                    auto& pt = cloud.points[tgt_idx];
-                    pt.x = static_cast<decltype(pt.x)>(xyz(0));
-                    pt.y = static_cast<decltype(pt.y)>(xyz(1));
-                    pt.z = static_cast<decltype(pt.z)>(xyz(2));
-                    CondBinaryOp<point::has_t_v<PointT>>::run(
-                        pt, ts, [](auto& pt, const auto& ts) {
-                            pt.t = static_cast<uint32_t>(ts); }
-                    );
-                }
-                continue;
+            if (organized) {
+                cloud.is_dense &= xyz.isNaN().any();
             } else {
-                if (!organized) cloud.points.emplace_back();
+                if (xyz.isNaN().any())
+                    continue;
+                else
+                    cloud.points.emplace_back();
             }