Skip to content

Commit 7ef4183

Browse files
author
beyza
committed
add an option for collision check
Signed-off-by: beyza <bnk@leodrive.ai>
1 parent be2299d commit 7ef4183

File tree

1 file changed

+22
-8
lines changed

1 file changed

+22
-8
lines changed

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

+22-8
Original file line numberDiff line numberDiff line change
@@ -86,13 +86,28 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
8686

8787
const auto preprocessing_duration_us = stopwatch.toc("preprocessing");
8888

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+
}
96111
update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_);
97112
std::optional<geometry_msgs::msg::Point> earliest_collision =
98113
find_earliest_collision(object_map_, ego_data);
@@ -128,7 +143,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
128143
"%2.2fus\n\tcollisions = %2.2fus\n",
129144
total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us);
130145
debug_data_.ego_footprints = ego_data.path_footprints;
131-
debug_data_.obstacle_footprints = obstacle_forward_footprints;
132146
debug_data_.z = ego_data.pose.position.z;
133147
return true;
134148
}

0 commit comments

Comments
 (0)