File tree 1 file changed +7
-7
lines changed
planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src
1 file changed +7
-7
lines changed Original file line number Diff line number Diff line change @@ -197,14 +197,10 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
197
197
const bool is_object_away_from_path,
198
198
const std::optional<double > & ego_crosswalk_passage_direction)
199
199
{
200
- const bool is_stopped = vel < planner_param.stop_object_velocity ;
200
+ const bool is_object_stopped = vel < planner_param.stop_object_velocity ;
201
201
202
202
// Check if the object can be ignored
203
- if (is_stopped) {
204
- if (collision_state == CollisionState::IGNORE) {
205
- return ;
206
- }
207
-
203
+ if (is_object_stopped && is_ego_yielding) {
208
204
if (!time_to_start_stopped) {
209
205
time_to_start_stopped = now;
210
206
}
@@ -215,14 +211,18 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC
215
211
planner_param.timeout_set_for_no_intention_to_walk , distance_to_crosswalk);
216
212
const bool intent_to_cross =
217
213
(now - *time_to_start_stopped).seconds () < timeout_no_intention_to_walk;
218
- if (is_ego_yielding && !intent_to_cross && is_object_away_from_path) {
214
+ if (!intent_to_cross && is_object_away_from_path) {
219
215
collision_state = CollisionState::IGNORE;
220
216
return ;
221
217
}
222
218
} else {
223
219
time_to_start_stopped = std::nullopt;
224
220
}
225
221
222
+ if (is_object_stopped && collision_state == CollisionState::IGNORE) {
223
+ return ;
224
+ }
225
+
226
226
// Compare time to collision and vehicle
227
227
if (collision_point) {
228
228
auto isVehicleType = [](const uint8_t label) {
You can’t perform that action at this time.
0 commit comments