From 49fb3a1f71de4ef2b121fd34512e2740c197c4c5 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 11 Apr 2024 18:14:57 +0900 Subject: [PATCH 1/2] Exclude LiDAR occluded object from OccupancyGrid in simple_sensor_simulator --- .../occupancy_grid/occupancy_grid_sensor.hpp | 4 +++- .../occupancy_grid/occupancy_grid_sensor.cpp | 13 +++++++++++-- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp index e77addf39fb..95af06dd0fc 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/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 getDetectedObjects( - const std::vector &) const; + const std::vector & status, + const std::vector & lidar_detected_entities) const; /** * @brief Extract sensor pose from entity statuses diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp index 5c1944a57cb..107e0b66d48 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp @@ -14,6 +14,7 @@ #include +#include #include #include #include @@ -39,11 +40,19 @@ geometry_msgs::Pose OccupancyGridSensorBase::getSensorPose( } const std::vector OccupancyGridSensorBase::getDetectedObjects( - const std::vector & status) const + const std::vector & status, + const std::vector & lidar_detected_entity) const { std::vector detected_entities; const auto pose = getSensorPose(status); for (const auto & s : status) { + if (const auto has_detected = + std::find(lidar_detected_entity.begin(), lidar_detected_entity.end(), s.name()) != + lidar_detected_entity.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()); @@ -96,7 +105,7 @@ auto OccupancyGridSensor::getOccupancyGrid( auto detected_entities = std::set(); { if (configuration_.filter_by_range()) { - auto v = getDetectedObjects(status); + auto v = getDetectedObjects(status, lidar_detected_entity); detected_entities.insert(v.begin(), v.end()); } else { detected_entities.insert(lidar_detected_entity.begin(), lidar_detected_entity.end()); From 0592f4092e5bccd7ce311c917eee7a302b6763c3 Mon Sep 17 00:00:00 2001 From: f0reachARR Date: Thu, 11 Apr 2024 18:30:47 +0900 Subject: [PATCH 2/2] Fix spell --- .../detection_sensor/detection_sensor.hpp | 2 +- .../occupancy_grid/occupancy_grid_sensor.hpp | 2 +- .../occupancy_grid/occupancy_grid_sensor.cpp | 12 ++++++------ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index eb477a9d661..f5545a0d9d4 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/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 &, const rclcpp::Time &, - const std::vector & lidar_detected_entity) -> void override; + const std::vector & lidar_detected_entities) -> void override; }; } // namespace simple_sensor_simulator diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp index 95af06dd0fc..a5f17dbd8aa 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/occupancy_grid/occupancy_grid_sensor.hpp @@ -126,7 +126,7 @@ class OccupancyGridSensor : public OccupancyGridSensorBase template <> auto OccupancyGridSensor::getOccupancyGrid( const std::vector & status, const rclcpp::Time & stamp, - const std::vector & lidar_detected_entity) -> nav_msgs::msg::OccupancyGrid; + const std::vector & 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/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp index 107e0b66d48..d947efe5d5d 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp @@ -41,14 +41,14 @@ geometry_msgs::Pose OccupancyGridSensorBase::getSensorPose( const std::vector OccupancyGridSensorBase::getDetectedObjects( const std::vector & status, - const std::vector & lidar_detected_entity) const + const std::vector & lidar_detected_entities) const { std::vector detected_entities; const auto pose = getSensorPose(status); for (const auto & s : status) { if (const auto has_detected = - std::find(lidar_detected_entity.begin(), lidar_detected_entity.end(), s.name()) != - lidar_detected_entity.end(); + std::find(lidar_detected_entities.begin(), lidar_detected_entities.end(), s.name()) != + lidar_detected_entities.end(); !has_detected) { continue; } @@ -66,7 +66,7 @@ const std::vector OccupancyGridSensorBase::getDetectedObjects( template <> auto OccupancyGridSensor::getOccupancyGrid( const std::vector & status, const rclcpp::Time & stamp, - const std::vector & lidar_detected_entity) -> nav_msgs::msg::OccupancyGrid + const std::vector & lidar_detected_entities) -> nav_msgs::msg::OccupancyGrid { // check if entities in `status` have unique names { @@ -105,10 +105,10 @@ auto OccupancyGridSensor::getOccupancyGrid( auto detected_entities = std::set(); { if (configuration_.filter_by_range()) { - auto v = getDetectedObjects(status, lidar_detected_entity); + 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()); } }