File tree 2 files changed +7
-2
lines changed
planning/behavior_velocity_out_of_lane_module/src
2 files changed +7
-2
lines changed Original file line number Diff line number Diff line change 21
21
#include < lanelet2_core/geometry/Polygon.h>
22
22
#include < tf2/utils.h>
23
23
24
+ #include < algorithm>
24
25
#include < vector>
25
26
26
27
namespace behavior_velocity_planner ::out_of_lane
@@ -58,14 +59,19 @@ std::vector<lanelet::BasicPolygon2d> calculate_path_footprints(
58
59
const auto base_footprint = make_base_footprint (params);
59
60
std::vector<lanelet::BasicPolygon2d> path_footprints;
60
61
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) {
62
66
const auto & path_pose = ego_data.path .points [i].point .pose ;
63
67
const auto angle = tf2::getYaw (path_pose.orientation );
64
68
const auto rotated_footprint = tier4_autoware_utils::rotatePolygon (base_footprint, angle);
65
69
lanelet::BasicPolygon2d footprint;
66
70
for (const auto & p : rotated_footprint.outer ())
67
71
footprint.emplace_back (p.x () + path_pose.position .x , p.y () + path_pose.position .y );
68
72
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 );
69
75
}
70
76
return path_footprints;
71
77
}
Original file line number Diff line number Diff line change 36
36
37
37
#include < lanelet2_core/geometry/LaneletMap.h>
38
38
39
- #include < algorithm>
40
39
#include < memory>
41
40
#include < string>
42
41
#include < utility>
You can’t perform that action at this time.
0 commit comments