Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit c19c76e

Browse files
committedFeb 7, 2024·
only calculate path footprints within range
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent aea87b2 commit c19c76e

File tree

2 files changed

+7
-2
lines changed

2 files changed

+7
-2
lines changed
 

‎planning/behavior_velocity_out_of_lane_module/src/footprint.cpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
#include <lanelet2_core/geometry/Polygon.h>
2222
#include <tf2/utils.h>
2323

24+
#include <algorithm>
2425
#include <vector>
2526

2627
namespace behavior_velocity_planner::out_of_lane
@@ -58,14 +59,19 @@ std::vector<lanelet::BasicPolygon2d> calculate_path_footprints(
5859
const auto base_footprint = make_base_footprint(params);
5960
std::vector<lanelet::BasicPolygon2d> path_footprints;
6061
path_footprints.reserve(ego_data.path.points.size());
61-
for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) {
62+
double length = 0.0;
63+
const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) +
64+
params.front_offset + params.extra_front_offset;
65+
for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) {
6266
const auto & path_pose = ego_data.path.points[i].point.pose;
6367
const auto angle = tf2::getYaw(path_pose.orientation);
6468
const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle);
6569
lanelet::BasicPolygon2d footprint;
6670
for (const auto & p : rotated_footprint.outer())
6771
footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y);
6872
path_footprints.push_back(footprint);
73+
if (i + 1 < ego_data.path.points.size())
74+
length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point);
6975
}
7076
return path_footprints;
7177
}

‎planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

-1
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,6 @@
3636

3737
#include <lanelet2_core/geometry/LaneletMap.h>
3838

39-
#include <algorithm>
4039
#include <memory>
4140
#include <string>
4241
#include <utility>

0 commit comments

Comments
 (0)
Please sign in to comment.