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 3a3f13d709338..3064cab1d23f7 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 @@ -179,60 +179,14 @@ struct PossibleCollisionInfo lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here -inline void handleCollisionOffset( - std::vector & possible_collisions, double offset, double margin) -{ - for (auto & pc : possible_collisions) { - pc.arc_lane_dist_at_collision.length -= offset; - pc.arc_lane_dist_at_collision.length -= margin; - } -} - -inline 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; -} - -inline void clipPathByLength( - const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0) -{ - double length_sum = 0; - for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { - length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (length_sum > max_length) return; - clipped.points.emplace_back(path.points.at(i)); - } -} - -inline bool isStuckVehicle(PredictedObject obj, const double min_vel) -{ - if ( - obj.classification.at(0).label == ObjectClassification::CAR || - obj.classification.at(0).label == ObjectClassification::TRUCK || - obj.classification.at(0).label == ObjectClassification::BUS) { - if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { - return true; - } - } - return false; -} -void filterCollisionByRoadType( - std::vector & possible_collisions, const DetectionAreaIdx road_type); +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); -bool splineInterpolate( - const PathWithLaneId & input, const double interval, PathWithLaneId * output, - const rclcpp::Logger logger); std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_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 59145de966dfe..9156d5d04ce07 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,87 +34,25 @@ int insertSafeVelocityToPath( const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, PathWithLaneId * inout_path); -// @brief calculates the maximum velocity allowing to decelerate within the given distance -inline double calculateMinSlowDownVelocity( - const double v0, const double len, const double a_max, const double safe_vel) -{ - // if target velocity is inserted backward return current velocity as limit - if (len < 0) return safe_vel; - return std::sqrt(std::max(std::pow(v0, 2.0) - 2.0 * std::abs(a_max) * len, 0.0)); -} - /** * * @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)); -} +double calculateLateralDistanceFromTTC( + const double longitudinal_distance, const PlannerParam & param); /** * @param: v: ego velocity config * @param: ttc: time to collision * @return safe motion **/ -inline SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) -{ - SafeMotion sm; - const double j_max = v.safety_ratio * v.max_stop_jerk; - const double a_max = v.safety_ratio * v.max_stop_accel; - const double t1 = v.delay_time; - double t2 = a_max / j_max; - double & v_safe = sm.safe_velocity; - double & stop_dist = sm.stop_dist; - if (ttc <= t1) { - // delay - v_safe = 0; - stop_dist = 0; - } else if (ttc <= t2 + t1) { - // delay + const jerk - t2 = ttc - t1; - v_safe = -0.5 * j_max * t2 * t2; - stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6; - } else { - const double t3 = ttc - t2 - t1; - // delay + const jerk + const accel - const double v2 = -0.5 * j_max * t2 * t2; - v_safe = v2 - a_max * t3; - stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3; - } - stop_dist += v.safe_margin; - return sm; -} +SafeMotion calculateSafeMotion(const Velocity & v, const double ttc); -inline double calculateInsertVelocity( +double calculateInsertVelocity( const double min_allowed_vel, const double safe_vel, const double min_vel, - const double original_vel) -{ - const double max_vel_noise = 0.05; - // ensure safe velocity doesn't exceed maximum allowed pbs deceleration - double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel); - // ensure safe path velocity is also above ego min velocity - cmp_safe_vel = std::max(cmp_safe_vel, min_vel); - // ensure we only lower the original velocity (and do not increase it) - cmp_safe_vel = std::min(cmp_safe_vel, original_vel); - return cmp_safe_vel; -} + const double original_vel); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner 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 002306a91e7f6..8ba933fd2db40 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 @@ -146,6 +146,52 @@ void calcSlowDownPointsForPossibleCollision( } } +void handleCollisionOffset(std::vector & possible_collisions, double offset) +{ + for (auto & pc : possible_collisions) { + pc.arc_lane_dist_at_collision.length -= offset; + } +} + +void clipPathByLength( + const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length) +{ + double length_sum = 0; + for (int i = 0; i < static_cast(path.points.size()) - 1; i++) { + length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (length_sum > max_length) return; + clipped.points.emplace_back(path.points.at(i)); + } +} + +bool isStuckVehicle(PredictedObject obj, const double min_vel) +{ + if ( + obj.classification.at(0).label == ObjectClassification::CAR || + obj.classification.at(0).label == ObjectClassification::TRUCK || + obj.classification.at(0).label == ObjectClassification::BUS) { + if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { + return true; + } + } + 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) @@ -337,21 +383,6 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -void filterCollisionByRoadType( - std::vector & possible_collisions, const DetectionAreaIdx area) -{ - std::pair focus_length = area.get(); - for (auto it = possible_collisions.begin(); it != possible_collisions.end();) { - const auto & pc_len = it->arc_lane_dist_at_collision.length; - if (focus_length.first < pc_len && pc_len < focus_length.second) { - it++; - } else { - // -----erase-----|start------target-------end|----erase--- - it = possible_collisions.erase(it); - } - } -} - void createPossibleCollisionsInDetectionArea( const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, 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 6c5b9f10d218b..cb7e56245a4c4 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 @@ -105,5 +105,66 @@ 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; + const double j_max = v.safety_ratio * v.max_stop_jerk; + const double a_max = v.safety_ratio * v.max_stop_accel; + const double t1 = v.delay_time; + double t2 = a_max / j_max; + double & v_safe = sm.safe_velocity; + double & stop_dist = sm.stop_dist; + if (ttc <= t1) { + // delay + v_safe = 0; + stop_dist = 0; + } else if (ttc <= t2 + t1) { + // delay + const jerk + t2 = ttc - t1; + v_safe = -0.5 * j_max * t2 * t2; + stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6; + } else { + const double t3 = ttc - t2 - t1; + // delay + const jerk + const accel + const double v2 = -0.5 * j_max * t2 * t2; + v_safe = v2 - a_max * t3; + stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3; + } + stop_dist += v.safe_margin; + return sm; +} + +double calculateInsertVelocity( + const double min_allowed_vel, const double safe_vel, const double min_vel, + const double original_vel) +{ + const double max_vel_noise = 0.05; + // ensure safe velocity doesn't exceed maximum allowed pbs deceleration + double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel); + // ensure safe path velocity is also above ego min velocity + cmp_safe_vel = std::max(cmp_safe_vel, min_vel); + // ensure we only lower the original velocity (and do not increase it) + cmp_safe_vel = std::min(cmp_safe_vel, original_vel); + return cmp_safe_vel; +} } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner 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 26aaaaae02df0..ad7d7da0f428a 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 @@ -99,7 +99,7 @@ bool OcclusionSpotModule::modifyPathVelocity( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here - utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego, 0.0); + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource 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 3a3d6013b49b6..60e2d211e9ca6 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 @@ -93,7 +93,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( 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, 0.0); + utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); if (param_.debug) {