Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(obstacle cruise): reduce computation cost (#7040) #1306

Merged
merged 1 commit into from
May 20, 2024
+18 −8
Merged

feat(obstacle cruise): reduce computation cost (#7040) #1306

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,7 @@ std::vector<PointWithStamp> getCollisionPoints(
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape,
const rclcpp::Time & current_time, const bool is_driving_forward,
std::vector<size_t> & collision_index,
const double max_lat_dist = std::numeric_limits<double>::max(),
std::vector<size_t> & collision_index, const double max_dist = std::numeric_limits<double>::max(),
const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());

} // namespace polygon_utils
Expand Down
17 changes: 14 additions & 3 deletions planning/obstacle_cruise_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1077,7 +1077,11 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle(
std::vector<size_t> collision_index;
const auto collision_points = polygon_utils::getCollisionPoints(
traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(),
is_driving_forward_, collision_index);
is_driving_forward_, collision_index,
calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length +
std::hypot(
vehicle_info_.vehicle_length_m,
vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise));
return collision_points;
}

Expand Down Expand Up @@ -1117,7 +1121,10 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle(
const auto collision_points = polygon_utils::getCollisionPoints(
traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(),
is_driving_forward_, collision_index,
vehicle_info_.vehicle_width_m + p.max_lat_margin_for_cruise,
calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length +
std::hypot(
vehicle_info_.vehicle_length_m,
vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise),
p.max_prediction_time_for_collision_check);
if (collision_points.empty()) {
// Ignore vehicle obstacles outside the trajectory without collision
Expand Down Expand Up @@ -1193,7 +1200,11 @@ std::optional<StopObstacle> ObstacleCruisePlannerNode::createStopObstacle(
std::vector<size_t> collision_index;
const auto collision_points = polygon_utils::getCollisionPoints(
traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(),
is_driving_forward_, collision_index);
is_driving_forward_, collision_index,
calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length +
std::hypot(
vehicle_info_.vehicle_length_m,
vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop));
if (collision_points.empty()) {
RCLCPP_INFO_EXPRESSION(
get_logger(), enable_debug_info_,
Expand Down
6 changes: 3 additions & 3 deletions planning/obstacle_cruise_planner/src/polygon_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,18 +44,18 @@ PointWithStamp calcNearestCollisionPoint(
return collision_points.at(min_idx);
}

// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon
// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon
// calculation.
std::optional<std::pair<size_t, std::vector<PointWithStamp>>> getCollisionIndex(
const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time,
const Shape & object_shape, const double max_lat_dist = std::numeric_limits<double>::max())
const Shape & object_shape, const double max_dist = std::numeric_limits<double>::max())
{
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape);
for (size_t i = 0; i < traj_polygons.size(); ++i) {
const double approximated_dist =
tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose);
if (approximated_dist > max_lat_dist) {
if (approximated_dist > max_dist) {
continue;
}

Expand Down
Loading