Skip to content

Commit

Permalink
fix(start_planner): issue when ego does not straddle lane bounds and …
Browse files Browse the repository at this point in the history
…starts from 0 speed (autowarefoundation#7004)

* fix issue when ego does not straddle lane bounds and starts from 0 speed

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add new status for ego departed

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
  • Loading branch information
danielsanchezaran committed May 15, 2024
1 parent 3a6742e commit 11ad7c6
Show file tree
Hide file tree
Showing 3 changed files with 20 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -738,10 +738,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
return std::make_pair(TurnSignalInfo{}, true);
}

if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
// Check if the ego will cross lane bounds.
// Note that pull out requires blinkers, even if the ego does not cross lane bounds
if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
return std::make_pair(TurnSignalInfo{}, true);
}

// If the ego has stopped and its close to completing its shift, turn off the blinkers
constexpr double STOPPED_THRESHOLD = 0.1; // [m/s]
if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) {
if (isNearEndOfShift(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,8 @@ struct PullOutStatus
//! record the first time when ego started forward-driving (maybe after backward driving
//! completion) in AUTONOMOUS operation mode
std::optional<rclcpp::Time> first_engaged_and_driving_forward_time{std::nullopt};
// record if the ego has departed from the start point
bool has_departed{false};

PullOutStatus() {}
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -183,6 +183,13 @@ void StartPlannerModule::updateData()
status_.first_engaged_and_driving_forward_time = clock_->now();
}

constexpr double moving_velocity_threshold = 0.1;
const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x;
if (status_.first_engaged_and_driving_forward_time && ego_velocity > moving_velocity_threshold) {
// Ego is engaged, and has moved
status_.has_departed = true;
}

status_.backward_driving_complete = hasFinishedBackwardDriving();
if (status_.backward_driving_complete) {
updateStatusAfterBackwardDriving();
Expand Down Expand Up @@ -1212,8 +1219,10 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
// In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction.
// This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and
// close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid
// this issue.
const bool override_ego_stopped_check = std::invoke([&]() {
// this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be
// activated.

const bool geometric_planner_has_not_finished_first_path = std::invoke([&]() {
if (status_.planner_type != PlannerType::GEOMETRIC) {
return false;
}
Expand All @@ -1224,6 +1233,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
return distance_from_ego_to_stop_point < distance_threshold;
});

const bool override_ego_stopped_check =
!status_.has_departed || geometric_planner_has_not_finished_first_path;

const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length,
status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);
Expand Down

0 comments on commit 11ad7c6

Please sign in to comment.