Skip to content

Commit

Permalink
fix(intersection): fixed stuck vehicle detection area (autowarefounda…
Browse files Browse the repository at this point in the history 
…tion#2463)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
Signed-off-by: kminoda <koji.minoda@tier4.jp>
  • Loading branch information
@soblin @kminoda
soblin authored and kminoda committed Jan 6, 2023
1 parent bc6c847 commit a9a3f2f
Show file tree
Hide file tree
Showing 5 changed files with 76 additions and 85 deletions.
2 changes: 1 addition & 1 deletion ...planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr
keep_detection_vel_thr: 0.833 # == 3.0km/h
stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
intersection_max_accel: 0.5 # m/ss
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/config/intersection.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr
keep_detection_vel_thr: 0.833 # == 3.0km/h
stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length.
stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-)
stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h
intersection_velocity: 2.778 # 2.778m/s = 10.0km/h
intersection_max_accel: 0.5 # m/ss
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ class IntersectionModule : public SceneModuleInterface
Polygon2d generateEgoIntersectionLanePolygon(
lanelet::LaneletMapConstPtr lanelet_map_ptr,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
const int start_idx, const double extra_dist, const double ignore_dist) const;
const double extra_dist, const double ignore_dist) const;

/**
* @brief Modify objects predicted path. remove path point if the time exceeds timer_thr.
Expand Down
146 changes: 72 additions & 74 deletions planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,87 +150,83 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
}
const size_t closest_idx = closest_idx_opt.get();

if (stop_lines_idx_opt.has_value()) {
const auto & stop_lines = stop_lines_idx_opt.value();
const size_t stop_line_idx = stop_lines.stop_line;
const size_t pass_judge_line_idx = stop_lines.pass_judge_line;
const size_t keep_detection_line_idx = stop_lines.keep_detection_line;

const bool is_over_pass_judge_line =
util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx);
const bool is_before_keep_detection_line =
util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx);
const bool keep_detection = is_before_keep_detection_line &&
std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;

if (is_over_pass_judge_line && keep_detection) {
// in case ego could not stop exactly before the stop line, but with some overshoot,
// keep detection within some margin under low velocity threshold
RCLCPP_DEBUG(
logger_,
"over the pass judge line, but before keep detection line and low speed, "
"continue planning");
// no return here, keep planning
} else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
RCLCPP_DEBUG(logger_, "===== plan end =====");
setSafe(true);
setDistance(motion_utils::calcSignedArcLength(
path->points, planner_data_->current_pose.pose.position,
path->points.at(stop_line_idx).point.pose.position));
// no plan needed.
return true;
}
}
/* collision checking */
bool is_entry_prohibited = false;

/* get dynamic object */
const auto objects_ptr = planner_data_->predicted_objects;

/* calculate final stop lines */
bool is_entry_prohibited = false;
const double detect_length =
/* check stuck vehicle */
const double ignore_length =
planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m;
const double detect_dist =
planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m;
const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon(
lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length,
planner_param_.stuck_vehicle_detect_dist);
lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length);
const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
int stop_line_idx_final = stuck_line_idx;
int pass_judge_line_idx_final = stuck_line_idx;

/* calculate dynamic collision around detection area */
const bool has_collision = checkCollision(
lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
closest_idx, stuck_vehicle_detect_area);

