File tree 5 files changed +22
-1
lines changed
planning/behavior_path_start_planner_module
include/behavior_path_start_planner_module
5 files changed +22
-1
lines changed Original file line number Diff line number Diff line change 36
36
backward_path_update_duration : 3.0
37
37
ignore_distance_from_lane_end : 15.0
38
38
# turns signal
39
+ prepare_time_before_start : 0.0
39
40
th_turn_signal_on_lateral_offset : 1.0
40
41
intersection_search_length : 30.0
41
42
length_ratio_for_turn_signal_deactivation_near_intersection : 0.5
Original file line number Diff line number Diff line change @@ -69,6 +69,9 @@ struct PullOutStatus
69
69
bool prev_is_safe_dynamic_objects{false };
70
70
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr };
71
71
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};
72
75
73
76
PullOutStatus () {}
74
77
};
@@ -228,6 +231,7 @@ class StartPlannerModule : public SceneModuleInterface
228
231
void updateStatusAfterBackwardDriving ();
229
232
PredictedObjects filterStopObjectsInPullOutLanes (
230
233
const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const ;
234
+ bool needToPrepareBlinkerBeforeStart () const ;
231
235
bool hasFinishedPullOut () const ;
232
236
bool hasFinishedBackwardDriving () const ;
233
237
bool hasCollisionWithDynamicObjects () const ;
Original file line number Diff line number Diff line change @@ -38,6 +38,7 @@ struct StartPlannerParameters
38
38
double th_arrived_distance{0.0 };
39
39
double th_stopped_velocity{0.0 };
40
40
double th_stopped_time{0.0 };
41
+ double prepare_time_before_start{0.0 };
41
42
double th_turn_signal_on_lateral_offset{0.0 };
42
43
double th_distance_to_middle_of_the_road{0.0 };
43
44
double intersection_search_length{0.0 };
Original file line number Diff line number Diff line change @@ -36,6 +36,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
36
36
p.th_arrived_distance = node->declare_parameter <double >(ns + " th_arrived_distance" );
37
37
p.th_stopped_velocity = node->declare_parameter <double >(ns + " th_stopped_velocity" );
38
38
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" );
39
40
p.th_turn_signal_on_lateral_offset =
40
41
node->declare_parameter <double >(ns + " th_turn_signal_on_lateral_offset" );
41
42
p.th_distance_to_middle_of_the_road =
Original file line number Diff line number Diff line change @@ -104,7 +104,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
104
104
BehaviorModuleOutput StartPlannerModule::run ()
105
105
{
106
106
updateData ();
107
- if (!isActivated ()) {
107
+ if (!isActivated () || needToPrepareBlinkerBeforeStart () ) {
108
108
return planWaitingApproval ();
109
109
}
110
110
@@ -161,6 +161,10 @@ void StartPlannerModule::updateData()
161
161
DEBUG_PRINT (" StartPlannerModule::updateData() received new route, reset status" );
162
162
}
163
163
164
+ if (!status_.first_approved_time && isActivated ()) {
165
+ status_.first_approved_time = clock_->now ();
166
+ }
167
+
164
168
if (hasFinishedBackwardDriving ()) {
165
169
updateStatusAfterBackwardDriving ();
166
170
DEBUG_PRINT (" StartPlannerModule::updateData() completed backward driving" );
@@ -962,6 +966,16 @@ bool StartPlannerModule::hasFinishedPullOut() const
962
966
return has_finished;
963
967
}
964
968
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
+
965
979
bool StartPlannerModule::isStuck ()
966
980
{
967
981
if (!isStopped ()) {
You can’t perform that action at this time.
0 commit comments