diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
index 44f623b96bb69..ccf90a0b29794 100644
--- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
+++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml
@@ -33,9 +33,10 @@
# parameters for the change of state. used only when approaching is enabled
state:
- stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
- stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
- disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
+ stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
+ stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
+ disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
+ keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
# parameter to avoid sudden stopping
slow_down_limit:
diff --git a/planning/behavior_velocity_planner/config/run_out.param.yaml b/planning/behavior_velocity_planner/config/run_out.param.yaml
index 44f623b96bb69..ccf90a0b29794 100644
--- a/planning/behavior_velocity_planner/config/run_out.param.yaml
+++ b/planning/behavior_velocity_planner/config/run_out.param.yaml
@@ -33,9 +33,10 @@
# parameters for the change of state. used only when approaching is enabled
state:
- stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
- stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
- disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
+ stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping
+ stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state
+ disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value
+ keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition
# parameter to avoid sudden stopping
slow_down_limit:
diff --git a/planning/behavior_velocity_planner/docs/run_out/state_transition.svg b/planning/behavior_velocity_planner/docs/run_out/state_transition.svg
deleted file mode 100644
index aad4bdd34c4cd..0000000000000
--- a/planning/behavior_velocity_planner/docs/run_out/state_transition.svg
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp
index e378e5b385428..0ee34bf6c9685 100644
--- a/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp
+++ b/planning/behavior_velocity_planner/include/scene_module/run_out/state_machine.hpp
@@ -44,6 +44,7 @@ class StateMachine
explicit StateMachine(const StateParam & state_param) { state_param_ = state_param; }
State getCurrentState() const { return state_; }
+ boost::optional getTargetObstacle() const { return target_obstacle_; }
std::string toString(const State & state) const;
void updateState(const StateInput & state_input, rclcpp::Clock & clock);
@@ -51,6 +52,9 @@ class StateMachine
StateParam state_param_;
State state_{State::GO};
rclcpp::Time stop_time_;
+ rclcpp::Time prev_approach_time_;
+ boost::optional prev_obstacle_{};
+ boost::optional target_obstacle_{};
};
} // namespace run_out_utils
} // namespace behavior_velocity_planner
diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp
index 357ec74c9d0a6..0a16649c8ed7a 100644
--- a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp
+++ b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp
@@ -82,6 +82,7 @@ struct StateParam
float stop_thresh;
float stop_time_thresh;
float disable_approach_dist;
+ float keep_approach_duration;
};
struct SlowDownLimit
diff --git a/planning/behavior_velocity_planner/run-out-design.md b/planning/behavior_velocity_planner/run-out-design.md
index 2772a39a363c3..95378350806d1 100644
--- a/planning/behavior_velocity_planner/run-out-design.md
+++ b/planning/behavior_velocity_planner/run-out-design.md
@@ -147,7 +147,21 @@ The decision to approach the obstacle is determined by a simple state transition
-
+```plantuml
+@startuml
+hide empty description
+left to right direction
+title State transition for approaching the obstacle
+
+[*] --> GO
+GO --> STOP : Current velocity is less than threshold
+STOP --> GO : Current velocity is larger than threshold
+
+STOP --> APPROACH : Stop duration is larger than threshold
+APPROACH --> GO : There are no obstacles or \n distance to the obstacle is larger than threshold
+APPROACH --> APPROACH : Approach duration is less than threshold
+@enduml
+```
##### Limit velocity with specified jerk and acc limit
@@ -190,11 +204,12 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab
| `margin` | double | [m] distance on how close ego approaches the obstacle |
| `limit_vel_kmph` | double | [km/h] limit velocity for approaching after stopping |
-| Parameter /state | Type | Description |
-| ----------------------- | ------ | ----------------------------------------------------------------------------------- |
-| `stop_thresh` | double | [m/s] threshold to decide if ego is stopping |
-| `stop_time_thresh` | double | [sec] threshold for stopping time to transit to approaching state |
-| `disable_approach_dist` | double | [m] end the approaching state if distance to the obstacle is longer than this value |
+| Parameter /state | Type | Description |
+| ------------------------ | ------ | ----------------------------------------------------------------------------------- |
+| `stop_thresh` | double | [m/s] threshold to decide if ego is stopping |
+| `stop_time_thresh` | double | [sec] threshold for stopping time to transit to approaching state |
+| `disable_approach_dist` | double | [m] end the approaching state if distance to the obstacle is longer than this value |
+| `keep_approach_duration` | double | [sec] keep approach state for this duration to avoid chattering of state transition |
| Parameter /slow_down_limit | Type | Description |
| -------------------------- | ------ | ------------------------------------------------------------- |
diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp
index 43846ba767e1a..2c4ab669cbf4c 100644
--- a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp
@@ -94,6 +94,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
p.stop_thresh = node.declare_parameter(ns_s + ".stop_thresh", 0.01);
p.stop_time_thresh = node.declare_parameter(ns_s + ".stop_time_thresh", 3.0);
p.disable_approach_dist = node.declare_parameter(ns_s + ".disable_approach_dist", 4.0);
+ p.keep_approach_duration = node.declare_parameter(ns_s + ".keep_approach_duration", 1.0);
}
{
diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
index 74bc9b8ac1bc9..e45d50d7429ab 100644
--- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
@@ -674,27 +674,28 @@ void RunOutModule::insertVelocityForState(
// get updated state and target obstacle to decelerate
const auto state = state_machine_->getCurrentState();
+ const auto target_obstacle = state_machine_->getTargetObstacle();
// no obstacles to decelerate
- if (!dynamic_obstacle) {
+ if (!target_obstacle) {
return;
}
// insert velocity for each state
switch (state) {
case State::GO: {
- insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path);
+ insertStoppingVelocity(target_obstacle, current_pose, current_vel, current_acc, output_path);
break;
}
case State::STOP: {
- insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path);
+ insertStoppingVelocity(target_obstacle, current_pose, current_vel, current_acc, output_path);
break;
}
case State::APPROACH: {
insertApproachingVelocity(
- *dynamic_obstacle, current_pose, planner_param.approaching.limit_vel_kmph / 3.6,
+ *target_obstacle, current_pose, planner_param.approaching.limit_vel_kmph / 3.6,
planner_param.approaching.margin, output_path);
debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP);
break;
diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp
index f2a92ac7dcdac..daeb86393b1e9 100644
--- a/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/run_out/state_machine.cpp
@@ -39,11 +39,7 @@ std::string StateMachine::toString(const State & state) const
void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & clock)
{
- // no obstacles
- if (!state_input.current_obstacle) {
- state_ = State::GO;
- return;
- }
+ target_obstacle_ = state_input.current_obstacle;
switch (state_) {
case State::GO: {
@@ -59,8 +55,10 @@ void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & c
}
case State::STOP: {
- // if current velocity is larger than the threshold, transit to STOP state
- if (state_input.current_velocity > state_param_.stop_thresh) {
+ // if current velocity is larger than the threshold or
+ // there are no obstacles, transit to GO state
+ if (
+ state_input.current_velocity > state_param_.stop_thresh || !state_input.current_obstacle) {
state_ = State::GO;
return;
}
@@ -69,6 +67,8 @@ void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & c
const auto elapsed_time = (clock.now() - stop_time_).seconds();
if (elapsed_time > state_param_.stop_time_thresh) {
state_ = State::APPROACH;
+ prev_approach_time_ = clock.now();
+ prev_obstacle_ = state_input.current_obstacle;
return;
}
@@ -77,15 +77,25 @@ void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & c
}
case State::APPROACH: {
- // if the obstacle is far enough from ego, transit to GO state
const bool enough_dist_from_obstacle =
state_input.dist_to_collision > state_param_.disable_approach_dist;
- if (enough_dist_from_obstacle) {
+ // if the obstacle is enough distance from ego or there are no obstacles, transit to GO state
+ if (enough_dist_from_obstacle || !state_input.current_obstacle) {
+ // if elapsed time from entering APPROACH state is less than threshold,
+ // keep APPROACH state to avoid chattering of state transition
+ const auto elapsed_time = (clock.now() - prev_approach_time_).seconds();
+ if (elapsed_time < state_param_.keep_approach_duration) {
+ target_obstacle_ = prev_obstacle_;
+ return;
+ }
+
state_ = State::GO;
return;
}
// continue APPROACH state
+ prev_approach_time_ = clock.now();
+ prev_obstacle_ = state_input.current_obstacle;
return;
}