Skip to content

Commit 321e46b

Browse files
committed
feat(strat_planner): add a prepare time for blinker before taking action for approval (autowarefoundation#6438)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 44b619a commit 321e46b

File tree

5 files changed

+22
-1
lines changed

5 files changed

+22
-1
lines changed

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
backward_path_update_duration: 3.0
3737
ignore_distance_from_lane_end: 15.0
3838
# turns signal
39+
prepare_time_before_start: 0.0
3940
th_turn_signal_on_lateral_offset: 1.0
4041
intersection_search_length: 30.0
4142
length_ratio_for_turn_signal_deactivation_near_intersection: 0.5

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,9 @@ struct PullOutStatus
6969
bool prev_is_safe_dynamic_objects{false};
7070
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
7171
bool has_stop_point{false};
72+
std::optional<Pose> stop_pose{std::nullopt};
73+
//! record the first time when the state changed from !isActivated() to isActivated()
74+
std::optional<rclcpp::Time> first_approved_time{std::nullopt};
7275

7376
PullOutStatus() {}
7477
};
@@ -228,6 +231,7 @@ class StartPlannerModule : public SceneModuleInterface
228231
void updateStatusAfterBackwardDriving();
229232
PredictedObjects filterStopObjectsInPullOutLanes(
230233
const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const;
234+
bool needToPrepareBlinkerBeforeStart() const;
231235
bool hasFinishedPullOut() const;
232236
bool hasFinishedBackwardDriving() const;
233237
bool hasCollisionWithDynamicObjects() const;

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ struct StartPlannerParameters
3838
double th_arrived_distance{0.0};
3939
double th_stopped_velocity{0.0};
4040
double th_stopped_time{0.0};
41+
double prepare_time_before_start{0.0};
4142
double th_turn_signal_on_lateral_offset{0.0};
4243
double th_distance_to_middle_of_the_road{0.0};
4344
double intersection_search_length{0.0};

planning/behavior_path_start_planner_module/src/manager.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
3636
p.th_arrived_distance = node->declare_parameter<double>(ns + "th_arrived_distance");
3737
p.th_stopped_velocity = node->declare_parameter<double>(ns + "th_stopped_velocity");
3838
p.th_stopped_time = node->declare_parameter<double>(ns + "th_stopped_time");
39+
p.prepare_time_before_start = node->declare_parameter<double>(ns + "prepare_time_before_start");
3940
p.th_turn_signal_on_lateral_offset =
4041
node->declare_parameter<double>(ns + "th_turn_signal_on_lateral_offset");
4142
p.th_distance_to_middle_of_the_road =

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+15-1
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
104104
BehaviorModuleOutput StartPlannerModule::run()
105105
{
106106
updateData();
107-
if (!isActivated()) {
107+
if (!isActivated() || needToPrepareBlinkerBeforeStart()) {
108108
return planWaitingApproval();
109109
}
110110

@@ -161,6 +161,10 @@ void StartPlannerModule::updateData()
161161
DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
162162
}
163163

164+
if (!status_.first_approved_time && isActivated()) {
165+
status_.first_approved_time = clock_->now();
166+
}
167+
164168
if (hasFinishedBackwardDriving()) {
165169
updateStatusAfterBackwardDriving();
166170
DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
@@ -962,6 +966,16 @@ bool StartPlannerModule::hasFinishedPullOut() const
962966
return has_finished;
963967
}
964968

969+
bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const
970+
{
971+
if (!status_.first_approved_time) {
972+
return true;
973+
}
974+
const auto first_approved_time = status_.first_approved_time.value();
975+
const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds();
976+
return elapsed < parameters_->prepare_time_before_start;
977+
}
978+
965979
bool StartPlannerModule::isStuck()
966980
{
967981
if (!isStopped()) {

0 commit comments

Comments
 (0)