Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Preempt PR3711(2) #13

Merged
merged 10 commits into from
May 22, 2023
Merged

Preempt PR3711(2) #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
1619b8d
fix(intersection): use 0 offset peeking line
soblin May 15, 2023
cfdedb2
use braking distance
soblin May 18, 2023
3db9cdd
add pass_judge_wall_pose marker
soblin May 18, 2023
6cb3b00
keep detection if ego is between stopline and pass judge line and vel…
soblin May 18, 2023
2d96f5f
modify condition
soblin May 19, 2023
406065e
use 0.0s for delay response time
soblin May 19, 2023
91f19a5
right turn scenario passes
soblin May 20, 2023
feb67bf
find all stop line candidates at first in order
soblin May 19, 2023
6f79886
use keep_detection_vel for staticPassJudgeLine
soblin May 22, 2023
e464b6e
Merge branch 'feat/disable-peeking-while-collision-detection' into fi…
soblin May 22, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 29 additions & 18 deletions planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -181,10 +181,12 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0));
}

const auto static_pass_judge_line_opt =
first_detection_area
? util::generateStaticPassJudgeLine(
first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_)
const std::optional<size_t> stuck_line_idx_opt =
first_conflicting_area
? util::generateStuckStopLine(
first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin,
planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval,
lane_interval_ip, logger_.get_child("util"))
: std::nullopt;

const auto default_stop_line_idx_opt =
Expand All @@ -194,6 +196,21 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
lane_interval_ip, logger_.get_child("util"))
: std::nullopt;

const auto static_pass_judge_line_opt =
first_detection_area
? util::generateStaticPassJudgeLine(
first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_,
planner_param_.collision_detection.keep_detection_vel_thr)
: std::nullopt;

const auto occlusion_peeking_line_idx_opt =
first_detection_area
? util::generatePeekingLimitLine(
first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_,
planner_param_.occlusion.peeking_offset)
: std::nullopt;

// TODO(Mamoru Sobue): check the ordering of these stop lines and refactor
/* calc closest index */
const auto closest_idx_opt =
motion_utils::findNearestIndex(path->points, current_pose, 3.0, M_PI_4);
Expand All @@ -218,7 +235,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
// if ego is over the pass judge line and not stopped
if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) {
/* do nothing */
} else if (is_over_default_stop_line && is_over_pass_judge_line) {
} else if (is_over_default_stop_line && is_over_pass_judge_line && !keep_detection) {
RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed.");
RCLCPP_DEBUG(logger_, "===== plan end =====");
return true;
Expand All @@ -240,13 +257,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
generateStuckVehicleDetectAreaPolygon(*path, ego_lane_with_next_lane, closest_idx);
const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area);
debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area);
const std::optional<size_t> stuck_line_idx_opt =
first_conflicting_area
? util::generateStuckStopLine(
first_conflicting_area.value(), planner_data_, planner_param_.common.stop_line_margin,
planner_param_.stuck_vehicle.use_stuck_stopline, path, path_ip, interval,
lane_interval_ip, logger_.get_child("util"))
: std::nullopt;

/* calculate dynamic collision around detection area */
/* set stop lines for base_link */
Expand All @@ -269,12 +279,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
first_detection_area.value(), path_ip, lane_interval_ip, detection_divisions_.value(),
occlusion_dist_thr)
: true;
const auto occlusion_peeking_line_idx_opt =
first_detection_area
? util::generatePeekingLimitLine(
first_detection_area.value(), path, path_ip, interval, lane_interval_ip, planner_data_,
planner_param_.occlusion.peeking_offset)
: std::nullopt;

/* calculate final stop lines */
std::optional<size_t> stop_line_idx = default_stop_line_idx_opt;
Expand Down Expand Up @@ -414,6 +418,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
setSafe(collision_state_machine_.getState() == StateMachine::State::GO);
}

RCLCPP_DEBUG(
logger_,
"has_collision = %d, is_occlusion_cleared = %d, collision_stop_required = %d, "
"first_phase_stop_required = %d, occlusion_stop_required = %d",
has_collision, is_occlusion_cleared, collision_stop_required, first_phase_stop_required,
occlusion_stop_required);

/* make decision */
if (!occlusion_activated_) {
is_go_out_ = false;
Expand Down
12 changes: 6 additions & 6 deletions planning/behavior_velocity_intersection_module/src/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -134,15 +134,15 @@ std::optional<size_t> generateStaticPassJudgeLine(
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval,
const std::pair<size_t, size_t> lane_interval,
const std::shared_ptr<const PlannerData> & planner_data)
const std::shared_ptr<const PlannerData> & planner_data, const double velocity)
{
const double velocity = planner_data->current_velocity->twist.linear.x;
const double acceleration = planner_data->current_acceleration->accel.accel.linear.x;
// const double velocity = planner_data->current_velocity->twist.linear.x;
// const double acceleration = planner_data->current_acceleration->accel.accel.linear.x;
const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold;
const double max_stop_jerk = planner_data->max_stop_jerk_threshold;
// const double max_stop_jerk = planner_data->max_stop_jerk_threshold;
// const double delay_response_time = planner_data->delay_response_time;
const double offset = -planning_utils::calcJudgeLineDistWithJerkLimit(
velocity, acceleration, max_stop_acceleration, max_stop_jerk, 0.0);
const double offset =
-planning_utils::calcJudgeLineDistWithAccLimit(velocity, max_stop_acceleration, 0.0);
const auto pass_judge_line_idx = generatePeekingLimitLine(
first_detection_area, original_path, path_ip, ip_interval, lane_interval, planner_data, offset);
if (pass_judge_line_idx) {
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_intersection_module/src/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ std::optional<size_t> generateStaticPassJudgeLine(
autoware_auto_planning_msgs::msg::PathWithLaneId * original_path,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval,
const std::pair<size_t, size_t> lane_interval,
const std::shared_ptr<const PlannerData> & planner_data);
const std::shared_ptr<const PlannerData> & planner_data, const double velocity);

std::optional<size_t> generatePeekingLimitLine(
const lanelet::CompoundPolygon3d & first_detection_area,
Expand Down