diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 824f18706fcf..8cb2e1b22478 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -218,13 +218,31 @@ target_link_libraries(scene_module_virtual_traffic_light scene_module_lib) # SceneModule OcclusionSpot -ament_auto_add_library(scene_module_occlusion_spot SHARED +# Util +ament_auto_add_library(occlusion_spot_lib SHARED src/scene_module/occlusion_spot/grid_utils.cpp + src/scene_module/occlusion_spot/occlusion_spot_utils.cpp + src/scene_module/occlusion_spot/risk_predictive_braking.cpp +) + +target_include_directories(occlusion_spot_lib + SYSTEM PUBLIC + ${BOOST_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} +) + +target_include_directories(occlusion_spot_lib + PUBLIC + $ + $ +) + +ament_target_dependencies(occlusion_spot_lib ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +ament_auto_add_library(scene_module_occlusion_spot SHARED src/scene_module/occlusion_spot/manager.cpp src/scene_module/occlusion_spot/debug.cpp src/scene_module/occlusion_spot/scene_occlusion_spot.cpp - src/scene_module/occlusion_spot/occlusion_spot_utils.cpp - src/scene_module/occlusion_spot/risk_predictive_braking.cpp ) target_include_directories(scene_module_occlusion_spot @@ -235,7 +253,7 @@ target_include_directories(scene_module_occlusion_spot ament_target_dependencies(scene_module_occlusion_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) -target_link_libraries(scene_module_occlusion_spot scene_module_lib) +target_link_libraries(scene_module_occlusion_spot scene_module_lib occlusion_spot_lib) # Scene Module Manager ament_auto_add_library(scene_module_manager SHARED diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 71bcf2694fa0..a0b9e9d3243e 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,11 +1,17 @@ /**: ros__parameters: occlusion_spot: - detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" - pass_judge: "current_velocity" # [-] candidate is "current_velocity"" - debug: false # [-] whether to publish debug markers. Note Default should be false for performance - use_partition_lanelet: true # [-] whether to use partition lanelet map data - pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity + detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" + pass_judge: "current_velocity" # [-] candidate is "current_velocity" + filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not + use_object_info: true # [-] whether to reflect object info to occupancy grid map or not + use_partition_lanelet: true # [-] whether to use partition lanelet map data + pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity + pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) + debug: # !Note: default should be false for performance + is_show_occlusion: false # [-] whether to show occlusion point markers. + is_show_cv_window: false # [-] whether to show open_cv debug window + is_show_processing_time: false # [-] whether to show processing time threshold: detection_area_length: 100.0 # [m] the length of path to consider perception range stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop @@ -17,12 +23,12 @@ non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed - delay_time: 0.1 # [s] safety time buffer for delay system response - safe_margin: 1.0 # [m] maximum safety distance for any error + safe_margin: 2.0 # [m] margin for detection failure(0.5m) + pedestrian radius(0.5m) + safe margin(1.0m) detection_area: min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. - max_lateral_distance: 4.0 # [m] buffer around the ego path used to build the detection area. + min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper. + max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area. grid: free_space_max: 40 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 60 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg new file mode 100644 index 000000000000..ec5685f04f74 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg @@ -0,0 +1 @@ +
ego
pedestrian
pedestrian
collision judgement is done by polygon consider  pedestrian radius
collision free
 \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg new file mode 100644 index 000000000000..9c4b72332c97 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg @@ -0,0 +1 @@ +
