Merge pull request #1227 from tier4/fix/occluded-object-in-grid

Exclude LiDAR occluded object on OccupancyGrid
tier4 · Apr 18, 2024 · b4777c8 · b4777c8
2 parents 9e1d468 + 99f7276
commit b4777c8
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 7 deletions.
diff --git a/...r/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/...r/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp
@@ -89,7 +89,7 @@ class DetectionSensor : public DetectionSensorBase
 
   auto update(
     const double, const std::vector<traffic_simulator_msgs::EntityStatus> &, const rclcpp::Time &,
-    const std::vector<std::string> & lidar_detected_entity) -> void override;
+    const std::vector<std::string> & lidar_detected_entities) -> void override;
 };
 }  // namespace simple_sensor_simulator
 

diff --git a/...nclude/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp b/...nclude/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp
@@ -58,10 +58,12 @@ class OccupancyGridSensorBase
 
   /**
    * @brief List all objects in range of sensor sight
+   * @warning `status` must contain EGO object
    * @return names of objects in range of sensor sight
    */
   const std::vector<std::string> getDetectedObjects(
-    const std::vector<traffic_simulator_msgs::EntityStatus> &) const;
+    const std::vector<traffic_simulator_msgs::EntityStatus> & status,
+    const std::vector<std::string> & lidar_detected_entities) const;
 
   /**
    * @brief Extract sensor pose from entity statuses
@@ -124,7 +126,7 @@ class OccupancyGridSensor : public OccupancyGridSensorBase
 template <>
 auto OccupancyGridSensor<nav_msgs::msg::OccupancyGrid>::getOccupancyGrid(
   const std::vector<traffic_simulator_msgs::EntityStatus> & status, const rclcpp::Time & stamp,
-  const std::vector<std::string> & lidar_detected_entity) -> nav_msgs::msg::OccupancyGrid;
+  const std::vector<std::string> & lidar_detected_entities) -> nav_msgs::msg::OccupancyGrid;
 }  // namespace simple_sensor_simulator
 
 #endif  // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__OCCUPANCY_GRID__OCCUPANCY_GRID_SENSOR_HPP_
diff --git a/...on/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/...on/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp
@@ -14,6 +14,7 @@
 
 #include <quaternion_operation/quaternion_operation.h>
 
+#include <algorithm>
 #include <memory>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <optional>
@@ -39,11 +40,19 @@ geometry_msgs::Pose OccupancyGridSensorBase::getSensorPose(
 }
 
 const std::vector<std::string> OccupancyGridSensorBase::getDetectedObjects(
-  const std::vector<traffic_simulator_msgs::EntityStatus> & status) const
+  const std::vector<traffic_simulator_msgs::EntityStatus> & status,
+  const std::vector<std::string> & lidar_detected_entities) const
 {
   std::vector<std::string> detected_entities;
   const auto pose = getSensorPose(status);
   for (const auto & s : status) {
+    if (const auto has_detected =
+          std::find(lidar_detected_entities.begin(), lidar_detected_entities.end(), s.name()) !=
+          lidar_detected_entities.end();
+        !has_detected) {
+      continue;
+    }
+
     double distance = std::hypot(
       s.pose().position().x() - pose.position().x(), s.pose().position().y() - pose.position().y(),
       s.pose().position().z() - pose.position().z());
@@ -57,7 +66,7 @@ const std::vector<std::string> OccupancyGridSensorBase::getDetectedObjects(
 template <>
 auto OccupancyGridSensor<nav_msgs::msg::OccupancyGrid>::getOccupancyGrid(
   const std::vector<traffic_simulator_msgs::EntityStatus> & status, const rclcpp::Time & stamp,
-  const std::vector<std::string> & lidar_detected_entity) -> nav_msgs::msg::OccupancyGrid
+  const std::vector<std::string> & lidar_detected_entities) -> nav_msgs::msg::OccupancyGrid
 {
   // check if entities in `status` have unique names
   {
@@ -96,10 +105,10 @@ auto OccupancyGridSensor<nav_msgs::msg::OccupancyGrid>::getOccupancyGrid(
   auto detected_entities = std::set<std::string>();
   {
     if (configuration_.filter_by_range()) {
-      auto v = getDetectedObjects(status);
+      auto v = getDetectedObjects(status, lidar_detected_entities);
       detected_entities.insert(v.begin(), v.end());
     } else {
-      detected_entities.insert(lidar_detected_entity.begin(), lidar_detected_entity.end());
+      detected_entities.insert(lidar_detected_entities.begin(), lidar_detected_entities.end());
     }
   }