Skip to content

Commit

Permalink
feat(crosswalk)!: improve stuck prevention on crosswalk (autowarefoun…
Browse files Browse the repository at this point in the history
…dation#6150)

* improve algorithm
* sync param

Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
yuki-takagi-66 authored and karishma1911 committed Jun 3, 2024
1 parent b27d431 commit 99a45bd
Show file tree
Hide file tree
Showing 4 changed files with 31 additions and 31 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection
stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not.
max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path.
stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path.
required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle
min_acc: -1.0 # min acceleration [m/ss]
min_jerk: -1.0 # min jerk [m/sss]
max_jerk: 1.0 # max jerk [m/sss]
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_velocity_crosswalk_module/src/manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,8 +68,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_velocity");
cp.max_stuck_vehicle_lateral_offset =
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset");
cp.stuck_vehicle_attention_range =
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range");
cp.required_clearance =
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.required_clearance");
cp.min_acc_for_stuck_vehicle = getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.min_acc");
cp.max_jerk_for_stuck_vehicle =
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.max_jerk");
Expand Down
54 changes: 27 additions & 27 deletions planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -906,48 +906,48 @@ std::optional<StopFactor> CrosswalkModule::checkStopForStuckVehicles(
continue;
}

const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position;
const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos);
const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pose.position);
if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) {
continue;
}

const auto & ego_pos = planner_data_->current_odometry->pose.position;
const auto ego_vel = planner_data_->current_velocity->twist.linear.x;
const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x;

const double near_attention_range =
calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back());
const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range;

const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos);

if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) {
// Plan STOP considering min_acc, max_jerk and min_jerk.
const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints(
ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle,
p.min_jerk_for_stuck_vehicle);
if (!min_feasible_dist_ego2stop) {
continue;
// check if STOP is required
const double crosswalk_front_to_obj_rear =
calcSignedArcLength(ego_path.points, path_intersects.front(), obj_pose.position) -
object.shape.dimensions.x / 2.0;
const double crosswalk_back_to_obj_rear =
calcSignedArcLength(ego_path.points, path_intersects.back(), obj_pose.position) -
object.shape.dimensions.x / 2.0;
const double required_space_length =
planner_data_->vehicle_info_.vehicle_length_m + planner_param_.required_clearance;

if (crosswalk_front_to_obj_rear > 0.0 && crosswalk_back_to_obj_rear < required_space_length) {
// If there exists at least one vehicle ahead, plan STOP considering min_acc, max_jerk and
// min_jerk. Note that nearest search is not required because the stop pose independents to
// the vehicles.
const auto braking_distance = calcDecelDistWithJerkAndAccConstraints(
planner_data_->current_velocity->twist.linear.x, 0.0,
planner_data_->current_acceleration->accel.accel.linear.x, p.min_acc_for_stuck_vehicle,
p.max_jerk_for_stuck_vehicle, p.min_jerk_for_stuck_vehicle);
if (!braking_distance) {
return {};
}

const auto & ego_pos = planner_data_->current_odometry->pose.position;
const double dist_ego2stop =
calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position);
const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop);
const double feasible_dist_ego2stop = std::max(*braking_distance, dist_ego2stop);
const double dist_to_ego =
calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos);

const auto feasible_stop_pose =
calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop);
if (!feasible_stop_pose) {
continue;
return {};
}

setObjectsOfInterestData(
object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);

// early return may not appropriate because the nearest in range object should be handled
return createStopFactor(*feasible_stop_pose, {obj_pos});
setObjectsOfInterestData(obj_pose, object.shape, ColorName::RED);
return createStopFactor(*feasible_stop_pose, {obj_pose.position});
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ class CrosswalkModule : public SceneModuleInterface
bool enable_stuck_check_in_intersection{false};
double stuck_vehicle_velocity;
double max_stuck_vehicle_lateral_offset;
double stuck_vehicle_attention_range;
double required_clearance;
double min_acc_for_stuck_vehicle;
double max_jerk_for_stuck_vehicle;
double min_jerk_for_stuck_vehicle;
Expand Down

0 comments on commit 99a45bd

Please sign in to comment.