temp

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
kyoichi-sugahara · Mar 26, 2024 · 86f2461 · 86f2461
1 parent 46a662c
commit 86f2461
Showing 1 changed file with 38 additions and 19 deletions.
diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
@@ -108,6 +108,10 @@ void RingOutlierFilterComponent::faster_filter(
     input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Distance)).offset;
   const auto intensity_offset =
     input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::Intensity)).offset;
+  const auto return_type_offset =
+    input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::ReturnType)).offset;
+  const auto time_stamp_offset =
+    input->fields.at(static_cast<size_t>(autoware_point_types::PointIndex::TimeStamp)).offset;
 
   std::vector<std::vector<size_t>> ring2indices;
   ring2indices.reserve(max_rings_num_);
@@ -198,15 +202,21 @@ void RingOutlierFilterComponent::faster_filter(
           const float & intensity = *reinterpret_cast<const float *>(
             &input->data[indices[walk_first_idx] + intensity_offset]);
           outlier_ptr->intensity = intensity;
-          // const uint16_t & ring = *reinterpret_cast<const uint16_t *>(
-          //   &input->data[indices[walk_first_idx] + ring_offset]);
-          // outlier_ptr->ring = ring;
-          // const float & azimuth = *reinterpret_cast<const float *>(
-          //   &input->data[indices[walk_first_idx] + azimuth_offset]);
-          // outlier_ptr->azimuth = azimuth;
-          // const float & distance = *reinterpret_cast<const float *>(
-          //   &input->data[indices[walk_first_idx] + distance_offset]);
-          // outlier_ptr->distance = distance;
+          const uint16_t & ring = *reinterpret_cast<const uint16_t *>(
+            &input->data[indices[walk_first_idx] + ring_offset]);
+          outlier_ptr->ring = ring;
+          const float & azimuth = *reinterpret_cast<const float *>(
+            &input->data[indices[walk_first_idx] + azimuth_offset]);
+          outlier_ptr->azimuth = azimuth;
+          const float & distance = *reinterpret_cast<const float *>(
+            &input->data[indices[walk_first_idx] + distance_offset]);
+          outlier_ptr->distance = distance;
+          const uint8_t & return_type =
+            *reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
+          outlier_ptr->return_type = return_type;
+          const double & time_stamp =
+            *reinterpret_cast<const float *>(&input->data[indices[i] + time_stamp_offset]);
+          outlier_ptr->time_stamp = time_stamp;
 
           outlier_points_size += outlier_points.point_step;
         }
@@ -256,23 +266,32 @@ void RingOutlierFilterComponent::faster_filter(
         const float & intensity =
           *reinterpret_cast<const float *>(&input->data[indices[i] + intensity_offset]);
         outlier_ptr->intensity = intensity;
-        // const uint16_t & ring =
-        //   *reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
-        // outlier_ptr->ring = ring;
-        // const float & azimuth =
-        //   *reinterpret_cast<const float *>(&input->data[indices[i] + azimuth_offset]);
-        // outlier_ptr->azimuth = azimuth;
-        // const float & distance =
-        //   *reinterpret_cast<const float *>(&input->data[indices[i] + distance_offset]);
-        // outlier_ptr->distance = distance;
+        const uint16_t & ring =
+          *reinterpret_cast<const uint16_t *>(&input->data[indices[i] + ring_offset]);
+        outlier_ptr->ring = ring;
+        const float & azimuth =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + azimuth_offset]);
+        outlier_ptr->azimuth = azimuth;
+        const float & distance =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + distance_offset]);
+        outlier_ptr->distance = distance;
+        const uint8_t & return_type =
+          *reinterpret_cast<const uint8_t *>(&input->data[indices[i] + return_type_offset]);
+        outlier_ptr->return_type = return_type;
+        const double & time_stamp =
+          *reinterpret_cast<const float *>(&input->data[indices[i] + time_stamp_offset]);
+        outlier_ptr->time_stamp = time_stamp;
+
+        outlier_points_size += outlier_points.point_step;
+
       }
     }
   }
 
   setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4);
 
   if (publish_outlier_pointcloud_) {
-    setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
+    setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/9);
     outlier_pointcloud_publisher_->publish(outlier_points);
 
     const auto frequency_image = createBinaryImage(*input);