/* calculate final stop lines */
int stop_line_idx_final = -1;
int pass_judge_line_idx_final = -1;
if (external_go) {
is_entry_prohibited = false;
} else if (external_stop) {
is_entry_prohibited = true;
} else if (is_stuck) {
} else if (is_stuck || has_collision) {
is_entry_prohibited = true;
stop_line_idx_final = stuck_line_idx;
pass_judge_line_idx_final = stuck_line_idx;
} else {
/* calculate dynamic collision around detection area */
const bool has_collision = checkCollision(
lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr,
closest_idx, stuck_vehicle_detect_area);
is_entry_prohibited = has_collision;
if (stop_lines_idx_opt.has_value()) {
const auto & stop_lines_idx = stop_lines_idx_opt.value();
stop_line_idx_final = stop_lines_idx.stop_line;
pass_judge_line_idx_final = stop_lines_idx.pass_judge_line;
} else {
if (has_collision) {
RCLCPP_ERROR(logger_, "generateStopLine() failed for detected objects");
RCLCPP_DEBUG(logger_, "===== plan end =====");
setSafe(true);
setDistance(std::numeric_limits<double>::lowest());
return false;
} else {
RCLCPP_DEBUG(logger_, "no need to stop");
RCLCPP_DEBUG(logger_, "===== plan end =====");
setSafe(true);
setDistance(std::numeric_limits<double>::lowest());
return true;
}
const double dist_stuck_stopline = motion_utils::calcSignedArcLength(
path->points, path->points.at(stuck_line_idx).point.pose.position,
path->points.at(closest_idx).point.pose.position);
const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline
const bool is_over_stuck_stopline =
util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) &&
dist_stuck_stopline > eps;
if (is_stuck && !is_over_stuck_stopline) {
stop_line_idx_final = stuck_line_idx;
pass_judge_line_idx_final = stuck_line_idx;
} else if (
((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) {
stop_line_idx_final = stop_lines_idx_opt.value().stop_line;
pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line;
}
}

if (stop_line_idx_final == -1) {
RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed");
RCLCPP_DEBUG(logger_, "===== plan end =====");
setSafe(true);
setDistance(std::numeric_limits<double>::lowest());
return false;
}

const bool is_over_pass_judge_line =
util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final);
const bool is_before_keep_detection_line =
stop_lines_idx_opt.has_value()
? util::isBeforeTargetIndex(
*path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line)
: false;
const bool keep_detection =
is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr;

if (is_over_pass_judge_line && keep_detection) {
// in case ego could not stop exactly before the stop line, but with some overshoot,
// keep detection within some margin under low velocity threshold
} else if (is_over_pass_judge_line && is_go_out_ && !external_stop) {
RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed.");
RCLCPP_DEBUG(logger_, "===== plan end =====");
setSafe(true);
setDistance(motion_utils::calcSignedArcLength(
path->points, planner_data_->current_pose.pose.position,
path->points.at(stop_line_idx_final).point.pose.position));
return true;
}

state_machine_.setStateWithMarginTime(
is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO,
logger_.get_child("state_machine"), *clock_);
Expand Down Expand Up @@ -314,8 +310,11 @@ bool IntersectionModule::checkCollision(
using lanelet::utils::getPolygonFromArcLength;

/* generate ego-lane polygon */
const auto ego_poly =
generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0);
const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d();
Polygon2d ego_poly{};
for (const auto & p : ego_lane_poly) {
ego_poly.outer().emplace_back(p.x(), p.y());
}
lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path);
lanelet::ConstLanelet closest_lanelet;
lanelet::utils::query::getClosestLanelet(
Expand Down Expand Up @@ -475,7 +474,7 @@ bool IntersectionModule::checkCollision(
Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon(
lanelet::LaneletMapConstPtr lanelet_map_ptr,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
const int start_idx, const double extra_dist, const double ignore_dist) const
const double extra_dist, const double ignore_dist) const
{
using lanelet::utils::getArcCoordinates;
using lanelet::utils::getLaneletLength3d;
Expand All @@ -484,15 +483,14 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon(

lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path);

const auto start_arc_coords = getArcCoordinates(
ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point));
const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front());

const auto closest_arc_coords = getArcCoordinates(
ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point));

const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length
? closest_arc_coords.length
: start_arc_coords.length + ignore_dist;
const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length
? intersection_exit_length - ignore_dist
: closest_arc_coords.length;

const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist;

Expand Down
9 changes: 1 addition & 8 deletions planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
const double keep_detection_line_margin, const bool use_stuck_stopline,
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger,
const rclcpp::Clock::SharedPtr clock)
[[maybe_unused]] const rclcpp::Clock::SharedPtr clock)
{
/* set judge line dist */
const double current_vel = planner_data->current_velocity->twist.linear.x;
Expand Down Expand Up @@ -196,13 +196,6 @@ std::pair<std::optional<size_t>, std::optional<StopLineIdx>> generateStopLine(
if (use_stuck_stopline) {
// the first point in intersection lane
stuck_stop_line_idx_ip = lane_interval_ip_start;
if (stuck_stop_line_idx_ip == 0) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
logger, *clock, 1000 /* ms */,
"use_stuck_stopline, but ego is already in the intersection, not generating stuck stop "
"line");
return {std::nullopt, std::nullopt};
}
} else {
const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons(
path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas);
Expand Down

0 comments on commit a9a3f2f

Please sign in to comment.