File tree 5 files changed +21
-1
lines changed
planning/behavior_path_start_planner_module
include/behavior_path_start_planner_module
5 files changed +21
-1
lines changed Original file line number Diff line number Diff line change 38
38
backward_path_update_duration : 3.0
39
39
ignore_distance_from_lane_end : 15.0
40
40
# turns signal
41
+ prepare_time_before_start : 3.0
41
42
th_turn_signal_on_lateral_offset : 1.0
42
43
intersection_search_length : 30.0
43
44
length_ratio_for_turn_signal_deactivation_near_intersection : 0.5
Original file line number Diff line number Diff line change @@ -59,6 +59,7 @@ struct StartPlannerParameters
59
59
double th_arrived_distance{0.0 };
60
60
double th_stopped_velocity{0.0 };
61
61
double th_stopped_time{0.0 };
62
+ double prepare_time_before_start{0.0 };
62
63
double th_turn_signal_on_lateral_offset{0.0 };
63
64
double th_distance_to_middle_of_the_road{0.0 };
64
65
double intersection_search_length{0.0 };
Original file line number Diff line number Diff line change @@ -70,6 +70,8 @@ struct PullOutStatus
70
70
bool prev_is_safe_dynamic_objects{false };
71
71
std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr };
72
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};
73
75
74
76
PullOutStatus () {}
75
77
};
@@ -261,6 +263,7 @@ class StartPlannerModule : public SceneModuleInterface
261
263
const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose,
262
264
const double velocity_threshold, const double object_check_backward_distance,
263
265
const double object_check_forward_distance) const ;
266
+ bool needToPrepareBlinkerBeforeStart () const ;
264
267
bool hasFinishedPullOut () const ;
265
268
bool hasFinishedBackwardDriving () const ;
266
269
bool hasCollisionWithDynamicObjects () const ;
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 @@ -117,7 +117,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
117
117
BehaviorModuleOutput StartPlannerModule::run ()
118
118
{
119
119
updateData ();
120
- if (!isActivated ()) {
120
+ if (!isActivated () || needToPrepareBlinkerBeforeStart () ) {
121
121
return planWaitingApproval ();
122
122
}
123
123
@@ -176,6 +176,10 @@ void StartPlannerModule::updateData()
176
176
DEBUG_PRINT (" StartPlannerModule::updateData() received new route, reset status" );
177
177
}
178
178
179
+ if (!status_.first_approved_time && isActivated ()) {
180
+ status_.first_approved_time = clock_->now ();
181
+ }
182
+
179
183
if (hasFinishedBackwardDriving ()) {
180
184
updateStatusAfterBackwardDriving ();
181
185
DEBUG_PRINT (" StartPlannerModule::updateData() completed backward driving" );
@@ -1078,6 +1082,16 @@ bool StartPlannerModule::hasFinishedPullOut() const
1078
1082
return has_finished;
1079
1083
}
1080
1084
1085
+ bool StartPlannerModule::needToPrepareBlinkerBeforeStart () const
1086
+ {
1087
+ if (!status_.first_approved_time ) {
1088
+ return true ;
1089
+ }
1090
+ const auto first_approved_time = status_.first_approved_time .value ();
1091
+ const double elapsed = rclcpp::Duration (clock_->now () - first_approved_time).seconds ();
1092
+ return elapsed < parameters_->prepare_time_before_start ;
1093
+ }
1094
+
1081
1095
bool StartPlannerModule::isStuck ()
1082
1096
{
1083
1097
if (!isStopped ()) {
You can’t perform that action at this time.
0 commit comments