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 bd2e601a4f91a..6a9adcf45d095 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 @@ -146,7 +146,7 @@ struct ObstacleInfo SafeMotion safe_motion; // safe motion of velocity and stop point geometry_msgs::msg::Point position; double max_velocity; // [m/s] Maximum velocity of the possible obstacle - double ttc; // [s] time to collision with ego + double ttv; // [s] time to vehicle for pedestrian }; /** 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 3be1bf9c5f0fd..aceef5069ee14 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 @@ -36,14 +36,10 @@ int insertSafeVelocityToPath( /** * @param: v: ego velocity config - * @param: ttc: time to collision + * @param: ttv: time to vehicle * @return safe motion **/ -SafeMotion calculateSafeMotion(const Velocity & v, const double ttc); - -double calculateInsertVelocity( - const double min_allowed_vel, const double safe_vel, const double min_vel, - const double original_vel); +SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // 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 df42a8fd0ab76..7f3cef1df88d7 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 @@ -150,9 +150,10 @@ void calcSlowDownPointsForPossibleCollision( const double d1 = dist_along_next_path_point; const auto p0 = p_prev.pose.position; const auto p1 = p_next.pose.position; + const double dist_to_next = std::abs(d1 - dist_to_col); const double v0 = p_prev.longitudinal_velocity_mps; const double v1 = p_next.longitudinal_velocity_mps; - const double v = getInterpolatedValue(d0, v0, dist_to_col, d1, v1); + const double v = (dist_to_next < 1e-6) ? v1 : v0; const double z = getInterpolatedValue(d0, p0.z, dist_to_col, d1, p1.z); // height is used to visualize marker correctly auto & col = possible_collisions.at(collision_index); @@ -320,15 +321,17 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( * ------------------ */ PossibleCollisionInfo pc; - const double ttc = std::abs(arc_coord_occlusion_with_offset.distance / param.pedestrian_vel); - SafeMotion sm = calculateSafeMotion(param.v, ttc); + // ttv: time to vehicle for pedestrian + // ttc: time to collision for ego vehicle + const double ttv = std::abs(arc_coord_occlusion_with_offset.distance / param.pedestrian_vel); + SafeMotion sm = calculateSafeMotion(param.v, ttv); double distance_to_stop = arc_coord_occlusion_with_offset.length - sm.stop_dist; const double eps = 0.1; // avoid inserting path point behind original path if (distance_to_stop < eps) distance_to_stop = eps; pc.arc_lane_dist_at_collision = {distance_to_stop, arc_coord_occlusion_with_offset.distance}; pc.obstacle_info.safe_motion = sm; - pc.obstacle_info.ttc = ttc; + pc.obstacle_info.ttv = ttv; pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position; pc.obstacle_info.max_velocity = param.pedestrian_vel; pc.collision_pose = calcPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); 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 3d4817939b7f2..319589252f71f 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 @@ -38,20 +38,26 @@ void applySafeVelocityConsideringPossibleCollision( const double v_min = param.v.min_allowed_velocity; for (auto & possible_collision : possible_collisions) { const double l_obs = possible_collision.arc_lane_dist_at_collision.length; - const double original_vel = possible_collision.collision_with_margin.longitudinal_velocity_mps; + const double v_org = possible_collision.collision_with_margin.longitudinal_velocity_mps; - // safe velocity : Consider ego emergency braking deceleration + // safe velocity : consider ego emergency braking deceleration const double v_safe = possible_collision.obstacle_info.safe_motion.safe_velocity; - // min allowed velocity : min allowed velocity consider maximum allowed braking - const double v_slow_down = - (l_obs < 0 && v0 <= v_safe) - ? v_safe - : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs); + // safe slow down: consider ego smooth brake + const double v_safe_slow_down = + planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs); + + // TODO(tanaka): consider edge case if ego passed safe margin + const double v_slow_down = (l_obs < 0 && v0 <= v_safe) ? v_safe : v_safe_slow_down; + // skip non effective velocity insertion - if (original_vel < v_safe) continue; - // compare safe velocity consider EBS, minimum allowed velocity and original velocity - const double safe_velocity = calculateInsertVelocity(v_slow_down, v_safe, v_min, original_vel); + if (v_org < v_safe || v_org < v_slow_down) continue; + + const double max_vel_noise = 0.05; + // ensure safe velocity doesn't exceed maximum allowed pbs deceleration + double safe_velocity = std::max(v_safe_slow_down + max_vel_noise, v_slow_down); + // set safe velocity is not to stop + safe_velocity = std::max(safe_velocity, v_min); possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity; const auto & pose = possible_collision.collision_with_margin.pose; insertSafeVelocityToPath(pose, safe_velocity, param, inout_path); @@ -86,7 +92,7 @@ int insertSafeVelocityToPath( return 0; } -SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) +SafeMotion calculateSafeMotion(const Velocity & v, const double ttv) { SafeMotion sm; const double j_max = v.safety_ratio * v.max_stop_jerk; @@ -95,38 +101,25 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttc) double t2 = a_max / j_max; double & v_safe = sm.safe_velocity; double & stop_dist = sm.stop_dist; - if (ttc <= t1) { + if (ttv <= t1) { // delay v_safe = 0; stop_dist = 0; - } else if (ttc <= t2 + t1) { + } else if (ttv <= t2 + t1) { // delay + const jerk - t2 = ttc - t1; + t2 = ttv - 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; + const double t3 = ttv - 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; } + // Note: safe_margin controls behavior insert point 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/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp index 2fc6f2d34b90e..93f435590f3e0 100644 --- a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp @@ -93,7 +93,7 @@ TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) * c : collision */ calcSlowDownPointsForPossibleCollision(0, path, -offset_from_start_to_ego, pcs); - if (pcs[0].collision_with_margin.longitudinal_velocity_mps - 3.0 > 1e-3) { + if (pcs[0].collision_with_margin.longitudinal_velocity_mps != 3.0) { for (size_t i = 0; i < path.points.size(); i++) { std::cout << "v : " << path.points[i].point.longitudinal_velocity_mps << "\t"; } @@ -104,7 +104,7 @@ TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) std::cout << std::endl; } EXPECT_DOUBLE_EQ(pcs[0].collision_with_margin.longitudinal_velocity_mps, 3); - EXPECT_DOUBLE_EQ(pcs[1].collision_with_margin.longitudinal_velocity_mps, 4.5); + EXPECT_DOUBLE_EQ(pcs[1].collision_with_margin.longitudinal_velocity_mps, 4); EXPECT_DOUBLE_EQ(pcs[2].collision_with_margin.longitudinal_velocity_mps, 6); } 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 7238dd971b02e..8501af4a4b7dc 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,18 +65,6 @@ TEST(safeMotion, delay_jerk_acceleration) } } -TEST(calculateInsertVelocity, min_max) -{ - using behavior_velocity_planner::occlusion_spot_utils::calculateInsertVelocity; - const double inf = std::numeric_limits::max(); - // upper bound rpb_vel - ASSERT_EQ(calculateInsertVelocity(inf, inf, inf, inf), inf); - // lower bound org_vel = 0 - ASSERT_EQ(calculateInsertVelocity(inf, inf, inf, 0), 0.0); - // lower bound min = 0 - ASSERT_EQ(calculateInsertVelocity(inf, inf, 0, inf), inf); -} - TEST(insertSafeVelocityToPath, replace_original_at_too_close_case) { /* straight path