@@ -86,13 +86,28 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
86
86
87
87
const auto preprocessing_duration_us = stopwatch.toc (" preprocessing" );
88
88
89
- stopwatch.tic (" footprints" );
90
- const auto obstacle_forward_footprints =
91
- make_forward_footprints (dynamic_obstacles, params_, hysteresis);
92
- const auto footprints_duration_us = stopwatch.toc (" footprints" );
93
- stopwatch.tic (" collisions" );
94
- auto collisions =
95
- find_collisions (ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
89
+ tier4_autoware_utils::MultiPolygon2d obstacle_forward_footprints;
90
+ tier4_autoware_utils::MultiPolygon2d obstacle_predicted_footprints;
91
+ std::vector<Collision> collisions;
92
+ double footprints_duration_us;
93
+ if (params_.use_predicted_path ){
94
+ stopwatch.tic (" footprints" );
95
+ obstacle_predicted_footprints = create_object_footprints (dynamic_obstacles, params_);
96
+ footprints_duration_us = stopwatch.toc (" footprints" );
97
+ stopwatch.tic (" collisions" );
98
+ collisions =
99
+ find_collisions (ego_data, dynamic_obstacles, obstacle_predicted_footprints, params_);
100
+ debug_data_.obstacle_footprints = obstacle_predicted_footprints;
101
+ } else {
102
+ stopwatch.tic (" footprints" );
103
+ obstacle_forward_footprints =
104
+ make_forward_footprints (dynamic_obstacles, params_, hysteresis);
105
+ footprints_duration_us = stopwatch.toc (" footprints" );
106
+ stopwatch.tic (" collisions" );
107
+ collisions =
108
+ find_collisions (ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
109
+ debug_data_.obstacle_footprints = obstacle_forward_footprints;
110
+ }
96
111
update_object_map (object_map_, collisions, clock_->now (), ego_data.path .points , params_);
97
112
std::optional<geometry_msgs::msg::Point > earliest_collision =
98
113
find_earliest_collision (object_map_, ego_data);
@@ -128,7 +143,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
128
143
" %2.2fus\n\t collisions = %2.2fus\n " ,
129
144
total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us);
130
145
debug_data_.ego_footprints = ego_data.path_footprints ;
131
- debug_data_.obstacle_footprints = obstacle_forward_footprints;
132
146
debug_data_.z = ego_data.pose .position .z ;
133
147
return true ;
134
148
}
0 commit comments