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 07cc14840b6f7..538d04cdadde6 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 @@ -117,7 +117,6 @@ void denoiseOccupancyGridCV( const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, const int num_iter, const bool use_object_footprints, const bool use_object_ray_casts); -void addObjectsToGridMap(const std::vector & objs, grid_map::GridMap & grid); } // namespace grid_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 6a9adcf45d095..5f9ac6fcaaa86 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 @@ -226,8 +226,6 @@ bool generatePossibleCollisionsFromObjects( 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); //!< @brief calculate intersection and collision point from occlusion spot void calculateCollisionPathPointFromOcclusionSpot( PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point, 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 aceef5069ee14..c201cc4158907 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 @@ -30,10 +30,6 @@ void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, const PlannerParam & param); -int insertSafeVelocityToPath( - const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, - PathWithLaneId * inout_path); - /** * @param: v: ego velocity config * @param: ttv: time to vehicle 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 41df01b5e7b8c..fa02d33fa0198 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 @@ -56,30 +56,6 @@ std::vector> pointsToRays( return lines; } -void addObjectsToGridMap(const std::vector & objs, grid_map::GridMap & grid) -{ - auto & grid_data = grid["layer"]; - for (const auto & obj : objs) { - Polygon2d foot_print_polygon = planning_utils::toFootprintPolygon(obj); - grid_map::Polygon grid_polygon; - const auto & pos = obj.kinematics.initial_pose_with_covariance.pose.position; - if (grid.isInside(grid_map::Position(pos.x, pos.y))) continue; - try { - for (const auto & point : foot_print_polygon.outer()) { - grid_polygon.addVertex({point.x(), point.y()}); - } - for (grid_map_utils::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); - ++iterator) { - const grid_map::Index & index = *iterator; - if (!grid.isValid(index)) continue; - grid_data(index.x(), index.y()) = grid_utils::occlusion_cost_value::OCCUPIED; - } - } catch (const std::invalid_argument & e) { - std::cerr << e.what() << std::endl; - } - } -} - void findOcclusionSpots( std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, const Polygon2d & polygon, [[maybe_unused]] double min_size) 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 7f3cef1df88d7..75404879f331d 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 @@ -79,39 +79,6 @@ bool buildDetectionAreaPolygon( slices, path, pose, da_range, p.pedestrian_vel); } -ROAD_TYPE getCurrentRoadType( - const lanelet::ConstLanelet & current_lanelet, - [[maybe_unused]] const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")}; - rclcpp::Clock clock{RCL_ROS_TIME}; - occlusion_spot_utils::ROAD_TYPE road_type; - std::string location; - if ( - current_lanelet.hasAttribute(lanelet::AttributeNamesString::Subtype) && - current_lanelet.attribute(lanelet::AttributeNamesString::Subtype) == - lanelet::AttributeValueString::Highway) { - location = "highway"; - } else { - location = current_lanelet.attributeOr("location", "else"); - } - RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "location: " << location); - if (location == "urban" || location == "public") { - road_type = occlusion_spot_utils::ROAD_TYPE::PUBLIC; - RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "public road: " << location); - } else if (location == "private") { - road_type = occlusion_spot_utils::ROAD_TYPE::PRIVATE; - RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "private road"); - } else if (location == "highway") { - road_type = occlusion_spot_utils::ROAD_TYPE::HIGHWAY; - RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "highway road"); - } else { - road_type = occlusion_spot_utils::ROAD_TYPE::UNKNOWN; - RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "unknown road"); - } - return road_type; -} - void calcSlowDownPointsForPossibleCollision( const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double offset, std::vector & possible_collisions) 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 e5cd038bb7dd4..3e7ff2c9e281a 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 @@ -64,34 +64,6 @@ void applySafeVelocityConsideringPossibleCollision( } } -int insertSafeVelocityToPath( - const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, - PathWithLaneId * inout_path) -{ - const auto closest_idx_opt = - motion_utils::findNearestIndex(inout_path->points, in_pose, param.dist_thr, param.angle_thr); - if (!closest_idx_opt) { - return -1; - } - const size_t closest_idx = closest_idx_opt.get(); - - PathPointWithLaneId inserted_point; - inserted_point = inout_path->points.at(closest_idx); - size_t insert_idx = closest_idx; - if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { - ++insert_idx; - if (insert_idx == static_cast(inout_path->points.size())) return -1; - } - // return if index is after the last path point - inserted_point.point.pose = in_pose; - // insert velocity to path if distance is not too close else insert new collision point - // if original path has narrow points it's better to set higher distance threshold - // TODO(tanaka): use Spherical Linear Interpolation for inserting point - const double eps = 0.05; - planning_utils::insertVelocity(*inout_path, inserted_point, safe_vel, insert_idx, eps); - return 0; -} - SafeMotion calculateSafeMotion(const Velocity & v, const double ttv) { SafeMotion sm; diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 1060367c6cbf4..0256c4ba09d49 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -227,28 +227,6 @@ void insertVelocity( setVelocityFromIndex(insert_index, v, &path); } -boost::optional insertDecelPoint( - const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, - const float target_velocity) -{ - // TODO(tanaka): consider proper overlap threshold for inserting decel point - const double overlap_threshold = 5e-2; - const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); - const auto insert_idx = - motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold); - - if (!insert_idx) { - return {}; - } - - for (size_t i = insert_idx.get(); i < output.points.size(); ++i) { - const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps; - output.points.at(i).point.longitudinal_velocity_mps = - std::min(original_velocity, target_velocity); - } - return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); -} - Polygon2d toFootprintPolygon(const PredictedObject & object) { Polygon2d obj_footprint; @@ -646,6 +624,28 @@ bool isOverLine( 0.0; } +boost::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity) +{ + // TODO(tanaka): consider proper overlap threshold for inserting decel point + const double overlap_threshold = 5e-2; + const size_t base_idx = motion_utils::findNearestSegmentIndex(output.points, stop_point); + const auto insert_idx = + motion_utils::insertTargetPoint(base_idx, stop_point, output.points, overlap_threshold); + + if (!insert_idx) { + return {}; + } + + for (size_t i = insert_idx.get(); i < output.points.size(); ++i) { + const auto & original_velocity = output.points.at(i).point.longitudinal_velocity_mps; + output.points.at(i).point.longitudinal_velocity_mps = + std::min(original_velocity, target_velocity); + } + return tier4_autoware_utils::getPose(output.points.at(insert_idx.get())); +} + boost::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output) { 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 8501af4a4b7dc..6f3a3e4100123 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 @@ -64,58 +64,3 @@ TEST(safeMotion, delay_jerk_acceleration) EXPECT_NEAR(std::round(sm.stop_dist * 100.0) / 100.0, 13.92, eps); } } - -TEST(insertSafeVelocityToPath, replace_original_at_too_close_case) -{ - /* straight path - 0 1 2 3 4 - 0 x x z x x - ---> - straight path - 0 1 2 3 4 - 0 x x r xox - */ - using behavior_velocity_planner::occlusion_spot_utils::insertSafeVelocityToPath; - const int num_path_point = 5; - const double inf = std::numeric_limits::max(); - behavior_velocity_planner::occlusion_spot_utils::PlannerParam param; - param.angle_thr = 1.0; - param.dist_thr = 10.0; - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(0.0, 0.0, static_cast(num_path_point - 1), 0.0, num_path_point); - geometry_msgs::msg::Pose pose{}; - pose.position.x = 2; - double safe_vel = inf; - // insert x=2 new point -> replace original - insertSafeVelocityToPath(pose, safe_vel, param, &path); - ASSERT_EQ(path.points.size(), static_cast(num_path_point)); - pose.position.x = 3.4; - insertSafeVelocityToPath(pose, safe_vel, param, &path); - ASSERT_EQ(path.points.size(), static_cast(num_path_point + 1)); -} - -TEST(insertSafeVelocityToPath, dont_insert_last_point) -{ - /* straight path - 0 1 2 3 4 - 0 x x z x xo - ---> - straight path - 0 1 2 3 4 - 0 x x x x x - */ - using behavior_velocity_planner::occlusion_spot_utils::insertSafeVelocityToPath; - const int num_path = 5; - geometry_msgs::msg::Pose pose{}; - pose.position.x = 5; - pose.position.y = 0; - pose.position.z = 0; - double safe_vel = 10; - behavior_velocity_planner::occlusion_spot_utils::PlannerParam param; - param.angle_thr = 1.0; - param.dist_thr = 10.0; - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(0.0, 0.0, static_cast(num_path - 1), 0.0, num_path); - ASSERT_EQ(insertSafeVelocityToPath(pose, safe_vel, param, &path), -1); - ASSERT_EQ(path.points.size(), static_cast(num_path)); -}