Skip to content

Commit

Permalink
fix(occlusion_spot): zero order hold for velocity (tier4#1542)
Browse files Browse the repository at this point in the history 
* fix(occlusion_spot): fix zero order hold for velocity

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* refactor(occlusion_spot): refactor to simplify

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>

* fix: build

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: unit test and bit of change

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: unit test

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Signed-off-by: taikitanaka3 <ttatcoder@outlook.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
Co-authored-by: Kyoichi Sugahara <81.s.kyo.19@gmail.com>
  • Loading branch information
@taikitanaka3 @kyoichi-sugahara @boyali
2 people authored and boyali committed Oct 19, 2022
1 parent 98a8ec6 commit 28162d1
Show file tree
Hide file tree
Showing 6 changed files with 34 additions and 54 deletions.
2 changes: 1 addition & 1 deletion ...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

/**
Expand Down
8 changes: 2 additions & 6 deletions ...behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
11 changes: 7 additions & 4 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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});
Expand Down
51 changes: 22 additions & 29 deletions ...ing/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -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
4 changes: 2 additions & 2 deletions planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
}
Expand All @@ -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);
}

Expand Down
12 changes: 0 additions & 12 deletions planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::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
Expand Down

0 comments on commit 28162d1

Please sign in to comment.