ego
use object info
ego
without object info
obj
obj
 \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp index b7f9df3371b6..5eb18a7ecde8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp @@ -18,9 +18,14 @@ #include #include #include +#include #include #include +#include +#include +#include +#include #include #include @@ -34,12 +39,17 @@ namespace behavior_velocity_planner { namespace grid_utils { +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using nav_msgs::msg::OccupancyGrid; namespace occlusion_cost_value { -static constexpr int NO_INFORMATION = -1; -static constexpr int FREE_SPACE = 0; -static constexpr int UNKNOWN = 50; -static constexpr int OCCUPIED = 100; +static constexpr unsigned char FREE_SPACE = 0; +static constexpr unsigned char UNKNOWN = 50; +static constexpr unsigned char OCCUPIED = 100; +static constexpr unsigned char FREE_SPACE_IMAGE = 0; +static constexpr unsigned char UNKNOWN_IMAGE = 128; +static constexpr unsigned char OCCUPIED_IMAGE = 255; } // namespace occlusion_cost_value struct GridParam @@ -71,7 +81,8 @@ void findOcclusionSpots( const Polygon2d & polygon, const double min_size); //!< @brief Return true if the path between the two given points is free of occupied cells bool isCollisionFree( - const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2); + const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2, + const double radius); //!< @brief get the corner positions of the square described by the given anchor void getCornerPositions( std::vector & corner_positions, const grid_map::GridMap & grid, @@ -80,8 +91,10 @@ void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid void toQuantizedImage( const nav_msgs::msg::OccupancyGrid & occupancy_grid, cv::Mat * cv_image, const GridParam & param); void denoiseOccupancyGridCV( - nav_msgs::msg::OccupancyGrid & occupancy_grid, grid_map::GridMap & grid_map, - const GridParam & param); + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, + grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, + const bool filter_occupancy_grid); +void addObjectsToGridMap(const PredictedObjects & objs, grid_map::GridMap & grid); } // namespace grid_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp index 831dca20f165..7efb3b522560 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp @@ -54,8 +54,6 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - - rclcpp::Publisher::SharedPtr pub_debug_occupancy_grid_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 0146947be203..2effa9e9ce55 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -82,6 +82,7 @@ enum PASS_JUDGE { SMOOTH_VELOCITY, CURRENT_VELOCITY }; struct DetectionArea { + double min_longitudinal_offset; // [m] detection area safety buffer from front bumper double max_lateral_distance; // [m] distance to care about occlusion spot double slice_length; // [m] size of each slice double min_occlusion_spot_size; // [m] minumum size to care about the occlusion spot @@ -112,14 +113,19 @@ struct PlannerParam { DETECTION_METHOD detection_method; PASS_JUDGE pass_judge; - bool debug; // [-] - bool use_partition_lanelet; // [-] + bool is_show_occlusion; // [-] + bool is_show_cv_window; // [-] + bool is_show_processing_time; // [-] + bool filter_occupancy_grid; // [-] + bool use_object_info; // [-] + bool use_partition_lanelet; // [-] // parameters in yaml double detection_area_length; // [m] double detection_area_max_length; // [m] double stuck_vehicle_vel; // [m/s] double lateral_distance_thr; // [m] lateral distance threshold to consider double pedestrian_vel; // [m/s] + double pedestrian_radius; // [m] double dist_thr; // [m] double angle_thr; // [rad] @@ -182,6 +188,7 @@ struct DebugData std::string road_type = ""; std::string detection_type = ""; Polygons2d detection_area_polygons; + std::vector close_partition; std::vector partition_lanelets; std::vector parked_vehicle_point; std::vector possible_collisions; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index b9fc46c2cd04..60424f15e19d 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -44,10 +45,9 @@ class OcclusionSpotModule : public SceneModuleInterface public: OcclusionSpotModule( - const int64_t module_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock, - const rclcpp::Publisher::SharedPtr publisher); + const int64_t module_id, const std::shared_ptr & planner_data, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid @@ -58,14 +58,12 @@ class OcclusionSpotModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; private: - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects_array_; - rclcpp::Publisher::SharedPtr publisher_; - // Parameter PlannerParam param_; + tier4_autoware_utils::StopWatch stop_watch_; protected: - int64_t module_id_; + int64_t module_id_{}; // Debug mutable DebugData debug_data_; diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 6632127a9e98..9128ec53863d 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -38,6 +38,8 @@ #include #include +#include +#include #include #include #include @@ -131,7 +133,9 @@ bool createDetectionAreaPolygons( const double obstacle_vel_mps); PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); Point2d calculateLateralOffsetPoint2d(const Pose & p, const double offset); - +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh = 30.0); void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys); void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input); void insertVelocity( diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index 0d9eda6fd393..a2bc68f2b0cf 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -90,11 +90,34 @@ The maximum length of detection area depends on ego current vehicle velocity and By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot. ![brief](./docs/occlusion_spot/occlusion_spot_partition.svg) +##### Use Object Info + +use object info to make occupancy grid more accurate +![brief](./docs/occlusion_spot/use_object_info.drawio.svg) + +##### Collision Free Judgement + +obstacle that can run out from occlusion should have free space until intersection from ego vehicle +![brief](./docs/occlusion_spot/collision_free.drawio.svg) + #### Module Parameters -| Parameter | Type | Description | -| ---------------- | ------ | ------------------------------------------------------------------------- | -| `pedestrian_vel` | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point | +| Parameter | Type | Description | +| ------------------- | ------ | -------------------------------------------------------------------------- | +| `pedestrian_vel` | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point. | +| `pedestrian_radius` | double | [m] assumed pedestrian radius which fits in occlusion spot. | + +| Parameter | Type | Description | +| ----------------------- | ---- | ---------------------------------------------------------------- | +| `filter_occupancy_grid` | bool | [-] whether to filter occupancy grid by morphologyEx or not. | +| `use_object_info` | bool | [-] whether to reflect object info to occupancy grid map or not. | +| `use_partition_lanelet` | bool | [-] whether to use partition lanelet map data. | + +| Parameter /debug | Type | Description | +| ------------------------- | ---- | ---------------------------------------------- | +| `is_show_occlusion` | bool | [-] whether to show occlusion point markers.　 | +| `is_show_cv_window` | bool | [-] whether to show open_cv debug window. | +| `is_show_processing_time` | bool | [-] whether to show processing time. | | Parameter /threshold | Type | Description | | ----------------------- | ------ | --------------------------------------------------------- | @@ -110,7 +133,6 @@ By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible | `non_effective_jerk` | double | [m/s^3] weak jerk for velocity planning. | | `non_effective_acceleration` | double | [m/s^2] weak deceleration for velocity planning. | | `min_allowed_velocity` | double | [m/s] minimum velocity allowed | -| `delay_time` | double | [m/s] time buffer for the system delay | | `safe_margin` | double | [m] maximum error to stop with emergency braking system. | | Parameter /detection_area | Type | Description | @@ -293,9 +315,12 @@ note right - filter occlusion spot by partition lanelets which prevent pedestrians come out. end note :calculate collision path point and intersection point; +note right + - use pedestrian polygon to judge "collision_free" or not. +end note :calculate safe velocity and safe margin for possible collision; note right - - safe velocity and safe margin is calculated from performance of ego emergency braking system. + - safe velocity and safe margin is calculated from the performance of ego emergency braking system. end note } partition handle_possible_collision { diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index b0749792956f..b688641a9bd0 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -151,16 +151,16 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( debug_marker.id = planning_utils::bitShift(id); debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; debug_marker.action = visualization_msgs::msg::Marker::ADD; - debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z); + debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.5); - debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); debug_marker.ns = ns; for (const auto & poly : polygons) { for (const auto & p : poly) { geometry_msgs::msg::Point point = - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5); debug_marker.points.push_back(point); } debug_markers.markers.push_back(debug_marker); @@ -180,7 +180,7 @@ visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( debug_marker.id = planning_utils::bitShift(id); debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; debug_marker.action = visualization_msgs::msg::Marker::ADD; - debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z); + debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); @@ -188,8 +188,7 @@ visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( debug_marker.ns = ns; for (const auto & slice : slices) { for (const auto & p : slice.outer()) { - geometry_msgs::msg::Point point = - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + geometry_msgs::msg::Point point = tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z); debug_marker.points.push_back(point); } debug_markers.markers.push_back(debug_marker); @@ -298,7 +297,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray } if (!debug_data_.partition_lanelets.empty()) { appendMarkerArray( - makePolygonMarker(debug_data_.partition_lanelets, "partition", module_id_, debug_data_.z), + makePolygonMarker(debug_data_.close_partition, "partition", module_id_, debug_data_.z), current_time, &debug_marker_array); } if (!debug_data_.interp_path.points.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index 8e91a5735b98..b78ee53e1f53 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include @@ -23,6 +22,70 @@ namespace behavior_velocity_planner { namespace grid_utils { + +Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) +{ + LineString2d line = {p0, p1}; + const double angle = atan2(p0.y() - p1.y(), p0.x() - p1.x()); + const double r = radius; + Polygon2d line_poly; + // add polygon counter clockwise + line_poly.outer().emplace_back(p0.x() + r * sin(angle), p0.y() - r * cos(angle)); + line_poly.outer().emplace_back(p1.x() + r * sin(angle), p1.y() - r * cos(angle)); + line_poly.outer().emplace_back(p1.x() - r * sin(angle), p1.y() + r * cos(angle)); + line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle)); + // std::cout << boost::geometry::wkt(line_poly) << std::endl; + // std::cout << boost::geometry::wkt(line) << std::endl; + return line_poly; +} + +std::vector> pointsToRays( + const grid_map::Position p0, const grid_map::Position p1, const double radius) +{ + using grid_map::Position; + std::vector> lines; + const double angle = atan2(p0.y() - p1.y(), p0.x() - p1.x()); + const double r = radius; + lines.emplace_back(std::make_pair(p0, p1)); + lines.emplace_back(std::make_pair( + Position(p0.x() + r * sin(angle), p0.y() - r * cos(angle)), + Position(p1.x() + r * sin(angle), p1.y() - r * cos(angle)))); + lines.emplace_back(std::make_pair( + Position(p1.x() - r * sin(angle), p1.y() + r * cos(angle)), + Position(p0.x() - r * sin(angle), p0.y() + r * cos(angle)))); + return lines; +} + +void addObjectsToGridMap(const PredictedObjects & objs, grid_map::GridMap & grid) +{ + auto & grid_data = grid["layer"]; + for (const auto & obj : objs.objects) { + Polygon2d foot_print_polygon = planning_utils::toFootprintPolygon(obj); + grid_map::Polygon grid_polygon; + const auto & pos = obj.kinematics.initial_pose_with_covariance.pose.position; + const auto & obj_label = obj.classification.at(0).label; + using autoware_auto_perception_msgs::msg::ObjectClassification; + if ( + obj_label != ObjectClassification::CAR || obj_label != ObjectClassification::TRUCK || + obj_label != ObjectClassification::BUS || obj_label != ObjectClassification::TRAILER) + continue; + if (grid.isInside(grid_map::Position(pos.x, pos.y))) continue; + try { + for (const auto & point : foot_print_polygon.outer()) { + grid_polygon.addVertex({point.x(), point.y()}); + } + for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); + ++iterator) { + const grid_map::Index & index = *iterator; + if (!grid.isValid(index)) continue; + grid_data(index.x(), index.y()) = grid_utils::occlusion_cost_value::OCCUPIED; + } + } catch (const std::invalid_argument & e) { + std::cerr << e.what() << std::endl; + } + } +} + bool isOcclusionSpotSquare( OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data, const grid_map::Index & cell, int min_occlusion_size, const grid_map::Size & grid_size) @@ -105,18 +168,43 @@ void findOcclusionSpots( } bool isCollisionFree( - const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2) + const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2, + const double radius) { const grid_map::Matrix & grid_data = grid["layer"]; + bool polys = true; try { - for (grid_map::LineIterator iterator(grid, p1, p2); !iterator.isPastEnd(); ++iterator) { - const grid_map::Index & index = *iterator; - if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { - return false; + if (polys) { + Point2d occlusion_p = {p1.x(), p1.y()}; + Point2d collision_p = {p2.x(), p2.y()}; + Polygon2d polygon = pointsToPoly(occlusion_p, collision_p, radius); + grid_map::Polygon grid_polygon; + for (const auto & point : polygon.outer()) { + grid_polygon.addVertex({point.x(), point.y()}); } - } + for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); + ++iterator) { + const grid_map::Index & index = *iterator; + if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { + return false; + } + } + } else { + std::vector> lines = + pointsToRays(p1, p2, radius); + for (const auto & p : lines) { + for (grid_map::LineIterator iterator(grid, p.first, p.second); !iterator.isPastEnd(); + ++iterator) { + const grid_map::Index & index = *iterator; + if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { + return false; + } + } + } + } // polys or not } catch (const std::invalid_argument & e) { std::cerr << e.what() << std::endl; + return false; } return true; } @@ -168,7 +256,16 @@ void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid for (int x = width - 1; x >= 0; x--) { for (int y = height - 1; y >= 0; y--) { const int idx = (height - 1 - y) + (width - 1 - x) * height; - const unsigned char intensity = cv_image.at(y, x); + unsigned char intensity = cv_image.at(y, x); + if (intensity == grid_utils::occlusion_cost_value::FREE_SPACE_IMAGE) { + intensity = grid_utils::occlusion_cost_value::FREE_SPACE; + } else if (intensity == grid_utils::occlusion_cost_value::UNKNOWN_IMAGE) { + intensity = grid_utils::occlusion_cost_value::UNKNOWN; + } else if (intensity == grid_utils::occlusion_cost_value::OCCUPIED_IMAGE) { + intensity = grid_utils::occlusion_cost_value::OCCUPIED; + } else { + throw std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); + } occupancy_grid->data.at(idx) = intensity; } } @@ -181,32 +278,41 @@ void toQuantizedImage( for (int x = width - 1; x >= 0; x--) { for (int y = height - 1; y >= 0; y--) { const int idx = (height - 1 - y) + (width - 1 - x) * height; - int8_t intensity = occupancy_grid.data.at(idx); - if (0 <= intensity && intensity <= param.free_space_max) { - intensity = grid_utils::occlusion_cost_value::FREE_SPACE; + unsigned char intensity = occupancy_grid.data.at(idx); + if (intensity <= param.free_space_max) { + intensity = grid_utils::occlusion_cost_value::FREE_SPACE_IMAGE; } else if (param.free_space_max < intensity && intensity < param.occupied_min) { - intensity = grid_utils::occlusion_cost_value::UNKNOWN; + intensity = grid_utils::occlusion_cost_value::UNKNOWN_IMAGE; } else if (param.occupied_min <= intensity) { - intensity = grid_utils::occlusion_cost_value::OCCUPIED; + intensity = grid_utils::occlusion_cost_value::OCCUPIED_IMAGE; } else { - std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); + throw std::logic_error("behavior_velocity[occlusion_spot_grid]: invalid if clause"); } cv_image->at(y, x) = intensity; } } } + void denoiseOccupancyGridCV( - nav_msgs::msg::OccupancyGrid & occupancy_grid, grid_map::GridMap & grid_map, - const GridParam & param) + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, + grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, + const bool filter_occupancy_grid) { + OccupancyGrid occupancy_grid = *occupancy_grid_ptr; cv::Mat cv_image( occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1, cv::Scalar(grid_utils::occlusion_cost_value::OCCUPIED)); toQuantizedImage(occupancy_grid, &cv_image, param); constexpr int num_iter = 2; //!< @brief opening & closing to remove noise in occupancy grid - cv::dilate(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), num_iter); - cv::erode(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), num_iter); + if (filter_occupancy_grid) { + cv::morphologyEx(cv_image, cv_image, cv::MORPH_CLOSE, cv::Mat(), cv::Point(-1, -1), num_iter); + } + if (is_show_debug_window) { + cv::namedWindow("morph", cv::WINDOW_NORMAL); + cv::imshow("morph", cv_image); + cv::waitKey(1); + } imageToOccupancyGrid(cv_image, &occupancy_grid); grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 25ac66574486..4eb5635461c7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -31,10 +31,6 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); - pub_debug_occupancy_grid_ = - node.create_publisher("~/debug/" + ns + "/occupancy_grid", 1); - - // for crosswalk parameters auto & pp = planner_param_; // for detection type { @@ -62,19 +58,26 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; } } - pp.debug = node.declare_parameter(ns + ".debug", false); + pp.filter_occupancy_grid = node.declare_parameter(ns + ".filter_occupancy_grid", false); + pp.use_object_info = node.declare_parameter(ns + ".use_object_info", false); pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false); - pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); + pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.5); + pp.pedestrian_radius = node.declare_parameter(ns + ".pedestrian_radius", 0.5); + + // debug + pp.is_show_occlusion = node.declare_parameter(ns + ".debug.is_show_occlusion", false); + pp.is_show_cv_window = node.declare_parameter(ns + ".debug.is_show_cv_window", false); + pp.is_show_processing_time = node.declare_parameter(ns + ".debug.is_show_processing_time", false); + + // threshold pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel", 1.0); pp.lateral_distance_thr = node.declare_parameter(ns + ".threshold.lateral_distance", 10.0); - pp.dist_thr = node.declare_parameter(ns + ".threshold.search_dist", 10.0); pp.angle_thr = node.declare_parameter(ns + ".threshold.search_angle", M_PI / 5.0); // ego additional velocity config pp.v.safety_ratio = node.declare_parameter(ns + ".motion.safety_ratio", 1.0); - pp.v.delay_time = node.declare_parameter(ns + ".motion.delay_time", 0.1); pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin", 1.0); pp.v.max_slow_down_jerk = node.declare_parameter(ns + ".motion.max_slow_down_jerk", -0.7); pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -2.5); @@ -85,6 +88,8 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // detection_area param pp.detection_area.min_occlusion_spot_size = node.declare_parameter(ns + ".detection_area.min_occlusion_spot_size", 2.0); + pp.detection_area.min_longitudinal_offset = + node.declare_parameter(ns + ".detection_area.min_longitudinal_offset", 1.0); pp.detection_area.max_lateral_distance = node.declare_parameter(ns + ".detection_area.max_lateral_distance", 4.0); pp.detection_area.slice_length = node.declare_parameter(ns + ".detection_area.slice_length", 1.5); @@ -104,8 +109,8 @@ void OcclusionSpotModuleManager::launchNewModules( // general if (!isModuleRegistered(module_id_)) { registerModule(std::make_shared( - module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, - pub_debug_occupancy_grid_)); + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), + clock_)); } } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index c0586aabff66..8b471ae92986 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -66,7 +66,8 @@ bool buildDetectionAreaPolygon( const auto & p = param; DetectionRange da_range; da_range.interval = p.detection_area.slice_length; - da_range.min_longitudinal_distance = offset + p.baselink_to_front; + da_range.min_longitudinal_distance = + offset + std::max(0.0, p.baselink_to_front - p.detection_area.min_longitudinal_offset); da_range.max_longitudinal_distance = std::min(p.detection_area_max_length, p.detection_area_length) + da_range.min_longitudinal_distance; @@ -381,7 +382,7 @@ bool createPossibleCollisionsInDetectionArea( grid_utils::findOcclusionSpots( occlusion_spot_positions, grid, detection_area_slice, param.detection_area.min_occlusion_spot_size); - if (param.debug) { + if (param.is_show_occlusion) { for (const auto & op : occlusion_spot_positions) { Point p = tier4_autoware_utils::createPoint(op[0], op[1], path.points.at(0).point.pose.position.z); @@ -428,7 +429,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS double distance_lower_bound = std::numeric_limits::max(); PossibleCollisionInfo candidate; bool has_collision = false; - const auto & partition_lanelets = debug_data.partition_lanelets; + const auto & partition_lanelets = debug_data.close_partition; for (const grid_map::Position & occlusion_spot_position : occlusion_spot_positions) { // arc intersection const lanelet::BasicPoint2d obstacle_point = { @@ -450,19 +451,19 @@ boost::optional generateOneNotableCollisionFromOcclusionS PossibleCollisionInfo pc = calculateCollisionPathPointFromOcclusionSpot( arc_coord_occlusion_point, arc_coord_collision_point, path_lanelet, param); const auto & ip = pc.intersection_pose.position; - bool collision_free_at_intersection = - grid_utils::isCollisionFree(grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y)); + bool collision_free_at_intersection = grid_utils::isCollisionFree( + grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y), param.pedestrian_radius); bool obstacle_not_blocked_by_partition = true; + if (!collision_free_at_intersection) continue; if (param.use_partition_lanelet) { const auto & op = obstacle_point; const LineString2d obstacle_vec = {{op[0], op[1]}, {ip.x, ip.y}}; obstacle_not_blocked_by_partition = isNotBlockedByPartition(obstacle_vec, partition_lanelets); } - if (collision_free_at_intersection && obstacle_not_blocked_by_partition) { - distance_lower_bound = dist; - candidate = pc; - has_collision = true; - } + if (!obstacle_not_blocked_by_partition) continue; + distance_lower_bound = dist; + candidate = pc; + has_collision = true; } if (has_collision) { return candidate; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 38250d6d7825..5b642d80df94 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -58,22 +58,6 @@ void applySafeVelocityConsideringPossibleCollision( } } -bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) -{ - geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); - bool is_target_ahead = (p.position.x > 0.0); - return is_target_ahead; -} - -bool setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input) -{ - for (size_t i = idx; i < input->points.size(); ++i) { - input->points.at(i).point.longitudinal_velocity_mps = - std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); - } - return true; -} - int insertSafeVelocityToPath( const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, PathWithLaneId * inout_path) @@ -88,7 +72,7 @@ int insertSafeVelocityToPath( size_t insert_idx = closest_idx; // insert velocity to path if distance is not too close else insert new collision point // if original path has narrow points it's better to set higher distance threshold - if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { + if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; if (insert_idx == static_cast(inout_path->points.size())) return -1; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 9cd7f1851247..f3f510445d7e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -28,16 +28,19 @@ #include #include +// turn on only when debugging. +#define DEBUG_PRINT(enable, x) \ + if (enable) RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, x); + namespace behavior_velocity_planner { namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( - const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock, - const rclcpp::Publisher::SharedPtr publisher) -: SceneModuleInterface(module_id, logger, clock), publisher_(publisher), param_(planner_param) + const int64_t module_id, const std::shared_ptr & planner_data, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), param_(planner_param) { if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; @@ -66,6 +69,7 @@ bool OcclusionSpotModule::modifyPathVelocity( param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold; param_.v.v_ego = planner_data_->current_velocity->twist.linear.x; param_.v.a_ego = planner_data_->current_accel.get(); + param_.v.delay_time = planner_data_->delay_response_time; const double detection_area_offset = 5.0; // for visualization and stability param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( @@ -79,8 +83,9 @@ bool OcclusionSpotModule::modifyPathVelocity( PathWithLaneId interp_path; //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, &interp_path, logger_); + PathWithLaneId predicted_path; if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { - interp_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); + predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { } debug_data_.interp_path = interp_path; @@ -91,17 +96,27 @@ bool OcclusionSpotModule::modifyPathVelocity( const double offset_from_start_to_ego = -offset.get(); auto & detection_area_polygons = debug_data_.detection_area_polygons; if (!utils::buildDetectionAreaPolygon( - detection_area_polygons, interp_path, offset_from_start_to_ego, param_)) { + detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) { return true; // path point is not enough } std::vector possible_collisions; + // extract only close lanelet + if (param_.use_partition_lanelet) { + planning_utils::extractClosePartition( + ego_pose.position, debug_data_.partition_lanelets, debug_data_.close_partition); + } + if (param_.is_show_processing_time) stop_watch_.tic("processing_time"); if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { const auto & occ_grid_ptr = planner_data_->occupancy_grid; if (!occ_grid_ptr) return true; // mo data - nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; grid_map::GridMap grid_map; - grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); - if (param_.debug) publisher_->publish(occ_grid); // + grid_utils::denoiseOccupancyGridCV( + occ_grid_ptr, grid_map, param_.grid, param_.is_show_cv_window, param_.filter_occupancy_grid); + if (param_.use_object_info) { + grid_utils::addObjectsToGridMap(*planner_data_->predicted_objects, grid_map); + } + DEBUG_PRINT( + param_.is_show_processing_time, "grid [ms]: " << stop_watch_.toc("processing_time", true)); // Note: Don't consider offset from path start to ego here if (!utils::createPossibleCollisionsInDetectionArea( possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, @@ -123,8 +138,9 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } } - RCLCPP_DEBUG_STREAM_THROTTLE( - logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); + DEBUG_PRINT( + param_.is_show_processing_time, "occlusion [ms]: " << stop_watch_.toc("processing_time", true)); + DEBUG_PRINT(param_.is_show_occlusion, "num collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); @@ -134,7 +150,7 @@ bool OcclusionSpotModule::modifyPathVelocity( debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; - debug_data_.path_raw = *path; + debug_data_.path_raw = clipped_path; return true; } diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 5e1cf7e37765..5a5e2e5d755c 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -142,6 +142,18 @@ bool createDetectionAreaPolygons( return true; } +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh) +{ + for (const auto & p : all_partitions) { + if (boost::geometry::distance(Point2d(position.x, position.y), p) < distance_thresh) { + close_partition.emplace_back(p); + } + } + return; +} + void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys) { const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); @@ -150,6 +162,8 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons for (const auto & p : partition) { line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()}); } + // corect line to calculate distance accuratry + boost::geometry::correct(line); polys.emplace_back(lanelet::BasicPolygon2d(line)); } } diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp index b45c7cce4c63..013b8164ec64 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -15,9 +15,12 @@ #include "utils.hpp" #include +#include +#include #include +#include #include struct indexHash @@ -101,3 +104,114 @@ TEST(isOcclusionSpotSquare, occlusion_spot_cell) } } } + +using behavior_velocity_planner::LineString2d; +using behavior_velocity_planner::Point2d; +using behavior_velocity_planner::Polygon2d; +using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; +using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; + +Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) +{ + LineString2d line = {p0, p1}; + const double angle = atan2(p0.y() - p1.y(), p0.x() - p1.x()); + const double r = radius; + Polygon2d line_poly; + // add polygon counter clockwise + line_poly.outer().emplace_back(p0.x() + r * sin(angle), p0.y() - r * cos(angle)); + line_poly.outer().emplace_back(p1.x() + r * sin(angle), p1.y() - r * cos(angle)); + line_poly.outer().emplace_back(p1.x() - r * sin(angle), p1.y() + r * cos(angle)); + line_poly.outer().emplace_back(p0.x() - r * sin(angle), p0.y() + r * cos(angle)); + // std::cout << boost::geometry::wkt(line_poly) << std::endl; + // std::cout << boost::geometry::wkt(line) << std::endl; + return line_poly; +} + +TEST(compareTime, polygon_vs_line_iterator) +{ + // Prepare an occupancy grid with a single occlusion_spot + + /* + 0 1 2 ....200 + 0 + 1 ? ? ? ? + 2 ? ? ? . + . ? ? ? ? + 200 ? ? ? ...? + */ + const size_t cell_size = 200; + grid_map::GridMap grid = test::generateGrid(cell_size, cell_size, 0.5); + std::vector unknown_cells(cell_size); + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + const grid_map::Index index = {i, j}; + grid.at("layer", index) = UNKNOWN; + } + } + const grid_map::Matrix & grid_data = grid["layer"]; + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("processing_time"); + size_t count = 0; + [[maybe_unused]] double time = 0; + for (size_t i = 0; i < 10; i++) { + const double length = static_cast(cell_size * i) * 0.005 * std::sqrt(2); + // std::cout << "length of polygon: " << length << std::endl; + // polygon iterator + { + const Point2d p0(0, 0); + const Point2d p1(length, length); + const Polygon2d polygon = pointsToPoly(p0, p1, 0.5); + grid_map::Polygon grid_polygon; + try { + for (const auto & point : polygon.outer()) { + grid_polygon.addVertex({point.x(), point.y()}); + } + for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); + ++iterator) { + const grid_map::Index & index = *iterator; + if (grid_data(index.x(), index.y()) == OCCUPIED) { + std::cout << "occupied" << std::endl; + // occupied + } else { + count++; + // continue + } + } + } catch (const std::invalid_argument & e) { + std::cerr << e.what() << std::endl; + } + } + time = stop_watch.toc("processing_time", true); + // if (i < 4) std::cout << "polygon iterator [ms]: " << time << " :num: " << count << std::endl; + count = 0; + // line iterator + { + const grid_map::Position p0(0, 0); + const grid_map::Position p1(length, length); + try { + for (grid_map::LineIterator iterator(grid, p0, p1); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index & index = *iterator; + if (grid_data(index.x(), index.y()) == OCCUPIED) { + std::cout << "occupied" << std::endl; + // occupied + } else { + for (int i = -1; i <= 1; i++) { + for (int j = -1; j <= 1; j++) { + if (grid_data(index.x() + i, index.y() + j) == OCCUPIED) { + // for compare + } + count++; + } + } + // continue + } + } + } catch (const std::invalid_argument & e) { + std::cerr << e.what() << std::endl; + } + } + time = stop_watch.toc("processing_time", true); + // if (i < 4) std::cout << "line iterator [ms]: " << time << " :num: " << count << std::endl; + count = 0; + } +}