From 097de640ee074923166ec2c68fe4104da02a4e97 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Fri, 18 Mar 2022 18:19:12 +0900 Subject: [PATCH] refactor(behavior_velocity): occlusion spot merge scene modules (#528) * refactor(behavior_velocity): refactor slice to general type Signed-off-by: tanaka3 * chore(behavior_velocity): Basic Polygon to general polygon2d Signed-off-by: tanaka3 * chore(behavior_velocity): generalize da polygon usage Signed-off-by: tanaka3 * chore(behavior_velocity): remove replaced function Signed-off-by: tanaka3 * refactor(behavior_velocity): unite create detection area polygon function as util Signed-off-by: tanaka3 * refactor(behavior_velocity): refactor method Signed-off-by: tanaka3 * refactor(behavior_velocity): add pass judge param Signed-off-by: tanaka3 * refactor(behavior_velocity): move judgement Signed-off-by: tanaka3 * refactor(behavior_velocity): replace with tier4 autoware utils Signed-off-by: tanaka3 * chore(behavior_velocity): fix spell check Signed-off-by: tanaka3 * refactor(behavior_velocity): merge occlusion spot sepated module Signed-off-by: tanaka3 * chore(behavior_velocity): handle boundary condition for better Signed-off-by: tanaka3 Signed-off-by: Tomohito Ando --- .../behavior_velocity_planner/CMakeLists.txt | 3 - .../config/occlusion_spot.param.yaml | 3 +- .../scene_module/occlusion_spot/geometry.hpp | 52 ------ .../occlusion_spot/grid_utils.hpp | 3 +- .../scene_module/occlusion_spot/manager.hpp | 2 +- .../occlusion_spot/occlusion_spot_utils.hpp | 56 +++---- .../risk_predictive_braking.hpp | 9 - .../scene_occlusion_spot_in_public_road.hpp | 73 -------- .../include/utilization/util.hpp | 29 +++- .../src/scene_module/occlusion_spot/debug.cpp | 24 +-- .../scene_module/occlusion_spot/geometry.cpp | 133 --------------- .../occlusion_spot/grid_utils.cpp | 4 +- .../scene_module/occlusion_spot/manager.cpp | 57 ++++--- .../occlusion_spot/occlusion_spot_utils.cpp | 78 +++++---- .../risk_predictive_braking.cpp | 19 --- .../occlusion_spot/scene_occlusion_spot.cpp | 94 +++++++---- .../scene_occlusion_spot_in_public_road.cpp | 104 ------------ .../src/utilization/util.cpp | 156 ++++++++++++++++++ .../test/src/test_geometry.cpp | 21 --- .../test/src/test_risk_predictive_braking.cpp | 43 ----- 20 files changed, 360 insertions(+), 603 deletions(-) delete mode 100644 planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp delete mode 100644 planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp delete mode 100644 planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp delete mode 100644 planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp delete mode 100644 planning/behavior_velocity_planner/test/src/test_geometry.cpp diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 927793f713433..824f18706fcfe 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -220,10 +220,8 @@ target_link_libraries(scene_module_virtual_traffic_light scene_module_lib) # SceneModule OcclusionSpot ament_auto_add_library(scene_module_occlusion_spot SHARED src/scene_module/occlusion_spot/grid_utils.cpp - src/scene_module/occlusion_spot/geometry.cpp src/scene_module/occlusion_spot/manager.cpp src/scene_module/occlusion_spot/debug.cpp - src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.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 @@ -313,7 +311,6 @@ if(BUILD_TESTING) ament_add_gtest(occlusion_spot-test test/src/test_occlusion_spot_utils.cpp test/src/test_risk_predictive_braking.cpp - test/src/test_geometry.cpp test/src/test_grid_utils.cpp ) target_link_libraries(occlusion_spot-test diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 5784cce657e0d..71bcf2694fa0d 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,7 +1,8 @@ /**: ros__parameters: occlusion_spot: - method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" + 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 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 deleted file mode 100644 index e022df839bccb..0000000000000 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ - -#include - -#include - -#include -#include - -#include - -namespace behavior_velocity_planner -{ -namespace occlusion_spot_utils -{ -namespace bg = boost::geometry; - -//!< @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 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) -{ - return a + t * (b - a); -} - -} // 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 8d7edf060cf6f..b7f9df3371b66 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,6 +19,7 @@ #include #include #include +#include #include @@ -67,7 +68,7 @@ bool isOcclusionSpotSquare( //!< @brief Find all occlusion spots inside the given lanelet void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, - const lanelet::BasicPolygon2d & polygon, const double min_size); + 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); 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 6464226c788b0..831dca20f1655 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 @@ -18,7 +18,6 @@ #include #include #include -#include #include #include @@ -49,6 +48,7 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface using PlannerParam = occlusion_spot_utils::PlannerParam; PlannerParam planner_param_; + int64_t module_id_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; 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 65547fd0d763a..0146947be2039 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 @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include @@ -42,6 +42,16 @@ #include #include +namespace tier4_autoware_utils +{ +template <> +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose; +} +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -67,7 +77,8 @@ using BasicPolygons2d = std::vector; namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; -enum METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; +enum DETECTION_METHOD { OCCUPANCY_GRID, PREDICTED_OBJECT }; +enum PASS_JUDGE { SMOOTH_VELOCITY, CURRENT_VELOCITY }; struct DetectionArea { @@ -99,7 +110,8 @@ struct LatLon struct PlannerParam { - METHOD method; + DETECTION_METHOD detection_method; + PASS_JUDGE pass_judge; bool debug; // [-] bool use_partition_lanelet; // [-] // parameters in yaml @@ -127,22 +139,6 @@ 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 @@ -185,7 +181,7 @@ struct DebugData double z; std::string road_type = ""; std::string detection_type = ""; - std::vector detection_area_polygons; + Polygons2d detection_area_polygons; std::vector partition_lanelets; std::vector parked_vehicle_point; std::vector possible_collisions; @@ -200,22 +196,26 @@ struct DebugData occlusion_points.clear(); } }; - +// apply current velocity to path +PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0); +//!< @brief wrapper for detection area polygon generation +bool buildDetectionAreaPolygon( + Polygons2d & slices, const PathWithLaneId & path, const double offset, + const PlannerParam & param); lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here void handleCollisionOffset(std::vector & possible_collisions, double offset); void clipPathByLength( const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0); bool isStuckVehicle(PredictedObject obj, const double min_vel); -double offsetFromStartToEgo( - const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx); std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const std::vector & polys); + std::vector & objs, const Polygons2d & polys); std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); -std::vector generatePossibleCollisionBehindParkedVehicle( - const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, +bool generatePossibleCollisionBehindParkedVehicle( + std::vector & possible_collisions, const PathWithLaneId & path, + const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects); ROAD_TYPE getCurrentRoadType( const lanelet::ConstLanelet & current_lanelet, const LaneletMapPtr & lanelet_map_ptr); @@ -236,10 +236,10 @@ void calcSlowDownPointsForPossibleCollision( //!< @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 base_point, + const double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); //!< @brief generate possible collisions coming from occlusion spots on the side of the path -void createPossibleCollisionsInDetectionArea( +bool createPossibleCollisionsInDetectionArea( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data); 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 9156d5d04ce07..3be1bf9c5f0fd 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 @@ -34,15 +34,6 @@ int insertSafeVelocityToPath( const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, PathWithLaneId * inout_path); -/** - * - * @param: longitudinal_distance: longitudinal distance to collision - * @param: param: planner param - * @return lateral distance - **/ -double calculateLateralDistanceFromTTC( - const double longitudinal_distance, const PlannerParam & param); - /** * @param: v: ego velocity config * @param: ttc: time to collision 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 deleted file mode 100644 index b194a4fb83d91..0000000000000 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ -#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include - -namespace behavior_velocity_planner -{ -class OcclusionSpotInPublicModule : public SceneModuleInterface -{ - using PlannerParam = occlusion_spot_utils::PlannerParam; - using DebugData = occlusion_spot_utils::DebugData; - -public: - OcclusionSpotInPublicModule( - const int64_t module_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); - - /** - * @brief plan occlusion spot velocity - */ - bool modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - tier4_planning_msgs::msg::StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - -private: - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects_array_; - - // Parameter - PlannerParam param_; - -protected: - int64_t module_id_; - - // Debug - mutable DebugData debug_data_; -}; -} // namespace behavior_velocity_planner - -#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index a73171972918a..703ee7e7e5db9 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -63,18 +64,35 @@ struct SearchRangeIndex size_t min_idx; size_t max_idx; }; +struct DetectionRange +{ + bool use_right = true; + bool use_left = true; + double interval; + double min_longitudinal_distance; + double max_longitudinal_distance; + double min_lateral_distance; + double max_lateral_distance; +}; struct PointWithSearchRangeIndex { geometry_msgs::msg::Point point; SearchRangeIndex index; }; -using autoware_auto_planning_msgs::msg::PathWithLaneId; + using Point2d = boost::geometry::model::d2::point_xy; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using Polygons2d = std::vector; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using BasicPolygons2d = std::vector; +using Polygons2d = std::vector; + namespace planning_utils { +using geometry_msgs::msg::Pose; inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; } inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) { return p.position; } inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p) @@ -110,6 +128,15 @@ inline geometry_msgs::msg::Pose getPose( { return traj.points.at(idx).pose; } + +// create detection area from given range return false if creation failure +bool createDetectionAreaPolygons( + Polygons2d & slices, const PathWithLaneId & path, const DetectionRange da_range, + 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 getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys); void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input); void insertVelocity( PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, 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 cb435400f1feb..b0749792956fd 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 @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -29,7 +28,6 @@ namespace { using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; -using Slices = std::vector; visualization_msgs::msg::Marker makeArrowMarker( const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id) @@ -173,7 +171,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( } visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( - const Slices & slices, const std::string ns, const int id, const double z) + const Polygons2d & slices, const std::string ns, const int id, const double z) { visualization_msgs::msg::MarkerArray debug_markers; visualization_msgs::msg::Marker debug_marker; @@ -189,7 +187,7 @@ visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); debug_marker.ns = ns; for (const auto & slice : slices) { - for (const auto & p : slice.polygon) { + for (const auto & p : slice.outer()) { geometry_msgs::msg::Point point = tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); debug_marker.points.push_back(point); @@ -283,24 +281,6 @@ visualization_msgs::msg::MarkerArray createOcclusionMarkerArray( } } // namespace -visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMarkerArray() -{ - const auto current_time = this->clock_->now(); - - visualization_msgs::msg::MarkerArray debug_marker_array; - if (!debug_data_.possible_collisions.empty()) { - appendMarkerArray( - createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array); - } - if (!debug_data_.detection_area_polygons.empty()) { - appendMarkerArray( - makeSlicePolygonMarker( - debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), - current_time, &debug_marker_array); - } - - return debug_marker_array; -} visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray() { const auto current_time = this->clock_->now(); 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 deleted file mode 100644 index acb1827bc686b..0000000000000 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace behavior_velocity_planner -{ -namespace occlusion_spot_utils -{ -using lanelet::BasicLineString2d; -using lanelet::BasicPoint2d; -using lanelet::BasicPolygon2d; -namespace bg = boost::geometry; -namespace lg = lanelet::geometry; - -BasicPoint2d calculateLateralOffsetPoint( - const BasicPoint2d & p0, const BasicPoint2d & p1, const double offset) -{ - // 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 double offset, - const bool is_on_right, const PlannerParam & param) -{ - BasicLineString2d center_line = path_lanelet.centerline2d().basicLineString(); - const auto & p = param; - /** - * @brief relationships for interpolated polygon - * - * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons - * | | - * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons - */ - 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(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 = 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 buildDetectionAreaPolygon( - std::vector & slices, const PathWithLaneId & path, const double offset, - const PlannerParam & param) -{ - std::vector left_slices; - std::vector right_slices; - lanelet::ConstLanelet path_lanelet = toPathLanelet(path); - // in most case lateral distance is much more effective for velocity planning - 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 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 8e8b408082e78..8e91a5735b981 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 @@ -80,12 +80,12 @@ bool isOcclusionSpotSquare( void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, - const lanelet::BasicPolygon2d & polygon, double min_size) + const Polygon2d & polygon, double min_size) { const grid_map::Matrix & grid_data = grid["layer"]; const int min_occlusion_spot_size = std::max(0.0, std::floor(min_size / grid.getResolution())); grid_map::Polygon grid_polygon; - for (const auto & point : 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) { 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 44b30e7814e51..25ac665744861 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 @@ -14,7 +14,6 @@ #include #include -#include #include #include @@ -25,7 +24,8 @@ namespace behavior_velocity_planner { -using occlusion_spot_utils::METHOD; +using occlusion_spot_utils::DETECTION_METHOD; +using occlusion_spot_utils::PASS_JUDGE; OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -36,14 +36,31 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) // for crosswalk parameters auto & pp = planner_param_; - // assume pedestrian coming out from occlusion spot with this velocity - const std::string method = node.declare_parameter(ns + ".method", "occupancy_grid"); - if (method == "occupancy_grid") { - pp.method = METHOD::OCCUPANCY_GRID; - } else if (method == "predicted_object") { - pp.method = METHOD::PREDICTED_OBJECT; - } else { - throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"}; + // for detection type + { + const std::string method = node.declare_parameter(ns + ".detection_method", "occupancy_grid"); + if (method == "occupancy_grid") { // module id 0 + pp.detection_method = DETECTION_METHOD::OCCUPANCY_GRID; + module_id_ = DETECTION_METHOD::OCCUPANCY_GRID; + } else if (method == "predicted_object") { // module id 1 + pp.detection_method = DETECTION_METHOD::PREDICTED_OBJECT; + module_id_ = DETECTION_METHOD::PREDICTED_OBJECT; + } else { + throw std::invalid_argument{ + "[behavior_velocity]: occlusion spot detection method has invalid argument"}; + } + } + // for passable judgement + { + const std::string pass_judge = node.declare_parameter(ns + ".pass_judge", "current_velocity"); + if (pass_judge == "current_velocity") { + pp.pass_judge = PASS_JUDGE::CURRENT_VELOCITY; + } else if (pass_judge == "smooth_velocity") { + pp.pass_judge = PASS_JUDGE::SMOOTH_VELOCITY; + } else { + throw std::invalid_argument{ + "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; + } } pp.debug = node.declare_parameter(ns + ".debug", false); pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false); @@ -84,23 +101,11 @@ void OcclusionSpotModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; - const int64_t module_id = static_cast(ModuleID::OCCUPANCY); - const int64_t public_road_module_id = static_cast(ModuleID::OBJECT); // general - if (!isModuleRegistered(module_id)) { - if (planner_param_.method == METHOD::OCCUPANCY_GRID) { - registerModule(std::make_shared( - module_id, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_, pub_debug_occupancy_grid_)); - } - } - // public - if (!isModuleRegistered(public_road_module_id)) { - if (planner_param_.method == METHOD::PREDICTED_OBJECT) { - registerModule(std::make_shared( - public_road_module_id, planner_data_, planner_param_, - logger_.get_child("occlusion_spot_in_public_module"), clock_)); - } + 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_)); } } 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 c83751876fcf8..4c6a88cef0d6b 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,7 +14,6 @@ #include #include -#include #include #include #include @@ -50,6 +49,32 @@ lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path) return lanelet::ConstLanelet(path_lanelet); } +PathWithLaneId applyVelocityToPath(const PathWithLaneId & path, const double v0) +{ + PathWithLaneId out; + for (size_t i = 0; i < path.points.size(); i++) { + PathPointWithLaneId p = path.points.at(i); + p.point.longitudinal_velocity_mps = std::max(v0, 1.0); + out.points.emplace_back(p); + } + return out; +} + +bool buildDetectionAreaPolygon( + Polygons2d & slices, const PathWithLaneId & path, const double offset, const PlannerParam & param) +{ + 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.max_longitudinal_distance = + std::min(p.detection_area_max_length, p.detection_area_length) + + da_range.min_longitudinal_distance; + da_range.min_lateral_distance = p.half_vehicle_width; + da_range.max_lateral_distance = p.detection_area.max_lateral_distance; + return planning_utils::createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel); +} + ROAD_TYPE getCurrentRoadType( const lanelet::ConstLanelet & current_lanelet, [[maybe_unused]] const lanelet::LaneletMapPtr & lanelet_map_ptr) @@ -177,21 +202,6 @@ bool isStuckVehicle(PredictedObject obj, const double min_vel) return false; } -double offsetFromStartToEgo( - const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx) -{ - double offset_from_ego_to_closest = 0; - for (int i = 0; i < closest_idx; i++) { - const auto & curr_p = path.points.at(i).point.pose.position; - const auto & next_p = path.points.at(i + 1).point.pose.position; - offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p); - } - const double offset_from_closest_to_target = - -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose) - .position.x; - return offset_from_ego_to_closest + offset_from_closest_to_target; -} - std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point) @@ -302,12 +312,12 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( return pc; } -std::vector generatePossibleCollisionBehindParkedVehicle( - const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, +bool generatePossibleCollisionBehindParkedVehicle( + std::vector & possible_collisions, const PathWithLaneId & path, + const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects) { lanelet::ConstLanelet path_lanelet = toPathLanelet(path); - std::vector possible_collisions; auto ll = path_lanelet.centerline2d(); for (const auto & dyn : dyn_objects) { ArcCoordinates arc_coord_occlusion = getOcclusionPoint(dyn, ll); @@ -333,11 +343,12 @@ std::vector generatePossibleCollisionBehindParkedVehicle( [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; }); - return possible_collisions; + if (possible_collisions.empty()) return false; + return true; } std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const std::vector & polys) + std::vector & objs, const Polygons2d & polys) { std::vector filtered_obj; // stuck points by predicted objects @@ -345,7 +356,7 @@ std::vector filterDynamicObjectByDetectionArea( // 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)) { + if (!bg::disjoint(obj_footprint, p)) { filtered_obj.emplace_back(object); } } @@ -353,20 +364,21 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -void createPossibleCollisionsInDetectionArea( +bool createPossibleCollisionsInDetectionArea( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data) { lanelet::ConstLanelet path_lanelet = toPathLanelet(path); if (path_lanelet.centerline2d().empty()) { - return; + return true; } double distance_lower_bound = std::numeric_limits::max(); - for (const Slice & detection_area_slice : debug_data.detection_area_polygons) { + const Polygons2d & da_polygons = debug_data.detection_area_polygons; + for (const Polygon2d & detection_area_slice : da_polygons) { std::vector occlusion_spot_positions; grid_utils::findOcclusionSpots( - occlusion_spot_positions, grid, detection_area_slice.polygon, + occlusion_spot_positions, grid, detection_area_slice, param.detection_area.min_occlusion_spot_size); if (param.debug) { for (const auto & op : occlusion_spot_positions) { @@ -377,7 +389,7 @@ void createPossibleCollisionsInDetectionArea( } 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 Point2d base_point = detection_area_slice.outer().at(0); const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); @@ -387,6 +399,14 @@ void createPossibleCollisionsInDetectionArea( distance_lower_bound = lateral_distance; possible_collisions.emplace_back(pc.get()); } + // sort by arc length + std::sort( + possible_collisions.begin(), possible_collisions.end(), + [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { + return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; + }); + if (possible_collisions.empty()) return false; + return true; } bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) @@ -399,7 +419,7 @@ bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons 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 double offset_from_start_to_ego, const Point2d base_point, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data) { const double baselink_to_front = param.baselink_to_front; @@ -413,7 +433,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS const lanelet::BasicPoint2d obstacle_point = { occlusion_spot_position[0], occlusion_spot_position[1]}; const double dist = - std::hypot(base_point[0] - obstacle_point[0], base_point[1] - obstacle_point[1]); + std::hypot(base_point.x() - obstacle_point[0], base_point.y() - obstacle_point[1]); // skip if absolute distance is larger if (distance_lower_bound < dist) continue; lanelet::ArcCoordinates arc_coord_occlusion_point = 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 94150d0a43fcd..38250d6d78255 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 @@ -98,25 +98,6 @@ int insertSafeVelocityToPath( return 0; } -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)); -} - SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) { SafeMotion sm; 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 726c5e7bec799..9cd7f1851247f 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 @@ -14,7 +14,6 @@ #include #include -#include #include #include #include @@ -22,6 +21,8 @@ #include #include +#include + #include #include @@ -29,8 +30,6 @@ namespace behavior_velocity_planner { -namespace bg = boost::geometry; -namespace lg = lanelet::geometry; namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( @@ -38,10 +37,15 @@ OcclusionSpotModule::OcclusionSpotModule( 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) +: SceneModuleInterface(module_id, logger, clock), publisher_(publisher), param_(planner_param) { - param_ = planner_param; - debug_data_.detection_type = "occupancy"; + if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { + debug_data_.detection_type = "occupancy"; + //! occupancy grid limitation( 100 * 100 ) + param_.detection_area_length = std::min(50.0, param_.detection_area_length); + } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { + debug_data_.detection_type = "object"; + } if (param_.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); @@ -62,41 +66,63 @@ 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_.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 double detection_area_offset = 5.0; // for visualization and stability + 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, + planner_data_->delay_response_time) + + detection_area_offset; } const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; - const auto & occ_grid_ptr = planner_data_->occupancy_grid; - if (!occ_grid_ptr) { - return true; - } - - const double max_range = 50.0; PathWithLaneId clipped_path; - utils::clipPathByLength(*path, clipped_path, max_range); + utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); PathWithLaneId interp_path; //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, &interp_path, logger_); - debug_data_.interp_path = interp_path; - int closest_idx = -1; - if (!planning_utils::calcClosestIndex( - interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { - return true; + if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { + interp_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); + } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { } - // return if ego is final point of interpolated path - if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; - grid_map::GridMap grid_map; - grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); - double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); + debug_data_.interp_path = interp_path; + const geometry_msgs::msg::Point start_point = interp_path.points.at(0).point.pose.position; + const auto offset = tier4_autoware_utils::calcSignedArcLength( + interp_path.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); + if (offset == boost::none) return true; + const double offset_from_start_to_ego = -offset.get(); auto & detection_area_polygons = debug_data_.detection_area_polygons; - utils::buildDetectionAreaPolygon( - detection_area_polygons, interp_path, offset_from_start_to_ego, param_); + if (!utils::buildDetectionAreaPolygon( + detection_area_polygons, interp_path, offset_from_start_to_ego, param_)) { + return true; // path point is not enough + } std::vector possible_collisions; - // Note: Don't consider offset from path start to ego here - utils::createPossibleCollisionsInDetectionArea( - possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_); + 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); // + // 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_, + debug_data_)) { + // no occlusion spot + return true; + } + } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { + const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; + if (!dynamic_obj_arr_ptr) return true; // mo data + std::vector obj = + utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); + const auto filtered_obj = + utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); + // Note: Don't consider offset from path start to ego here + if (!utils::generatePossibleCollisionBehindParkedVehicle( + possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) { + // no occlusion spot + return true; + } + } RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); @@ -105,9 +131,7 @@ bool OcclusionSpotModule::modifyPathVelocity( // 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); - } + 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 deleted file mode 100644 index 7040c7c221af1..0000000000000 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -namespace behavior_velocity_planner -{ -using occlusion_spot_utils::PossibleCollisionInfo; -namespace utils = occlusion_spot_utils; - -OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( - 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) -: SceneModuleInterface(module_id, logger, clock) -{ - param_ = planner_param; - debug_data_.detection_type = "object"; - if (param_.use_partition_lanelet) { - const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); - planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); - } -} - -bool OcclusionSpotInPublicModule::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) -{ - debug_data_.resetData(); - if (path->points.size() < 2) { - return true; - } - // set planner data - { - param_.v.max_stop_jerk = planner_data_->max_stop_jerk_threshold; - 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 & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - - if (!dynamic_obj_arr_ptr) { - return true; - } - PathWithLaneId clipped_path; - utils::clipPathByLength(*path, clipped_path, param_.detection_area_length); - PathWithLaneId interp_path; - splineInterpolate(clipped_path, 1.0, &interp_path, logger_); - int closest_idx = -1; - if (!planning_utils::calcClosestIndex( - interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { - return true; - } - // return if ego is final point of interpolated path - if (closest_idx == static_cast(interp_path.points.size()) - 1) return true; - 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); - auto & detection_area_polygons = debug_data_.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, filtered_obj); - 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); - // apply safe velocity using ebs and pbs deceleration - utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); - debug_data_.possible_collisions = possible_collisions; - return true; -} - -} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 97950997510e0..882c9fce166d8 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -23,6 +23,162 @@ namespace behavior_velocity_planner { namespace planning_utils { +Point2d calculateLateralOffsetPoint2d(const Pose & pose, const double offset) +{ + using tier4_autoware_utils::calcOffsetPose; + return to_bg2d(calcOffsetPose(pose, 0.0, offset, 0.0)); +} + +PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) +{ + auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; + PathPoint p; + Pose pose; + const auto pp0 = p0.pose.position; + const auto pp1 = p1.pose.position; + pose.position.x = lerp(pp0.x, pp1.x, ratio); + pose.position.y = lerp(pp0.y, pp1.y, ratio); + pose.position.z = lerp(pp0.z, pp1.z, ratio); + const double yaw = tier4_autoware_utils::calcAzimuthAngle(pp0, pp1); + pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + p.pose = pose; + const double v = lerp(p0.longitudinal_velocity_mps, p1.longitudinal_velocity_mps, ratio); + p.longitudinal_velocity_mps = v; + return p; +} + +bool createDetectionAreaPolygons( + Polygons2d & da_polys, const PathWithLaneId & path, const DetectionRange da_range, + const double obstacle_vel_mps) +{ + /** + * @brief relationships for interpolated polygon + * + * +(min_length,max_distance)-+ - +---+(max_length,max_distance) = outer_polygons + * | | + * +--------------------------+ - +---+(max_length,min_distance) = inner_polygons + */ + const double min_len = da_range.min_longitudinal_distance; + const double max_len = da_range.max_longitudinal_distance; + const double min_dst = da_range.min_lateral_distance; + const double max_dst = da_range.max_lateral_distance; + const double interval = da_range.interval; + const double min_velocity = 0.5; // min velocity that autoware can cruise stably + //! max index is the last index of path point + const size_t max_index = static_cast(path.points.size() - 1); + double dist_sum = 0; + size_t first_idx = 0; // first path point found in front of ego front bumper + offset + const auto & pp = path.points; + //! avoid bug with same point polygon + const double eps = 1e-3; + if (path.points.size() < 2) return false; // case of path point is only one + auto p0 = path.points.front().point; + // handle the first point + { + double current_dist = 0.0; + for (size_t i = 1; i <= max_index - 1; i++) { + dist_sum += tier4_autoware_utils::calcDistance2d(pp.at(i - 1), pp.at(i)); + if (dist_sum > min_len) { + first_idx = i; + break; + } + current_dist = dist_sum; + } + if (first_idx == 0) return false; // case of all path point is behind ego front bumper + offset + const double ds = dist_sum - current_dist; + if (std::abs(ds) < eps) { + p0 = pp.at(first_idx - 1).point; + } else { + const double ratio = (min_len - current_dist) / ds; + p0 = getLerpPathPointWithLaneId(pp.at(first_idx - 1).point, pp.at(first_idx).point, ratio); + } + } + double ttc = 0.0; + dist_sum = min_len; + double length = 0; + // initial point of detection area polygon + LineString2d left_inner_bound = {calculateLateralOffsetPoint2d(p0.pose, min_dst)}; + LineString2d left_outer_bound = {calculateLateralOffsetPoint2d(p0.pose, min_dst + eps)}; + LineString2d right_inner_bound = {calculateLateralOffsetPoint2d(p0.pose, -min_dst)}; + LineString2d right_outer_bound = {calculateLateralOffsetPoint2d(p0.pose, -min_dst - eps)}; + for (size_t s = first_idx; s <= max_index; s++) { + const auto p1 = path.points.at(s).point; + const double ds = tier4_autoware_utils::calcDistance2d(p0, p1); + dist_sum += ds; + length += ds; + // calculate the distance that obstacles can move until ego reach the trajectory point + const double v_average = 0.5 * (p0.longitudinal_velocity_mps + p1.longitudinal_velocity_mps); + const double v = std::max(v_average, min_velocity); + const double dt = ds / v; + ttc += dt; + // for offset calculation + const double max_lateral_distance = std::min(max_dst, min_dst + ttc * obstacle_vel_mps + eps); + // left bound + if (da_range.use_left) { + left_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, min_dst)); + left_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, max_lateral_distance)); + } + // right bound + if (da_range.use_right) { + right_inner_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, -min_dst)); + right_outer_bound.emplace_back(calculateLateralOffsetPoint2d(p1.pose, -max_lateral_distance)); + } + // replace previous point with next point + p0 = p1; + // separate detection area polygon with fixed interval or at the end of detection max length + if (length > interval || max_len < dist_sum || s == max_index) { + if (left_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(left_inner_bound, left_outer_bound)); + if (right_inner_bound.size() > 1) + da_polys.emplace_back(lines2polygon(right_outer_bound, right_inner_bound)); + left_inner_bound = {left_inner_bound.back()}; + left_outer_bound = {left_outer_bound.back()}; + right_inner_bound = {right_inner_bound.back()}; + right_outer_bound = {right_outer_bound.back()}; + length = 0; + if (max_len < dist_sum || s == max_index) return true; + } + } + return true; +} + +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys) +{ + const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); + for (const auto & partition : partitions) { + lanelet::BasicLineString2d line; + for (const auto & p : partition) { + line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()}); + } + polys.emplace_back(lanelet::BasicPolygon2d(line)); + } +} + +SearchRangeIndex getPathIndexRangeIncludeLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id) +{ + /** + * @brief find path index range include given lane_id + * |<-min_idx |<-max_idx + * ------|oooooooooooooooo|------- + */ + SearchRangeIndex search_range = {0, path.points.size() - 1}; + bool found_first_idx = false; + for (size_t i = 0; i < path.points.size(); i++) { + const auto & p = path.points.at(i); + for (const auto & id : p.lane_ids) { + if (id == lane_id) { + if (!found_first_idx) { + search_range.min_idx = i; + found_first_idx = true; + } + search_range.max_idx = i; + } + } + } + return search_range; +} + void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input) { for (size_t i = begin_idx; i < input->points.size(); ++i) { diff --git a/planning/behavior_velocity_planner/test/src/test_geometry.cpp b/planning/behavior_velocity_planner/test/src/test_geometry.cpp deleted file mode 100644 index 0d7abf4bf7271..0000000000000 --- a/planning/behavior_velocity_planner/test/src/test_geometry.cpp +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "utils.hpp" - -#include - -#include - -#include 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 ff47f2414428a..7238dd971b02e 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 @@ -65,49 +65,6 @@ TEST(safeMotion, delay_jerk_acceleration) } } -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; - } - } -} - TEST(calculateInsertVelocity, min_max) { using behavior_velocity_planner::occlusion_spot_utils::calculateInsertVelocity;