From dbb7cbb130debe7e9a937af97aea2bc20a207e29 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Wed, 16 Feb 2022 15:32:46 +0900 Subject: [PATCH] feat(behavior_velocity): improve occlusion spot detection area polygon (#354) * feat(behavior_velocity): create triangle detection area polygon Signed-off-by: tanaka3 * chore(behavior_velocity): refactor Signed-off-by: tanaka3 * chore(behavior_velocity): refactor and use planner param Signed-off-by: tanaka3 * chore(behavior_velocity): refactor detection area creation Signed-off-by: tanaka3 * chore(behavior_velocity): refactor possible collision search Signed-off-by: tanaka3 * feat(behavior_velocity): create dynamic poly Signed-off-by: tanaka3 * feat(behavior_velocity): move dynamic object poly to upper function layer Signed-off-by: taikitanaka Signed-off-by: tanaka3 * chore(behavior_velocity): refactor utils Signed-off-by: taikitanaka Signed-off-by: tanaka3 * feat(behavior_velocity): add da polygon to public module too Signed-off-by: taikitanaka Signed-off-by: tanaka3 * feat(behavior_velocity): add da polygon gtest Signed-off-by: taikitanaka Signed-off-by: tanaka3 * chore(behavior_velocity): improve visualization Signed-off-by: tanaka3 * chore(behavior_velocity): visualize occlusion spot Signed-off-by: tanaka3 * chore(behavior_velocity): add debug topics Signed-off-by: tanaka3 * feat(behavior_velocity): better occlusion spot detection Signed-off-by: tanaka3 * chore(behavior_velocity): better expression and early return Signed-off-by: tanaka3 * feat(behavior_velocity): change cmp method of notable collision Signed-off-by: tanaka3 * !chore(dummy_perception): disable raytracing need to revert this before merge Signed-off-by: tanaka3 * chore(behavior_velocity): fix typo Signed-off-by: tanaka3 * chore(behavior_velocity): add description of detection area polygon Signed-off-by: tanaka3 * feat(behavior_velocity): filter occlusion by detection area Signed-off-by: tanaka3 * chore(behavior_velocity): early skip at continue case Signed-off-by: tanaka3 * chore(behavior_velocity): refactor names Signed-off-by: tanaka3 * feat(behavior_velocity): care corner position at grid Signed-off-by: tanaka3 * feat(behavior_velocity): introduce detection area max length Signed-off-by: tanaka3 * chore(behavior_velocity): minor update Signed-off-by: tanaka3 * chore(behavior_velocity): better visualization Signed-off-by: tanaka3 * ci(pre-commit): autofix * chore(behavior_velocity): better comment Signed-off-by: tanaka3 * doc(behavior_velocity): update doc Signed-off-by: tanaka3 * chore(behavior_velocity): minor fix Signed-off-by: tanaka3 * !chore(behavior_velocity): revert debug values Signed-off-by: tanaka3 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/occlusion_spot.param.yaml | 15 +- .../occlusion_spot/detection_area_poly.svg | 4 + .../scene_module/occlusion_spot/geometry.hpp | 36 +-- .../occlusion_spot/grid_utils.hpp | 5 +- .../occlusion_spot/occlusion_spot_utils.hpp | 68 +++-- .../risk_predictive_braking.hpp | 27 +- .../scene_occlusion_spot_in_private_road.hpp | 3 +- .../scene_occlusion_spot_in_public_road.hpp | 7 +- .../occlusion-spot-design.md | 46 ++-- .../src/scene_module/occlusion_spot/debug.cpp | 99 ++++--- .../scene_module/occlusion_spot/geometry.cpp | 158 ++++++------ .../occlusion_spot/grid_utils.cpp | 47 ++-- .../scene_module/occlusion_spot/manager.cpp | 16 +- .../occlusion_spot/occlusion_spot_utils.cpp | 114 ++++---- .../scene_occlusion_spot_in_private_road.cpp | 29 ++- .../scene_occlusion_spot_in_public_road.cpp | 18 +- .../test/src/test_geometry.cpp | 72 ------ .../test/src/test_grid_utils.cpp | 244 ++++-------------- .../test/src/test_risk_predictive_braking.cpp | 62 ++++- 19 files changed, 522 insertions(+), 548 deletions(-) create mode 100644 planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 3397c6479a5be..078305a1b7375 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: occlusion_spot: + debug: false # [-] whether to publish debug markers. Note Default should be false for performance pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity threshold: detection_area_length: 100.0 # [m] the length of path to consider perception range @@ -9,13 +10,15 @@ motion: safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety max_slow_down_acceleration: -1.5 # [m/s^2] minimum deceleration for safe deceleration. - min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed + 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 - sidewalk: + 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_size: 1.5 # [m] size of sidewalk slices in both length and distance relative to the ego trajectory. - focus_range: 3.0 # [m] buffer around the ego path used to build the sidewalk area. + 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. 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 + 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/detection_area_poly.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg new file mode 100644 index 0000000000000..bf19afce34657 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg @@ -0,0 +1,4 @@ + + + +
t_4
ego
ego
offset
offset
area which obstacle is coming  before ego vehicle pass
area which obstacle is coming  befo...
front bamper
front ba...
lateral max distance
lateral m...
calculate  lateral distance from TTC top consider case if obstacle coming out from here
calculate  lateral distance from TTC top consider c...
t_1
interpolated polygon
interpolated polygon
t_4
expand detail
expand detail
collision point of obstacle
collision point of obst...
place where obstacle  might be coming
place where obstacle  m...
area which ego vehicle pass before hidden obstacle is coming
area which ego vehicle pass before...
area which ego vehicle pass before hidden obstacle is coming
area which ego vehicle pass before...
detection area max length
detection area max length
max length is calculated from ego current velocity and current acceleration
max length is calculated from ego current velocity and current acceleration
area which planned velocity is not effective
area which planned velocity i...
area which planned velocity is not effective
area which planned velocity i...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp index 1f7fde9acd103..e022df839bccb 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp @@ -15,6 +15,8 @@ #ifndef SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ #define SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ +#include + #include #include @@ -24,37 +26,19 @@ namespace behavior_velocity_planner { -namespace geometry +namespace occlusion_spot_utils { namespace bg = boost::geometry; -// @brief represent the range of a slice -// (either by index on the lanelet centerline or by arc coordinates) -struct SliceRange -{ - double min_length{}; - double max_length{}; - double min_distance{}; - double max_distance{}; -}; - -// @brief representation of a slice along a path -struct Slice -{ - SliceRange range{}; - lanelet::BasicPolygon2d polygon{}; -}; - //!< @brief build slices all along the trajectory // using the given range and desired slice length and width void buildSlices( - std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, const double slice_width, const double resolution); -//!< @brief build sidewalk slice from path -void buildSidewalkSlices( - std::vector & slice, const lanelet::ConstLanelet & path_lanelet, - const double longitudinal_offset, const double lateral_offset, const double min_size, - const double lateral_max_dist); + std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const double offset, + const bool is_on_right, const PlannerParam & param); +//!< @brief build detection_area slice from path +void buildDetectionAreaPolygon( + std::vector & slices, const PathWithLaneId & path, const double offset, + const PlannerParam & param); //!< @brief calculate interpolation between a and b at distance ratio t template T lerp(T a, T b, double t) @@ -62,7 +46,7 @@ T lerp(T a, T b, double t) return a + t * (b - a); } -} // namespace geometry +} // namespace occlusion_spot_utils } // namespace behavior_velocity_planner #endif // SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ 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 1601d49b5806d..8d7edf060cf6f 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 @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -51,7 +50,7 @@ struct OcclusionSpotSquare { grid_map::Index index; // index of the anchor grid_map::Position position; // position of the anchor - int side_size; // number of cells for each side of the square + int min_occlusion_size; // number of cells for each side of the square }; // @brief structure representing a OcclusionSpot on the OccupancyGrid struct OcclusionSpot @@ -64,7 +63,7 @@ struct OcclusionSpot // if the given cell is a occlusion_spot square of size min_size*min_size in the given grid bool isOcclusionSpotSquare( OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data, - const grid_map::Index & cell, const int side_size, const grid_map::Size & grid_size); + const grid_map::Index & cell, const int min_occlusion_size, const grid_map::Size & grid_size); //!< @brief Find all occlusion spots inside the given lanelet void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, 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 e03ec0d154597..260fdb40357be 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 @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -30,6 +29,8 @@ #include #include +#include + #include #include #include @@ -65,10 +66,10 @@ namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; -struct Sidewalk +struct DetectionArea { - double focus_range; // [m] distance to care about occlusion spot - double slice_size; // [m] size of each slice + 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 }; struct Velocity @@ -77,6 +78,8 @@ struct Velocity double max_stop_jerk; // [m/s^3] emergency braking system jerk double max_stop_accel; // [m/s^2] emergency braking system deceleration double max_slow_down_accel; // [m/s^2] maximum allowed deceleration + double non_effective_jerk; // [m/s^3] too weak jerk for velocity planning. + double non_effective_accel; // [m/s^2] too weak deceleration for velocity planning. double min_allowed_velocity; // [m/s] minimum allowed velocity not to stop double a_ego; // [m/s^2] current ego acceleration double v_ego; // [m/s] current ego velocity @@ -84,24 +87,31 @@ struct Velocity double safe_margin; // [m] maximum safety distance for any error }; +struct LatLon +{ + double lateral_distance; // [m] lateral distance + double longitudinal_distance; // [m] longitudinal distance +}; + struct PlannerParam { + bool debug; // [-] // parameters in yaml - double detection_area_length; // [m] - double stuck_vehicle_vel; // [m/s] - double lateral_distance_thr; // [m] lateral distance threshold to consider - double pedestrian_vel; // [m/s] + 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 dist_thr; // [m] - double angle_thr; // [rad] - bool show_debug_grid; // [-] + double dist_thr; // [m] + double angle_thr; // [rad] // vehicle info double half_vehicle_width; // [m] half vehicle_width from vehicle info double baselink_to_front; // [m] wheel_base + front_overhang Velocity v; - Sidewalk sidewalk; + DetectionArea detection_area; grid_utils::GridParam grid; }; @@ -111,6 +121,22 @@ struct SafeMotion double safe_velocity; }; +// @brief represent the range of a each polygon +struct SliceRange +{ + double min_length{}; + double max_length{}; + double min_distance{}; + double max_distance{}; +}; + +// @brief representation of a polygon along a path +struct Slice +{ + SliceRange range{}; + lanelet::BasicPolygon2d polygon{}; +}; + struct ObstacleInfo { SafeMotion safe_motion; // safe motion of velocity and stop point @@ -199,6 +225,8 @@ inline bool isStuckVehicle(PredictedObject obj, const double min_vel) } void filterCollisionByRoadType( std::vector & possible_collisions, const DetectionAreaIdx road_type); +std::vector filterDynamicObjectByDetectionArea( + std::vector & objs, const std::vector polys); bool splineInterpolate( const PathWithLaneId & input, const double interval, PathWithLaneId * output, const rclcpp::Logger logger); @@ -228,17 +256,17 @@ void calcSlowDownPointsForPossibleCollision( DetectionAreaIdx extractTargetRoadArcLength( const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path, const ROAD_TYPE & target_road_type); -//!< @brief convert a set of occlusion spots found on sidewalk slice -void generateSidewalkPossibleCollisionFromOcclusionSpot( - std::vector & possible_collisions, const grid_map::GridMap & grid, - const std::vector & occlusion_spot_positions, - const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet, - const PlannerParam & param); +//!< @brief convert a set of occlusion spots found on detection_area slice +boost::optional generateOneNotableCollisionFromOcclusionSpot( + const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, + const double offset_from_start_to_ego, const BasicPoint2d basic_point, + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param); //!< @brief generate possible collisions coming from occlusion spots on the side of the path -void generateSidewalkPossibleCollisions( +void createPossibleCollisionsInDetectionArea( + const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, - std::vector & debug); + std::vector & debug_points); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp index 10e79f71930cb..59145de966dfe 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -44,7 +44,32 @@ inline double calculateMinSlowDownVelocity( } /** - * @param: sv: ego velocity config + * + * @param: longitudinal_distance: longitudinal distance to collision + * @param: param: planner param + * @return lateral distance + **/ +inline double calculateLateralDistanceFromTTC( + const double longitudinal_distance, const PlannerParam & param) +{ + const auto & v = param.v; + const auto & p = param; + double v_min = 1.0; + const double lateral_buffer = 0.5; + const double min_distance = p.half_vehicle_width + lateral_buffer; + const double max_distance = p.detection_area.max_lateral_distance; + if (longitudinal_distance <= 0) return min_distance; + if (v_min < param.v.min_allowed_velocity) v_min = param.v.min_allowed_velocity; + // use min velocity if ego velocity is below min allowed + const double v0 = (v.v_ego > v_min) ? v.v_ego : v_min; + // here is a part where ego t(ttc) can be replaced by calculation of velocity smoother or ? + double t = longitudinal_distance / v0; + double lateral_distance = t * param.pedestrian_vel + p.half_vehicle_width; + return std::min(max_distance, std::max(min_distance, lateral_distance)); +} + +/** + * @param: v: ego velocity config * @param: ttc: time to collision * @return safe motion **/ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp index 93d382576c29e..fed53518eb1ca 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp @@ -46,8 +46,9 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface { double z; std::string road_type = "private"; - std::vector sidewalks; + std::vector detection_areas; std::vector possible_collisions; + std::vector occlusion_points; PathWithLaneId path_raw; PathWithLaneId interp_path; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index 250972dea8342..869084e7ed410 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -44,10 +44,13 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface struct DebugData { std::string road_type = "public"; - double z; - std::vector sidewalks; + std::vector detection_areas; std::vector parked_vehicle_point; std::vector possible_collisions; + std::vector occlusion_points; + PathWithLaneId path_raw; + PathWithLaneId interp_path; + double z; }; OcclusionSpotInPublicModule( diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index 2cbc0d2368fd0..7dc1cb6330cd1 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -47,17 +47,11 @@ This module considers any occlusion spot around ego path computed from the occup ![occupancy_grid](./docs/occlusion_spot/occupancy_grid.svg) -Occlusion spot computation: searching occlusion spots for all cells in the occupancy_grid inside "focus range" requires a lot of computational cost, so this module will stop searching if the first occlusion spot is found in the following searching process. - -![brief](./docs/occlusion_spot/sidewalk_slice.svg) - -Note that the accuracy and performance of this search method is limited due to the approximation. - #### Occlusion Spot Common ##### The Concept of Safe Velocity -Safe velocity is calculated from below parameters of ego emergency braking system and time to collision. +Safe velocity is calculated from the below parameters of ego emergency braking system and time to collision. - jerk limit[m/s^3] - deceleration limit[m/s2] @@ -68,11 +62,18 @@ Safe velocity is calculated from below parameters of ego emergency braking syste ##### Safe Behavior After Passing Safe Margin Point -This module defines safe margin consider ego distance to stop and collision path point geometrically. -while ego is cruising from safe margin to collision path point ego keeps same velocity as occlusion spot safe velocity. +This module defines safe margin to consider ego distance to stop and collision path point geometrically. +While ego is cruising from safe margin to collision path point, ego vehicle keeps the same velocity as occlusion spot safe velocity. ![brief](./docs/occlusion_spot/behavior_after_safe_margin.svg) +##### Detection area polygon + +Occlusion spot computation: searching occlusion spots for all cells in the occupancy_grid inside "max lateral distance" requires a lot of computational cost, so this module use only one most notable occlusion spot for each partition. (currently offset is from baselink to front for safety) +The maximum length of detection area depends on ego current vehicle velocity and acceleration. + +![brief](./docs/occlusion_spot/detection_area_poly.svg) + #### Module Parameters | Parameter | Type | Description | @@ -93,16 +94,16 @@ while ego is cruising from safe margin to collision path point ego keeps same ve | `delay_time` | double | [m/s] time buffer for the system delay | | `safe_margin` | double | [m] maximum error to stop with emergency braking system. | -| Parameter /sidewalk | Type | Description | -| ------------------------- | ------ | --------------------------------------------------------------- | -| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot | -| `slice_size` | double | [m] the distance of divided detection area | -| `focus_range` | double | [m] buffer around the ego path used to build the sidewalk area. | +| Parameter /detection_area | Type | Description | +| ------------------------- | ------ | --------------------------------------------------------------------- | +| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot | +| `slice_length` | double | [m] the distance of divided detection area | +| `max_lateral_distance` | double | [m] buffer around the ego path used to build the detection_area area. | -| Parameter /grid | Type | Description | -| ---------------- | ------ | --------------------------------------------------------------- | -| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid | -| `occupied_min` | double | [-] buffer around the ego path used to build the sidewalk area. | +| Parameter /grid | Type | Description | +| ---------------- | ------ | --------------------------------------------------------------------- | +| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid | +| `occupied_min` | double | [-] buffer around the ego path used to build the detection_area area. | #### Flowchart @@ -135,8 +136,13 @@ else (no) stop endif } -partition find_possible_collision { :calculate offset from start to ego; +partition generate_detection_area_polygon { +:convert path to path lanelet; +:generate left/right slice of polygon that starts from path start; +:generate interpolated polygon which is created from ego TTC and lateral distance that pedestrian can reach within ego TTC.; +} +partition find_possible_collision { :generate possible collision; :calculate collision path point and intersection point; note right @@ -201,6 +207,7 @@ note right - velocity is below `stuck_vehicle_vel`. end note } +:generate_detection_area_polygon; partition find_possible_collision { :generate possible collision behind parked vehicle; note right @@ -258,6 +265,7 @@ note right convert from occupancy grid to image to use opencv functions. end note } +:generate_detection_area_polygon; partition generate_possible_collision { :calculate offset from path start to ego; :generate possible collision from occlusion spot; 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 4af5a95ce4413..4cae4d7b92a80 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 @@ -27,13 +27,15 @@ namespace behavior_velocity_planner { namespace { +using builtin_interfaces::msg::Time; + visualization_msgs::msg::Marker makeArrowMarker( - const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, int id) + const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id) { visualization_msgs::msg::Marker debug_marker; debug_marker.header.frame_id = "map"; debug_marker.ns = "occlusion spot arrow"; - debug_marker.id = id; + debug_marker.id = planning_utils::bitShift(id); debug_marker.type = visualization_msgs::msg::Marker::ARROW; debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.05, 0.2, 0.5); @@ -49,7 +51,7 @@ visualization_msgs::msg::Marker makeArrowMarker( std::vector makeSlowDownMarkers( const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, - const std::string road_type, int id) + const std::string road_type, const int id) { // virtual wall std::vector debug_markers; @@ -59,7 +61,7 @@ std::vector makeSlowDownMarkers( wall_marker.lifetime = rclcpp::Duration::from_seconds(0.5); wall_marker.type = visualization_msgs::msg::Marker::CUBE; wall_marker.action = visualization_msgs::msg::Marker::ADD; - wall_marker.id = id; + wall_marker.id = planning_utils::bitShift(id); // cylinder at collision point wall_marker.pose = possible_collision.intersection_pose; wall_marker.pose.position.z += 1.0; @@ -73,7 +75,7 @@ std::vector makeSlowDownMarkers( visualization_msgs::msg::Marker slowdown_reason_marker; slowdown_reason_marker.header.frame_id = "map"; slowdown_reason_marker.ns = "slow factor_text"; - slowdown_reason_marker.id = id; + slowdown_reason_marker.id = planning_utils::bitShift(id); slowdown_reason_marker.lifetime = rclcpp::Duration::from_seconds(0.5); slowdown_reason_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; slowdown_reason_marker.action = visualization_msgs::msg::Marker::ADD; @@ -91,13 +93,14 @@ std::vector makeSlowDownMarkers( } std::vector makeCollisionMarkers( - const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, int id, bool show_text) + const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id, + const bool show_text) { std::vector debug_markers; visualization_msgs::msg::Marker debug_marker; debug_marker.header.frame_id = "map"; debug_marker.ns = "collision_point"; - debug_marker.id = id; + debug_marker.id = planning_utils::bitShift(id); // cylinder at collision with margin point debug_marker.type = visualization_msgs::msg::Marker::CYLINDER; debug_marker.pose = possible_collision.collision_with_margin.pose; @@ -139,12 +142,13 @@ std::vector makeCollisionMarkers( } visualization_msgs::msg::MarkerArray makePolygonMarker( - const std::vector & polygons, double z) + const std::vector & polygons, const int id, const double z) { visualization_msgs::msg::MarkerArray debug_markers; visualization_msgs::msg::Marker debug_marker; debug_marker.header.frame_id = "map"; - debug_marker.id = 0; + debug_marker.header.stamp = rclcpp::Time(0); + 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); @@ -152,7 +156,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( 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); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); - debug_marker.ns = "sidewalk"; + debug_marker.ns = "detection_area"; for (const auto & poly : polygons) { for (const auto & p : poly) { geometry_msgs::msg::Point point = @@ -196,23 +200,15 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } template -visualization_msgs::msg::MarkerArray createMarkers( +visualization_msgs::msg::MarkerArray createPossibleCollisionMarkers( T & debug_data, [[maybe_unused]] const int64_t module_id_) { // add slow down markers for occlusion spot visualization_msgs::msg::MarkerArray occlusion_spot_slowdown_markers; auto & possible_collisions = debug_data.possible_collisions; - // sort collision - std::sort( - possible_collisions.begin(), possible_collisions.end(), - []( - occlusion_spot_utils::PossibleCollisionInfo pc1, - occlusion_spot_utils::PossibleCollisionInfo pc2) { - return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; - }); // draw virtual wall markers - int id = 0; + int id = planning_utils::bitShift(module_id_); for (const auto & possible_collision : possible_collisions) { std::vector collision_markers = makeSlowDownMarkers(possible_collision, debug_data.road_type, id++); @@ -222,7 +218,7 @@ visualization_msgs::msg::MarkerArray createMarkers( } // draw obstacle collision - id = 0; + id = planning_utils::bitShift(module_id_); for (const auto & possible_collision : possible_collisions) { // debug marker std::vector collision_markers = @@ -235,6 +231,25 @@ visualization_msgs::msg::MarkerArray createMarkers( return occlusion_spot_slowdown_markers; } +visualization_msgs::msg::MarkerArray createOcclusionMarkerArray( + const std::vector & occlusion_points, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + { + const Time now = rclcpp::Time(0); + auto marker = createDefaultMarker( + "map", now, "occlusion", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.scale = createMarkerScale(0.5, 0.5, 0.5); + marker.lifetime = rclcpp::Duration::from_seconds(0.1); + for (size_t i = 0; i < occlusion_points.size(); ++i) { + marker.id = i + planning_utils::bitShift(module_id); + marker.pose.position = occlusion_points.at(i); + msg.markers.push_back(marker); + } + } + return msg; +} } // namespace visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMarkerArray() @@ -242,7 +257,16 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar const auto current_time = this->clock_->now(); visualization_msgs::msg::MarkerArray debug_marker_array; - appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array); + if (!debug_data_.possible_collisions.empty()) { + appendMarkerArray( + createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array); + } + if (!debug_data_.detection_areas.empty()) { + appendMarkerArray( + makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time, + &debug_marker_array); + } + return debug_marker_array; } visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMarkerArray() @@ -250,15 +274,28 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMa const auto current_time = this->clock_->now(); visualization_msgs::msg::MarkerArray debug_marker_array; - appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array); - appendMarkerArray( - makePolygonMarker(debug_data_.sidewalks, debug_data_.z), current_time, &debug_marker_array); - appendMarkerArray( - createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), current_time, - &debug_marker_array); - appendMarkerArray( - createPathMarkerArray(debug_data_.interp_path, "path_interp", 0, 0.0, 1.0, 1.0), current_time, - &debug_marker_array); + if (!debug_data_.possible_collisions.empty()) { + appendMarkerArray( + createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array); + } + if (!debug_data_.detection_areas.empty()) { + appendMarkerArray( + makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time, + &debug_marker_array); + } + if (!debug_data_.interp_path.points.empty()) { + appendMarkerArray( + createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), current_time, + &debug_marker_array); + appendMarkerArray( + createPathMarkerArray(debug_data_.interp_path, "path_interp", 0, 0.0, 1.0, 1.0), current_time, + &debug_marker_array); + } + if (!debug_data_.occlusion_points.empty()) { + appendMarkerArray( + createOcclusionMarkerArray(debug_data_.occlusion_points, module_id_), current_time, + &debug_marker_array); + } return debug_marker_array; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp index e88274458e229..acb1827bc686b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp @@ -16,13 +16,15 @@ #include #include #include +#include +#include #include #include namespace behavior_velocity_planner { -namespace geometry +namespace occlusion_spot_utils { using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; @@ -30,106 +32,102 @@ using lanelet::BasicPolygon2d; namespace bg = boost::geometry; namespace lg = lanelet::geometry; -void createOffsetLineString( - const BasicLineString2d & in, const double offset, BasicLineString2d & offset_line_string) +BasicPoint2d calculateLateralOffsetPoint( + const BasicPoint2d & p0, const BasicPoint2d & p1, const double offset) { - for (size_t i = 0; i < in.size() - 1; i++) { - const auto & p0 = in.at(i); - const auto & p1 = in.at(i + 1); - // translation - const double dy = p1[1] - p0[1]; - const double dx = p1[0] - p0[0]; - // rotation (use inverse matrix of rotation) - const double yaw = std::atan2(dy, dx); - // translation - const double offset_x = p0[0] - std::sin(yaw) * offset; - const double offset_y = p0[1] + std::cos(yaw) * offset; - offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y}); - //! insert final offset linestring using prev vertical direction - if (i == in.size() - 2) { - const double offset_x = p1[0] - std::sin(yaw) * offset; - const double offset_y = p1[1] + std::cos(yaw) * offset; - offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y}); - } - } - return; + // translation + const double dy = p1[1] - p0[1]; + const double dx = p1[0] - p0[0]; + // rotation (use inverse matrix of rotation) + const double yaw = std::atan2(dy, dx); + const double offset_x = p1[0] - std::sin(yaw) * offset; + const double offset_y = p1[1] + std::cos(yaw) * offset; + return BasicPoint2d{offset_x, offset_y}; } void buildSlices( - std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, const double slice_width, const double resolution) + std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const double offset, + const bool is_on_right, const PlannerParam & param) { - const int num_lateral_slice = - static_cast(std::abs(range.max_distance - range.min_distance) / slice_width); + BasicLineString2d center_line = path_lanelet.centerline2d().basicLineString(); + const auto & p = param; /** - * @brief bounds - * +---------- outer bounds - * | +------ inner bounds(original path) - * | | + * @brief relationships for interpolated polygon + * + * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons + * | | + * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons */ - BasicLineString2d inner_bounds = path_lanelet.centerline2d().basicLineString(); - BasicLineString2d outer_bounds; - if (inner_bounds.size() < 2) return; - createOffsetLineString(inner_bounds, range.max_distance, outer_bounds); - const double ratio_dist_start = std::abs(range.min_distance / range.max_distance); - const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance)); - lanelet::BasicPolygon2d poly; - const int num_step = static_cast(slice_length / resolution); + const double min_length = offset; // + p.baselink_to_front; + // Note: min_detection_area_length is for occlusion spot visualization but not effective for + // planning + const double min_detection_area_length = 10.0; + const double max_length = std::max( + min_detection_area_length, std::min(p.detection_area_max_length, p.detection_area_length)); + const double min_distance = (is_on_right) ? -p.half_vehicle_width : p.half_vehicle_width; + const double slice_length = p.detection_area.slice_length; + const int num_step = static_cast(slice_length); //! max index is the last index of path point - const int max_index = static_cast(inner_bounds.size() - 1); + const int max_index = static_cast(center_line.size() - 2); + int idx = 0; + /** + * Note: create polygon from path point is better than from ego offset to consider below + * - less computation cost and no need to recalculated interpolated point start from ego + * - less stable for localization noise + **/ for (int s = 0; s < max_index; s += num_step) { const double length = s * slice_length; - const double next_length = (s + num_step) * resolution; - for (int d = 0; d < num_lateral_slice; d++) { - const double ratio_dist = ratio_dist_start + d * ratio_dist_increment; - const double next_ratio_dist = ratio_dist_start + (d + 1.0) * ratio_dist_increment; - Slice slice; - BasicLineString2d inner_polygons; - BasicLineString2d outer_polygons; - // build interpolated polygon for lateral - for (int i = 0; i <= num_step; i++) { - if (s + i >= max_index) continue; - inner_polygons.emplace_back( - lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), ratio_dist)); - outer_polygons.emplace_back( - lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), next_ratio_dist)); - } - // Build polygon - inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); - slice.polygon = lanelet::BasicPolygon2d(inner_polygons); - // add range info - slice.range.min_length = length; - slice.range.max_length = next_length; - slice.range.min_distance = ratio_dist * range.max_distance; - slice.range.max_distance = next_ratio_dist * range.max_distance; - slices.emplace_back(slice); + const double next_length = static_cast(s + num_step); + /// if (max_length < length) continue; + Slice slice; + BasicLineString2d inner_polygons; + BasicLineString2d outer_polygons; + // build connected polygon for lateral + for (int i = 0; i <= num_step; i++) { + idx = s + i; + const double arc_length_from_ego = std::max(0.0, static_cast(idx - min_length)); + if (arc_length_from_ego > max_length) break; + if (idx >= max_index) break; + const auto & c0 = center_line.at(idx); + const auto & c1 = center_line.at(idx + 1); + /** + * @brief points + * +--outer point (lateral distance obstacle can reach) + * | + * +--inner point(min distance) + */ + const BasicPoint2d inner_point = calculateLateralOffsetPoint(c0, c1, min_distance); + double lateral_distance = calculateLateralDistanceFromTTC(arc_length_from_ego, param); + if (is_on_right) lateral_distance *= -1; + const BasicPoint2d outer_point = calculateLateralOffsetPoint(c0, c1, lateral_distance); + inner_polygons.emplace_back(inner_point); + outer_polygons.emplace_back(outer_point); } + if (inner_polygons.empty()) continue; + // connect invert point + inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); + slice.polygon = lanelet::BasicPolygon2d(inner_polygons); + // add range info + slice.range.min_length = length; + slice.range.max_length = next_length; + slices.emplace_back(slice); } } -void buildSidewalkSlices( - std::vector & slices, const lanelet::ConstLanelet & path_lanelet, - const double longitudinal_offset, const double lateral_offset, const double slice_size, - const double lateral_max_dist) +void buildDetectionAreaPolygon( + std::vector & slices, const PathWithLaneId & path, const double offset, + const PlannerParam & param) { std::vector left_slices; std::vector right_slices; - const double longitudinal_max_dist = lg::length2d(path_lanelet); - SliceRange left_slice_range = { - longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist}; + lanelet::ConstLanelet path_lanelet = toPathLanelet(path); // in most case lateral distance is much more effective for velocity planning - const double slice_length = 4.0 * slice_size; - const double slice_width = slice_size; - const double resolution = 1.0; - buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution); - SliceRange right_slice_range = { - longitudinal_offset, longitudinal_max_dist, -lateral_offset, - -lateral_offset - lateral_max_dist}; - buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution); + buildSlices(left_slices, path_lanelet, offset, false /*is_on_right*/, param); + buildSlices(right_slices, path_lanelet, offset, true /*is_on_right*/, param); // Properly order lanelets from closest to furthest slices = left_slices; slices.insert(slices.end(), right_slices.begin(), right_slices.end()); return; } -} // namespace geometry +} // namespace occlusion_spot_utils } // namespace behavior_velocity_planner 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 498c873b30d5f..6e7d31e4b5705 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 @@ -24,15 +24,22 @@ namespace grid_utils { bool isOcclusionSpotSquare( OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data, - const grid_map::Index & cell, int side_size, const grid_map::Size & grid_size) + const grid_map::Index & cell, int min_occlusion_size, const grid_map::Size & grid_size) { + const int offset = (min_occlusion_size != 1) ? (min_occlusion_size - 1) : min_occlusion_size; + const int cell_max_x = grid_size.x() - 1; + const int cell_max_y = grid_size.y() - 1; // Calculate ranges to check - int min_x; - int max_x; - int min_y; - int max_y; + int min_x = cell.x() - offset; + int max_x = cell.x() + offset; + int min_y = cell.y() - offset; + int max_y = cell.y() + offset; + if (min_x < 0) max_x += std::abs(min_x); + if (max_x > cell_max_x) min_x -= std::abs(max_x - cell_max_x); + if (min_y < 0) max_y += std::abs(min_y); + if (max_y > cell_max_y) min_y -= std::abs(max_y - cell_max_y); // No occlusion_spot with size 0 - if (side_size == 0) { + if (min_occlusion_size == 0) { return false; } /** @@ -41,25 +48,31 @@ bool isOcclusionSpotSquare( * . . * (min_x,max_y)...(max_x,max_y) */ - const int offset = side_size - 1; - min_x = cell.x(); - max_x = cell.x() + offset; - min_y = cell.y() - offset; - max_y = cell.y(); // Ensure we stay inside the grid min_x = std::max(0, min_x); - max_x = std::min(grid_size.x() - 1, max_x); + max_x = std::min(cell_max_x, max_x); min_y = std::max(0, min_y); - max_y = std::min(grid_size.y() - 1, max_y); + max_y = std::min(cell_max_y, max_y); + int not_unknown_count = 0; + if (grid_data(cell.x(), cell.y()) != grid_utils::occlusion_cost_value::UNKNOWN) { + return false; + } for (int x = min_x; x <= max_x; ++x) { for (int y = min_y; y <= max_y; ++y) { // if the value is not unknown value return false if (grid_data(x, y) != grid_utils::occlusion_cost_value::UNKNOWN) { - return false; + not_unknown_count++; } + /** + * @brief case pass o: unknown x: freespace or occupied + * oxx oxo oox xxx oxo oxo + * oox oxx oox ooo oox oxo ... etc + * ooo ooo oox ooo xoo oxo + */ + if (not_unknown_count > min_occlusion_size + 1) return false; } } - occlusion_spot.side_size = side_size; + occlusion_spot.min_occlusion_size = min_occlusion_size; occlusion_spot.index = cell; return true; } @@ -112,12 +125,12 @@ void getCornerPositions( const OcclusionSpotSquare & occlusion_spot_square) { // Special case with size = 1: only one cell - if (occlusion_spot_square.side_size == 1) { + if (occlusion_spot_square.min_occlusion_size == 1) { corner_positions.emplace_back(occlusion_spot_square.position); return; } std::vector corner_indexes; - const int offset = (occlusion_spot_square.side_size - 1) / 2; + const int offset = (occlusion_spot_square.min_occlusion_size - 1) / 2; /** * @brief relation of each grid position * bl br 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 316873d80d751..739fa9169c461 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 @@ -80,6 +80,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & pp = planner_param_; // assume pedestrian coming out from occlusion spot with this velocity + pp.debug = node.declare_parameter(ns + ".debug", false); pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); 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); @@ -87,19 +88,22 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) 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); - pp.show_debug_grid = node.declare_parameter(ns + ".show_debug_grid", false); // 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_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -1.5); + pp.v.non_effective_jerk = node.declare_parameter(ns + ".motion.non_effective_jerk", -0.3); + pp.v.non_effective_accel = + node.declare_parameter(ns + ".motion.non_effective_acceleration", -1.0); pp.v.min_allowed_velocity = node.declare_parameter(ns + ".motion.min_allowed_velocity", 1.0); - // sidewalk param - pp.sidewalk.min_occlusion_spot_size = - node.declare_parameter(ns + ".sidewalk.min_occlusion_spot_size", 2.0); - pp.sidewalk.focus_range = node.declare_parameter(ns + ".sidewalk.focus_range", 4.0); - pp.sidewalk.slice_size = node.declare_parameter(ns + ".sidewalk.slice_size", 1.5); + // 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.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); // occupancy grid param pp.grid.free_space_max = node.declare_parameter(ns + ".grid.free_space_max", 10); pp.grid.occupied_min = node.declare_parameter(ns + ".grid.occupied_min", 51); 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 4922d994bc8e0..6b07021e50d5f 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 @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace behavior_velocity_planner { +namespace bg = boost::geometry; namespace occlusion_spot_utils { lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path) @@ -400,6 +402,23 @@ DetectionAreaIdx extractTargetRoadArcLength( return DetectionAreaIdx(std::make_pair(start_dist, dist_sum)); } +std::vector filterDynamicObjectByDetectionArea( + std::vector & objs, const std::vector polys) +{ + std::vector filtered_obj; + // stuck points by predicted objects + for (const auto & object : objs) { + // check if the footprint is in the stuck detect area + const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); + for (const auto & p : polys) { + if (!bg::disjoint(obj_footprint, p.polygon)) { + filtered_obj.emplace_back(object); + } + } + } + return filtered_obj; +} + void filterCollisionByRoadType( std::vector & possible_collisions, const DetectionAreaIdx area) { @@ -415,61 +434,45 @@ void filterCollisionByRoadType( } } -void generateSidewalkPossibleCollisions( +void createPossibleCollisionsInDetectionArea( + const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, - std::vector & debug) + std::vector & debug_points) { lanelet::ConstLanelet path_lanelet = toPathLanelet(path); if (path_lanelet.centerline2d().empty()) { return; } - std::vector sidewalk_slices; - geometry::buildSidewalkSlices( - sidewalk_slices, path_lanelet, 0.0, param.half_vehicle_width, param.sidewalk.slice_size, - param.sidewalk.focus_range); - double length_lower_bound = std::numeric_limits::max(); double distance_lower_bound = std::numeric_limits::max(); - // sort distance closest first to skip inferior collision - std::sort( - sidewalk_slices.begin(), sidewalk_slices.end(), - [](const geometry::Slice & s1, const geometry::Slice & s2) { - return std::abs(s1.range.min_distance) < std::abs(s2.range.min_distance); - }); - - std::sort( - sidewalk_slices.begin(), sidewalk_slices.end(), - [](const geometry::Slice & s1, const geometry::Slice & s2) { - return s1.range.min_length < s2.range.min_length; - }); - - for (const geometry::Slice & sidewalk_slice : sidewalk_slices) { - debug.push_back(sidewalk_slice.polygon); - if ((sidewalk_slice.range.min_length < length_lower_bound || - std::abs(sidewalk_slice.range.min_distance) < distance_lower_bound)) { - std::vector occlusion_spot_positions; - grid_utils::findOcclusionSpots( - occlusion_spot_positions, grid, sidewalk_slice.polygon, - param.sidewalk.min_occlusion_spot_size); - generateSidewalkPossibleCollisionFromOcclusionSpot( - possible_collisions, grid, occlusion_spot_positions, offset_from_start_to_ego, path_lanelet, - param); - if (!possible_collisions.empty()) { - length_lower_bound = sidewalk_slice.range.min_length; - distance_lower_bound = std::abs(sidewalk_slice.range.min_distance); - possible_collisions.insert( - possible_collisions.end(), possible_collisions.begin(), possible_collisions.end()); - debug.push_back(sidewalk_slice.polygon); - } + for (const Slice & detection_area_slice : detection_area_polygons) { + std::vector occlusion_spot_positions; + grid_utils::findOcclusionSpots( + occlusion_spot_positions, grid, detection_area_slice.polygon, + param.detection_area.min_occlusion_spot_size); + Point p; + for (const auto & op : occlusion_spot_positions) { + p.x = op[0]; + p.y = op[1]; + debug_points.emplace_back(p); } + if (occlusion_spot_positions.empty()) continue; + // for each partition find nearest occlusion spot from polygon's origin + BasicPoint2d base_point = detection_area_slice.polygon.at(0); + const auto pc = generateOneNotableCollisionFromOcclusionSpot( + grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param); + if (!pc) continue; + const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance); + if (lateral_distance > distance_lower_bound) continue; + distance_lower_bound = lateral_distance; + possible_collisions.emplace_back(pc.get()); } } -void generateSidewalkPossibleCollisionFromOcclusionSpot( - std::vector & possible_collisions, const grid_map::GridMap & grid, - const std::vector & occlusion_spot_positions, - const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet, - const PlannerParam & param) +boost::optional generateOneNotableCollisionFromOcclusionSpot( + const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, + const double offset_from_start_to_ego, const BasicPoint2d base_point, + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { const double baselink_to_front = param.baselink_to_front; const double half_vehicle_width = param.half_vehicle_width; @@ -481,27 +484,34 @@ void generateSidewalkPossibleCollisionFromOcclusionSpot( lanelet::BasicPoint2d obstacle_point = {occlusion_spot_position[0], occlusion_spot_position[1]}; lanelet::ArcCoordinates arc_coord_occlusion_point = lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); + const double dist = + std::hypot(base_point[0] - obstacle_point[0], base_point[1] - obstacle_point[1]); + // skip if absolute distance is larger + if (distance_lower_bound < dist) continue; const double length_to_col = arc_coord_occlusion_point.length - baselink_to_front; - ArcCoordinates arc_coord_collision_point = { - length_to_col, - calcSignedLateralDistanceWithOffset(arc_coord_occlusion_point.distance, half_vehicle_width)}; + // skip if occlusion is behind ego bumper if (length_to_col < offset_from_start_to_ego) { continue; } + ArcCoordinates arc_coord_collision_point = { + length_to_col, + calcSignedLateralDistanceWithOffset(arc_coord_occlusion_point.distance, half_vehicle_width)}; 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)); - // this is going to extract collision that is nearest to path - if ( - collision_free_at_intersection && distance_lower_bound > arc_coord_collision_point.distance) { - distance_lower_bound = arc_coord_collision_point.distance; - candidate = {pc}; + if (collision_free_at_intersection) { + distance_lower_bound = dist; + candidate = pc; has_collision = true; } } - if (has_collision) possible_collisions.emplace_back(candidate); + if (has_collision) { + return candidate; + } else { + return {}; + } } } // namespace occlusion_spot_utils diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp index 2bd245a91f76b..b8e7a3a9f77da 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -45,6 +46,7 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); + debug_data_.road_type = "private"; if (path->points.size() < 2) { return true; } @@ -54,6 +56,9 @@ bool OcclusionSpotInPrivateModule::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_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( + param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk, + 0.0); } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; const auto & lanelet_map_ptr = planner_data_->lanelet_map; @@ -67,6 +72,7 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( PathWithLaneId clipped_path; utils::clipPathByLength(*path, clipped_path, max_range); PathWithLaneId interp_path; + //! never change this interpolation interval(will affect module accuracy) utils::splineInterpolate(clipped_path, 1.0, &interp_path, logger_); debug_data_.interp_path = interp_path; int closest_idx = -1; @@ -82,18 +88,20 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( 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_.show_debug_grid) { - publisher_->publish(occ_grid); - } double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); + using Slice = occlusion_spot_utils::Slice; + std::vector detection_area_polygons; + utils::buildDetectionAreaPolygon( + detection_area_polygons, interp_path, offset_from_start_to_ego, param_); RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego); std::vector possible_collisions; // Note: Don't consider offset from path start to ego here - utils::generateSidewalkPossibleCollisions( - possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, - debug_data_.sidewalks); + utils::createPossibleCollisionsInDetectionArea( + detection_area_polygons, possible_collisions, grid_map, interp_path, offset_from_start_to_ego, + param_, debug_data_.occlusion_points); + if (detection_area_polygons.empty()) return true; utils::filterCollisionByRoadType(possible_collisions, focus_area); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); @@ -102,6 +110,15 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); + // these debug topics needs computation resource + if (param_.debug) { + publisher_->publish(occ_grid); + for (const auto & p : detection_area_polygons) { + debug_data_.detection_areas.emplace_back(p.polygon); + } + } else { + debug_data_.occlusion_points.clear(); + } debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; debug_data_.path_raw = *path; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index ceff1798eb2bc..99aebf5ad60d8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -45,6 +46,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { debug_data_ = DebugData(); + debug_data_.road_type = "public"; if (path->points.size() < 2) { return true; } @@ -54,6 +56,9 @@ bool OcclusionSpotInPublicModule::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_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( + param_.v.v_ego, param_.v.a_ego, param_.v.non_effective_accel, param_.v.non_effective_jerk, + 0.0); } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; const auto & lanelet_map_ptr = planner_data_->lanelet_map; @@ -81,17 +86,26 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( std::vector obj = utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); + using Slice = occlusion_spot_utils::Slice; + std::vector detection_area_polygons; + utils::buildDetectionAreaPolygon( + detection_area_polygons, interp_path, offset_from_start_to_ego, param_); + const auto filtered_obj = utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); // Note: Don't consider offset from path start to ego here std::vector possible_collisions = utils::generatePossibleCollisionBehindParkedVehicle( - interp_path, param_, offset_from_start_to_ego, obj); + interp_path, param_, offset_from_start_to_ego, filtered_obj); utils::filterCollisionByRoadType(possible_collisions, focus_area); 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, 0.0); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); - + if (param_.debug) { + for (const auto & p : detection_area_polygons) { + debug_data_.detection_areas.emplace_back(p.polygon); + } + } debug_data_.possible_collisions = possible_collisions; return true; } diff --git a/planning/behavior_velocity_planner/test/src/test_geometry.cpp b/planning/behavior_velocity_planner/test/src/test_geometry.cpp index cc1e631e91c58..0d7abf4bf7271 100644 --- a/planning/behavior_velocity_planner/test/src/test_geometry.cpp +++ b/planning/behavior_velocity_planner/test/src/test_geometry.cpp @@ -19,75 +19,3 @@ #include #include - -TEST(buildSlices, one_full_slice) -{ - using behavior_velocity_planner::geometry::buildSlices; - using behavior_velocity_planner::geometry::Slice; - using behavior_velocity_planner::geometry::SliceRange; - - /* Simple vertical lanelet - 3x3 slice on the left - 0 1 2 3 4 5 6 - 0 | |------ - 1 | | | - 2 | |SLICE| - 3 | |-----| - 4 | | - 5 | | - */ - const int nb_points = 6; - lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, 5.0, nb_points); - SliceRange range; - range.min_distance = 0.0; - range.max_distance = -3.0; - const double slice_length = 3.0; - const double slice_width = 1.5; - std::vector slices; - buildSlices(slices, traj_lanelet, range, slice_length, slice_width, 0.5); - ASSERT_EQ(slices.size(), static_cast(4)); - EXPECT_EQ(slices[0].range.min_length, 0.0); - EXPECT_EQ(slices[0].range.min_distance, 0.0); - EXPECT_EQ(slices[0].range.max_length, 3.0); - EXPECT_EQ(slices[0].range.max_distance, -1.5); -} - -TEST(buildSlices, 3x3square_slice) -{ - using behavior_velocity_planner::geometry::buildSlices; - using behavior_velocity_planner::geometry::Slice; - using behavior_velocity_planner::geometry::SliceRange; - - /* Simple vertical lanelet - 3x3 slice on the left - 0 1 2 3 4 5 6 - 0 | |------ - 1 | | | - 2 | |SLICE| - 3 | |-----| - */ - const int nb_points = 4; - const double len = 3.0; - lanelet::ConstLanelet lanelet = test::createLanelet({0.0, 0.0}, {0.0, len}, nb_points); - SliceRange range; - range.min_distance = -1.0; - range.max_distance = -3.0; - const double slice_length = 1.0; - const double slice_width = 1.0; - const int num_right_slice = - (len / slice_length) * (std::abs(range.max_distance - range.min_distance) / slice_width); - // - { - std::vector slices; - buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0); - ASSERT_EQ(slices.size(), static_cast(num_right_slice)); - } - // change to the left side (positive distance) - range.min_distance = 1.0; - range.max_distance = 3.0; - { - std::vector slices; - buildSlices(slices, lanelet, range, slice_length, slice_width, 1.0); - ASSERT_EQ(slices.size(), static_cast(num_right_slice)); - } -} 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 3eac8b058463d..b45c7cce4c63d 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -37,217 +37,67 @@ struct indexEq using test::grid_param; -TEST(isOcclusionSpotSquare, one_cell_occlusion_spot) +TEST(isOcclusionSpotSquare, occlusion_spot_cell) { using behavior_velocity_planner::grid_utils::isOcclusionSpotSquare; using behavior_velocity_planner::grid_utils::OcclusionSpotSquare; using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; // Prepare an occupancy grid with a single occlusion_spot - /* - 0 1 2 3 - 0 ? ? ? - 1 ? ? - 2 ? ? ? - 3 ? ? - */ - grid_map::GridMap grid = test::generateGrid(4, 4, 1.0); - std::unordered_set unknown_cells = { - {0, 0}, {0, 3}, {1, 1}, {1, 2}, {2, 0}, {2, 1}, {2, 2}, {3, 0}, {3, 2}, {3, 3}}; - for (grid_map::Index index : unknown_cells) { - grid.at("layer", index) = UNKNOWN; - } + const int min_occlusion_spot_size = 2; - for (int i = 0; i < grid.getLength().x(); ++i) { - for (int j = 0; j < grid.getLength().y(); ++j) { - OcclusionSpotSquare occlusion_spot; - bool found = isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, 1, grid.getSize()); - if (unknown_cells.count({i, j})) { - ASSERT_TRUE(found); - } else { - ASSERT_FALSE(found); - } + // case simple + { + /* + 0 1 2 3 4 + 0 + 1 ? ? + 2 ? x ? + 3 ? ? + 4 + */ + grid_map::GridMap grid = test::generateGrid(5, 5, 1.0); + std::vector unknown_cells = {{1, 1}, {1, 2}, {1, 3}, {2, 2}, + {2, 3}, {3, 1}, {3, 2}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; } - } -} -TEST(isOcclusionSpotSquare, many_cells_occlusion_spot) -{ - using behavior_velocity_planner::grid_utils::isOcclusionSpotSquare; - using behavior_velocity_planner::grid_utils::OcclusionSpotSquare; - using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; - const double resolution = 1.0; - const double size = 2 * resolution; - /* - 0 1 2 3 - 0 ? ? ? - 1 ? ? - 2 ? ? ? - 3 ? ? - */ - grid_map::GridMap grid = test::generateGrid(4, 4, resolution); - std::unordered_set unknown_cells; - std::unordered_set occlusion_spot_cells; - unknown_cells = {{0, 0}, {0, 3}, {1, 1}, {1, 2}, {2, 0}, {2, 1}, {2, 2}, {3, 0}, {3, 2}, {3, 3}}; - occlusion_spot_cells = {{2, 0}, {3, 0}, {1, 2}, {3, 3}}; - for (grid_map::Index index : unknown_cells) { - grid.at("layer", index) = UNKNOWN; - } - for (int i = 0; i < grid.getLength().x(); ++i) { - for (int j = 0; j < grid.getLength().y(); ++j) { - OcclusionSpotSquare occlusion_spot; - bool found = - isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); - if (occlusion_spot_cells.count({i, j})) { - ASSERT_TRUE(found); - } else { - ASSERT_FALSE(found); + // occlusion spot (2,2) + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = isOcclusionSpotSquare( + occlusion_spot, grid["layer"], {i, j}, min_occlusion_spot_size, grid.getSize()); + if (i == 2 && j == 2) { + EXPECT_TRUE(found); + } else { + EXPECT_FALSE(found); + } } } } - /* - 0 1 2 3 - 0 ? - 1 ? ? - 2 ? ? ? - 3 - */ - grid = test::generateGrid(4, 4, 1.0); - unknown_cells = {{0, 2}, {1, 1}, {2, 0}, {2, 1}, {2, 2}, {3, 2}}; - for (grid_map::Index index : unknown_cells) { - grid.at("layer", index) = UNKNOWN; - } - - // No occlusion_spots - for (int i = 0; i < grid.getLength().x(); ++i) { - for (int j = 0; j < grid.getLength().y(); ++j) { - OcclusionSpotSquare occlusion_spot; - bool found = - isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); - ASSERT_FALSE(found); + // case noisy + { + std::cout << "TooNoisyCase->No OcclusionSpot 2by2" << std::endl + << " 0|1|2|3|4| " << std::endl + << " 0 |?| | | | " << std::endl + << " 1 |?| | | | " << std::endl + << " 2 |?|?|?| | " << std::endl; + grid_map::GridMap grid = test::generateGrid(5, 3, 1.0); + std::vector unknown_cells = {{1, 0}, {1, 1}, {1, 2}, {2, 2}, {3, 2}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; } - } - /* - 0 1 2 3 - 0 - 1 ? ? - 2 ? ? - 3 - */ - grid = test::generateGrid(4, 4, 1.0); - unknown_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}}; - for (grid_map::Index index : unknown_cells) { - grid.at("layer", index) = UNKNOWN; - } - - // Only one occlusion_spot square - for (int i = 0; i < grid.getLength().x(); ++i) { - for (int j = 0; j < grid.getLength().y(); ++j) { - OcclusionSpotSquare occlusion_spot; - bool found = - isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); - if (i == 1 && j == 2) { - ASSERT_TRUE(found); - } else { + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = isOcclusionSpotSquare( + occlusion_spot, grid["layer"], {i, j}, min_occlusion_spot_size, grid.getSize()); + if (found) { + std::cout << "i: " << i << " j: " << j << " change algorithm or update test" << std::endl; + } ASSERT_FALSE(found); } } } - - /* - 0 1 2 3 - 0 ? - 1 ? ? - 2 ? ? ? ? - 3 ? - */ - grid = test::generateGrid(4, 4, 1.0); - unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {1, 3}, {2, 1}, {2, 2}, {3, 2}}; - for (grid_map::Index index : unknown_cells) { - grid.at("layer", index) = UNKNOWN; - } - occlusion_spot_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}}; - - for (int i = 0; i < grid.getLength().x(); ++i) { - for (int j = 0; j < grid.getLength().y(); ++j) { - OcclusionSpotSquare occlusion_spot; - bool found = - isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); - if (i == 1 && j == 2) { - ASSERT_TRUE(found); - } else { - ASSERT_FALSE(found); - } - } - } - - std::cout << "TooNoisyCase->No OcclusionSpot 2by2" << std::endl - << " 0|1|2|3|4| " << std::endl - << " 0 |?| | | | " << std::endl - << " 1 |?|?|?| | " << std::endl - << " 2 ?|?| |?| | " << std::endl - << " 3 |?| | | | " << std::endl - << " 4 |?| | | | " << std::endl; - grid = test::generateGrid(5, 5, 1.0); - unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {1, 3}, {2, 1}, {3, 1}, {3, 2}}; - for (grid_map::Index index : unknown_cells) { - grid.at("layer", index) = UNKNOWN; - } - occlusion_spot_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}}; - - for (int i = 0; i < grid.getLength().x(); ++i) { - for (int j = 0; j < grid.getLength().y(); ++j) { - OcclusionSpotSquare occlusion_spot; - bool found = - isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); - if (found) { - std::cout << "i: " << i << " j: " << j << " change algorithm or update test" << std::endl; - } - ASSERT_FALSE(found); - } - } -} - -TEST(buildSlices, test_buffer_offset) -{ - using behavior_velocity_planner::geometry::buildSlices; - using behavior_velocity_planner::geometry::Slice; - using behavior_velocity_planner::geometry::SliceRange; - - // Curving lanelet - // lanelet::Lanelet l; - // lanelet::Points3d line; - boost::geometry::model::linestring line; - boost::geometry::model::multi_polygon> - poly; - line.emplace_back(0.0, 1.0); - line.emplace_back(1.0, 4.0); - line.emplace_back(2.0, 6.0); - line.emplace_back(3.0, 7.0); - line.emplace_back(4.0, 6.0); - line.emplace_back(5.0, 4.0); - line.emplace_back(6.0, 1.0); - - boost::geometry::strategy::buffer::distance_asymmetric distance_strategy(0.0, 1.1); - boost::geometry::strategy::buffer::end_flat end_strategy; - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::strategy::buffer::join_miter join_strategy; - boost::geometry::strategy::buffer::point_square point_strategy; - boost::geometry::buffer( - line, poly, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); - - std::cout << boost::geometry::wkt(line) << "\n"; - std::cout << boost::geometry::wkt(poly[0]) << "\n"; - - boost::geometry::model::multi_point in1; - for (const auto & p : line) { - in1.push_back(p); - } - boost::geometry::model::multi_point in2; - for (const auto & p : poly[0].outer()) { - in2.push_back(p); - } - boost::geometry::model::multi_point output; - boost::geometry::difference(in2, in1, output); - - std::cout << boost::geometry::wkt(output) << "\n"; } diff --git a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp index 644790d44e50c..ff47f2414428a 100644 --- a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp @@ -35,28 +35,76 @@ TEST(safeMotion, delay_jerk_acceleration) * case2 delay + jerk * case3 delay + jerk + acc */ - utils::Velocity v{1.0, -3.0, -4.5, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0}; + utils::Velocity v{}; + v.safety_ratio = 1.0; + v.max_stop_jerk = -3.0; + v.max_stop_accel = -4.5; + v.delay_time = 0.5; double ttc = 0.0; + const double eps = 1e-3; // case 1 delay { ttc = 0.5; utils::SafeMotion sm = utils::calculateSafeMotion(v, ttc); - EXPECT_DOUBLE_EQ(sm.safe_velocity, 0.0); - EXPECT_DOUBLE_EQ(sm.stop_dist, 0.0); + EXPECT_NEAR(sm.safe_velocity, 0.0, eps); + EXPECT_NEAR(sm.stop_dist, 0.0, eps); } // case 2 delay + jerk { ttc = 1.5; utils::SafeMotion sm = utils::calculateSafeMotion(v, ttc); - EXPECT_DOUBLE_EQ(sm.safe_velocity, 1.5); - EXPECT_DOUBLE_EQ(sm.stop_dist, 1.25); + EXPECT_NEAR(sm.safe_velocity, 1.5, eps); + EXPECT_NEAR(sm.stop_dist, 1.25, eps); } // case 3 delay + jerk + acc { ttc = 3.25; utils::SafeMotion sm = utils::calculateSafeMotion(v, ttc); - EXPECT_DOUBLE_EQ(sm.safe_velocity, 9); - EXPECT_DOUBLE_EQ(std::round(sm.stop_dist * 100.0) / 100.0, 13.92); + EXPECT_NEAR(sm.safe_velocity, 9, eps); + EXPECT_NEAR(std::round(sm.stop_dist * 100.0) / 100.0, 13.92, eps); + } +} + +TEST(detectionArea, calcLateralDistance) +{ + namespace utils = behavior_velocity_planner::occlusion_spot_utils; + using utils::calculateLateralDistanceFromTTC; + /** + * @brief check if calculation is correct in below parameter + * lateral distance is calculated from + * - ego velocity + * - min distance(safety margin) + * - max distance(ignore distance above this) + * - pedestrian velocity + * - min allowed velocity(not to stop) + */ + utils::PlannerParam p; + p.half_vehicle_width = 2.0; + p.baselink_to_front = 3.0; + p.pedestrian_vel = 1.5; + p.detection_area.max_lateral_distance = 5.0; + p.v.min_allowed_velocity = 1.5; + p.v.v_ego = 5.0; + const double offset_from_ego_to_start = 0.0; + { + for (size_t i = 0; i <= 15; i += 5) { + // arc length in path point + const double l = i * 1.0; + const double s = l - offset_from_ego_to_start; + const double d = utils::calculateLateralDistanceFromTTC(s, p); + const double eps = 1e-3; + std::cout << "s: " << l << " v: " << p.v.v_ego << " d: " << d << std::endl; + if (i == 0) + EXPECT_NEAR(d, 2.5, eps); + else if (i == 5) + EXPECT_NEAR(d, 3.5, eps); + else if (i == 10) + EXPECT_NEAR(d, 5.0, eps); + else if (i == 15) + EXPECT_NEAR(d, 5.0, eps); + else + break; + } } }