Skip to content

Commit 4225f4e

Browse files
authored
fix(run_out): do not insert stop point when stopped in Object method (#5911)
Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
1 parent 9761d21 commit 4225f4e

File tree

1 file changed

+15
-6
lines changed
  • planning/behavior_velocity_run_out_module/src

1 file changed

+15
-6
lines changed

planning/behavior_velocity_run_out_module/src/scene.cpp

+15-6
Original file line numberDiff line numberDiff line change
@@ -615,16 +615,25 @@ std::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
615615
// vehicle have to decelerate if there is not enough distance with deceleration_jerk
616616
const bool deceleration_needed =
617617
*stop_dist > dist_to_collision - planner_param_.run_out.stop_margin;
618-
// avoid acceleration when ego is decelerating
619-
// TODO(Tomohito Ando): replace with more appropriate method
620-
constexpr float epsilon = 1.0e-2;
621-
constexpr float stopping_vel_mps = 2.5 / 3.6;
622-
const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon;
623-
if (!deceleration_needed && !is_stopping) {
618+
const auto & detection_method = planner_param_.run_out.detection_method;
619+
620+
if (!deceleration_needed && detection_method == "Object") {
624621
debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK);
625622
return {};
626623
}
627624

625+
// If the detection method assumes running out, avoid acceleration when the ego is decelerating.
626+
// TODO(Tomohito Ando): replace with more appropriate way
627+
if (!deceleration_needed && detection_method != "Object") {
628+
constexpr float epsilon = 1.0e-2;
629+
constexpr float stopping_vel_mps = 2.5 / 3.6;
630+
const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon;
631+
if (!is_stopping) {
632+
debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK);
633+
return {};
634+
}
635+
}
636+
628637
// calculate the stop point for base link
629638
const float base_to_collision_point =
630639
planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front;

0 commit comments

Comments
 (0)