-
Notifications
You must be signed in to change notification settings - Fork 698
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.
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
fix(start_planner): issue when ego does not straddle lane bounds and starts from 0 speed #7004
Changes from 1 commit
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,4 +1,4 @@ | ||
// Copyright 2022 TIER IV, Inc. | ||
Check notice on line 1 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp
|
||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
|
@@ -241,9 +241,12 @@ | |
DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); | ||
} | ||
|
||
constexpr double moving_velocity_threshold = 0.1; | ||
const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x; | ||
if ( | ||
planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && | ||
status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { | ||
status_.driving_forward && !status_.first_engaged_and_driving_forward_time && | ||
Check warning on line 248 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp
|
||
ego_velocity > moving_velocity_threshold) { | ||
Check notice on line 249 in planning/behavior_path_start_planner_module/src/start_planner_module.cpp
|
||
status_.first_engaged_and_driving_forward_time = clock_->now(); | ||
} | ||
|
||
|
@@ -1265,8 +1268,12 @@ | |
// 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. | ||
// this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be | ||
// activated. | ||
const bool override_ego_stopped_check = std::invoke([&]() { | ||
if (!status_.first_engaged_and_driving_forward_time) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. and change this condition to There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. @soblin I cannot check the current speed, I need a check that tells me if at some point in the PAST the ego has moved faster than the threshold speed. So this approach cannot work. |
||
return true; | ||
} | ||
if (status_.planner_type != PlannerType::GEOMETRIC) { | ||
return false; | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Can you keep this condition as before, and