Created InvalidRegion struct and tidied up invalid point remover

tier4 · Sep 1, 2023 · cb04f79 · cb04f79
1 parent 23b1be6
commit cb04f79
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 17 deletions.
diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp
@@ -11,6 +11,14 @@ namespace nebula
 {
 namespace drivers
 {
+
+/// @brief Invalid region on the cloud to be removed
+struct InvalidRegion {
+  int ring;
+  uint16_t start;
+  uint16_t end;
+};
+
 /// @brief struct for Velodyne sensor configuration
 struct VelodyneSensorConfiguration : SensorConfigurationBase
 {
@@ -20,9 +28,7 @@ struct VelodyneSensorConfiguration : SensorConfigurationBase
   uint16_t cloud_min_angle;
   uint16_t cloud_max_angle;
   bool invalid_point_remove;
-  std::vector<int64_t> invalid_rings;
-  std::vector<int64_t> invalid_angles_start;
-  std::vector<int64_t> invalid_angles_end;
+  std::vector<InvalidRegion> invalid_regions;
 };
 /// @brief Convert VelodyneSensorConfiguration to string (Overloading the << operator)
 /// @param os

diff --git a/...oders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/...oders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp
@@ -192,14 +192,12 @@ class VelodyneScanDecoder
   /// @brief Checks if the point is invalid.
   /// @param point The point to check.
   /// @return True if the point is invalid, false otherwise.
-  bool check_invalid_point(const int& channel, const uint16_t& azimuth)
+  bool check_invalid_point(const int & channel, const uint16_t & azimuth)
   {
     if (!sensor_configuration_->invalid_point_remove) return false;
-    for (size_t i = 0; i < sensor_configuration_->invalid_rings.size(); ++i) {
-      if (
-        sensor_configuration_->invalid_rings.at(i) == channel &&
-        azimuth >= sensor_configuration_->invalid_angles_start.at(i) &&
-        azimuth <= sensor_configuration_->invalid_angles_end.at(i)) {
+
+    for (const InvalidRegion & region : sensor_configuration_->invalid_regions) {
+      if (region.ring == channel && azimuth >= region.start && azimuth <= region.end) {
         return true;
       }
     }

diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp
@@ -266,17 +266,21 @@ Status VelodyneDriverRosWrapper::GetParameters(
     }
   }
 
+  std::vector<int64_t> invalid_rings;
+  std::vector<int64_t> invalid_angles_start;
+  std::vector<int64_t> invalid_angles_end;
+
   {
     rcl_interfaces::msg::ParameterDescriptor descriptor;
     descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY;
     descriptor.read_only = false;
     descriptor.dynamic_typing = false;
     descriptor.additional_constraints = "";
     this->declare_parameter<std::vector<int64_t>>("invalid_rings", descriptor);
-    sensor_configuration.invalid_rings = this->get_parameter("invalid_rings").as_integer_array();
+    invalid_rings = this->get_parameter("invalid_rings").as_integer_array();
 
     RCLCPP_INFO_STREAM(this->get_logger(), "Invalid rings: ");
-    for (const auto & invalid_ring : sensor_configuration.invalid_rings) {
+    for (const auto & invalid_ring : invalid_rings) {
       RCLCPP_INFO_STREAM(this->get_logger(), invalid_ring << " ");
     }
   }
@@ -288,11 +292,10 @@ Status VelodyneDriverRosWrapper::GetParameters(
     descriptor.dynamic_typing = false;
     descriptor.additional_constraints = "";
     this->declare_parameter<std::vector<int64_t>>("invalid_angles_start", descriptor);
-    sensor_configuration.invalid_angles_start =
-      this->get_parameter("invalid_angles_start").as_integer_array();
+    invalid_angles_start = this->get_parameter("invalid_angles_start").as_integer_array();
 
     RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles start: ");
-    for (const auto & invalid_angle_start : sensor_configuration.invalid_angles_start) {
+    for (const auto & invalid_angle_start : invalid_angles_start) {
       RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_start << " ");
     }
   }
@@ -304,15 +307,35 @@ Status VelodyneDriverRosWrapper::GetParameters(
     descriptor.dynamic_typing = false;
     descriptor.additional_constraints = "";
     this->declare_parameter<std::vector<int64_t>>("invalid_angles_end", descriptor);
-    sensor_configuration.invalid_angles_end =
-      this->get_parameter("invalid_angles_end").as_integer_array();
+    invalid_angles_end = this->get_parameter("invalid_angles_end").as_integer_array();
 
     RCLCPP_INFO_STREAM(this->get_logger(), "Invalid angles end: ");
-    for (const auto & invalid_angle_end : sensor_configuration.invalid_angles_end) {
+    for (const auto & invalid_angle_end : invalid_angles_end) {
       RCLCPP_INFO_STREAM(this->get_logger(), invalid_angle_end << " ");
     }
   }
 
+  if (sensor_configuration.invalid_point_remove) {
+    // Check length of invalid_rings, invalid_angles_start and invalid_angles_end
+    if (
+      invalid_rings.size() != invalid_angles_start.size() ||
+      invalid_rings.size() != invalid_angles_end.size()) {
+      RCLCPP_ERROR_STREAM(
+        this->get_logger(),
+        "Invalid rings, invalid angles start and invalid angles end must have the same length.");
+      return Status::SENSOR_CONFIG_ERROR;
+    }
+
+    // Add invalid regions to sensor configuration
+    for (size_t i = 0; i < invalid_rings.size(); i++) {
+      drivers::InvalidRegion invalid_region;
+      invalid_region.ring = invalid_rings[i];
+      invalid_region.start = invalid_angles_start[i];
+      invalid_region.end = invalid_angles_end[i];
+      sensor_configuration.invalid_regions.push_back(invalid_region);
+    }
+  }
+
   if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) {
     return Status::INVALID_SENSOR_MODEL;
   }