Skip to content

Commit 0276b44

Browse files
feat(lane_departure_checker,start_planner): add check for path within lanes for bvspm (autowarefoundation#6366)
* WIP add new methods for lane departure checker Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add lanelet polygon check for lane departure Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * use new checkPathWillLeaveLane function Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * working solution, fix union bug Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Add check fo backwards path Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * delete departure check lanes Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add lane departure check to geometric pullout Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * merge all union polygon Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * pre-commit changes Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * move the cheap/fast check first to possibly boost performance Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 8cd2176 commit 0276b44

File tree

11 files changed

+177
-51
lines changed

11 files changed

+177
-51
lines changed

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@
1818
#include <boost/geometry/core/cs.hpp>
1919
#include <boost/geometry/geometries/geometries.hpp>
2020
#include <boost/geometry/geometries/register/point.hpp>
21+
#include <boost/geometry/geometries/register/ring.hpp>
2122

2223
#define EIGEN_MPL2_ONLY
2324
#include <Eigen/Core>
@@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLIN
9899
tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT
99100
BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT
100101
tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT
102+
BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d) // NOLINT
101103

102104
#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

+20-1
Original file line numberDiff line numberDiff line change
@@ -29,14 +29,20 @@
2929
#include <geometry_msgs/msg/twist_stamped.hpp>
3030
#include <nav_msgs/msg/odometry.hpp>
3131

32+
#include <boost/geometry/algorithms/envelope.hpp>
33+
#include <boost/geometry/algorithms/union.hpp>
3234
#include <boost/geometry/index/rtree.hpp>
33-
#include <boost/optional.hpp>
3435

3536
#include <lanelet2_core/LaneletMap.h>
37+
#include <lanelet2_core/geometry/BoundingBox.h>
38+
#include <lanelet2_core/geometry/LaneletMap.h>
39+
#include <lanelet2_core/geometry/LineString.h>
40+
#include <lanelet2_core/geometry/Polygon.h>
3641

3742
#include <map>
3843
#include <memory>
3944
#include <string>
45+
#include <utility>
4046
#include <vector>
4147

4248
namespace lane_departure_checker
@@ -112,6 +118,19 @@ class LaneDepartureChecker
112118
bool checkPathWillLeaveLane(
113119
const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
114120

121+
std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
122+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
123+
124+
std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
125+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
126+
127+
bool checkPathWillLeaveLane(
128+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
129+
130+
PathWithLaneId cropPointsOutsideOfLanes(
131+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path,
132+
const size_t end_index);
133+
115134
static bool isOutOfLane(
116135
const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint);
117136

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

+89
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@ using motion_utils::calcArcLength;
3434
using tier4_autoware_utils::LinearRing2d;
3535
using tier4_autoware_utils::LineString2d;
3636
using tier4_autoware_utils::MultiPoint2d;
37+
using tier4_autoware_utils::MultiPolygon2d;
3738
using tier4_autoware_utils::Point2d;
3839

3940
namespace
@@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets(
9293

9394
return candidate_lanelets;
9495
}
96+
9597
} // namespace
9698

9799
namespace lane_departure_checker
@@ -298,6 +300,92 @@ bool LaneDepartureChecker::willLeaveLane(
298300
return false;
299301
}
300302

303+
std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLaneletsFromPath(
304+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
305+
{
306+
// Get Footprint Hull basic polygon
307+
std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
308+
LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints);
309+
auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d {
310+
lanelet::BasicPolygon2d basic_polygon;
311+
for (const auto & point : footprint_hull) {
312+
Eigen::Vector2d p(point.x(), point.y());
313+
basic_polygon.push_back(p);
314+
}
315+
return basic_polygon;
316+
};
317+
lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull);
318+
319+
// Find all lanelets that intersect the footprint hull
320+
return lanelet::geometry::findWithin2d(
321+
lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
322+
}
323+
324+
std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
325+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
326+
{
327+
const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
328+
// Fuse lanelets into a single BasicPolygon2d
329+
auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
330+
if (lanelets_distance_pair.empty()) return std::nullopt;
331+
if (lanelets_distance_pair.size() == 1)
332+
return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
333+
334+
lanelet::BasicPolygon2d merged_polygon =
335+
lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
336+
for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
337+
const auto & route_lanelet = lanelets_distance_pair.at(i).second;
338+
const auto & poly = route_lanelet.polygon2d().basicPolygon();
339+
340+
std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
341+
boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
342+
343+
// Update merged_polygon by accumulating all merged results
344+
merged_polygon.clear();
345+
for (const auto & temp_poly : lanelet_union_temp) {
346+
merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
347+
}
348+
}
349+
if (merged_polygon.empty()) return std::nullopt;
350+
return merged_polygon;
351+
}();
352+
353+
return fused_lanelets;
354+
}
355+
356+
bool LaneDepartureChecker::checkPathWillLeaveLane(
357+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
358+
{
359+
// check if the footprint is not fully contained within the fused lanelets polygon
360+
const std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
361+
const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
362+
if (!fused_lanelets_polygon) return true;
363+
return !std::all_of(
364+
vehicle_footprints.begin(), vehicle_footprints.end(),
365+
[&fused_lanelets_polygon](const auto & footprint) {
366+
return boost::geometry::within(footprint, fused_lanelets_polygon.value());
367+
});
368+
}
369+
370+
PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes(
371+
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index)
372+
{
373+
PathWithLaneId temp_path;
374+
const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path);
375+
if (path.points.empty() || !fused_lanelets_polygon) return temp_path;
376+
const auto vehicle_footprints = createVehicleFootprints(path);
377+
size_t idx = 0;
378+
std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) {
379+
if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) {
380+
temp_path.points.push_back(path.points.at(idx));
381+
}
382+
++idx;
383+
});
384+
PathWithLaneId cropped_path = path;
385+
cropped_path.points = temp_path.points;
386+
return cropped_path;
387+
}
388+
301389
bool LaneDepartureChecker::isOutOfLane(
302390
const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint)
303391
{
@@ -364,4 +452,5 @@ bool LaneDepartureChecker::willCrossBoundary(
364452
}
365453
return false;
366454
}
455+
367456
} // namespace lane_departure_checker

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,8 @@
1818
#include "behavior_path_planner_common/data_manager.hpp"
1919
#include "behavior_path_planner_common/parameters.hpp"
2020

21+
#include <lane_departure_checker/lane_departure_checker.hpp>
22+
2123
#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
2224
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
2325
#include <geometry_msgs/msg/point.hpp>
@@ -72,7 +74,8 @@ class GeometricParallelParking
7274
const bool left_side_parking);
7375
bool planPullOut(
7476
const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
75-
const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start);
77+
const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
78+
const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
7679
void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
7780
void setPlannerData(const std::shared_ptr<const PlannerData> & planner_data)
7881
{
@@ -115,7 +118,8 @@ class GeometricParallelParking
115118
const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
116119
const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
117120
const bool is_forward, const bool left_side_parking, const double end_pose_offset,
118-
const double lane_departure_margin, const double arc_path_interval);
121+
const double lane_departure_margin, const double arc_path_interval,
122+
const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
119123
PathWithLaneId generateArcPath(
120124
const Pose & center, const double radius, const double start_yaw, double end_yaw,
121125
const double arc_path_interval, const bool is_left_turn, const bool is_forward);

planning/behavior_path_planner_common/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@
4949
<depend>freespace_planning_algorithms</depend>
5050
<depend>geometry_msgs</depend>
5151
<depend>interpolation</depend>
52+
<depend>lane_departure_checker</depend>
5253
<depend>lanelet2_extension</depend>
5354
<depend>magic_enum</depend>
5455
<depend>motion_utils</depend>

planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp

+24-4
Original file line numberDiff line numberDiff line change
@@ -116,7 +116,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
116116
: parameters_.backward_parking_path_interval;
117117
auto arc_paths = planOneTrial(
118118
start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
119-
end_pose_offset, lane_departure_margin, arc_path_interval);
119+
end_pose_offset, lane_departure_margin, arc_path_interval, {});
120120
if (arc_paths.empty()) {
121121
return std::vector<PathWithLaneId>{};
122122
}
@@ -222,7 +222,8 @@ bool GeometricParallelParking::planPullOver(
222222

223223
bool GeometricParallelParking::planPullOut(
224224
const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
225-
const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start)
225+
const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
226+
const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
226227
{
227228
constexpr bool is_forward = false; // parking backward means pull_out forward
228229
constexpr double start_pose_offset = 0.0; // start_pose is current_pose
@@ -242,7 +243,7 @@ bool GeometricParallelParking::planPullOut(
242243
auto arc_paths = planOneTrial(
243244
*end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start,
244245
start_pose_offset, parameters_.pull_out_lane_departure_margin,
245-
parameters_.pull_out_path_interval);
246+
parameters_.pull_out_path_interval, lane_departure_checker);
246247
if (arc_paths.empty()) {
247248
// not found path
248249
continue;
@@ -362,7 +363,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
362363
const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
363364
const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
364365
const bool is_forward, const bool left_side_parking, const double end_pose_offset,
365-
const double lane_departure_margin, const double arc_path_interval)
366+
const double lane_departure_margin, const double arc_path_interval,
367+
const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
366368
{
367369
clearPaths();
368370

@@ -496,6 +498,24 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
496498
setLaneIdsToPath(path_turn_first);
497499
setLaneIdsToPath(path_turn_second);
498500

501+
if (lane_departure_checker) {
502+
const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
503+
504+
const bool is_path_turn_first_outside_lanes =
505+
lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_first);
506+
507+
if (is_path_turn_first_outside_lanes) {
508+
return std::vector<PathWithLaneId>{};
509+
}
510+
511+
const bool is_path_turn_second_outside_lanes =
512+
lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_second);
513+
514+
if (is_path_turn_second_outside_lanes) {
515+
return std::vector<PathWithLaneId>{};
516+
}
517+
}
518+
499519
// generate arc path vector
500520
paths_.push_back(path_turn_first);
501521
paths_.push_back(path_turn_second);

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp

+8-1
Original file line numberDiff line numberDiff line change
@@ -19,20 +19,27 @@
1919
#include "behavior_path_start_planner_module/pull_out_path.hpp"
2020
#include "behavior_path_start_planner_module/pull_out_planner_base.hpp"
2121

22+
#include <lane_departure_checker/lane_departure_checker.hpp>
23+
2224
#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
2325

26+
#include <memory>
27+
2428
namespace behavior_path_planner
2529
{
2630
class GeometricPullOut : public PullOutPlannerBase
2731
{
2832
public:
29-
explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);
33+
explicit GeometricPullOut(
34+
rclcpp::Node & node, const StartPlannerParameters & parameters,
35+
const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker);
3036

3137
PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
3238
std::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
3339

3440
GeometricParallelParking planner_;
3541
ParallelParkingParameters parallel_parking_parameters_;
42+
std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker_;
3643
};
3744
} // namespace behavior_path_planner
3845

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -273,6 +273,7 @@ class StartPlannerModule : public SceneModuleInterface
273273
const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
274274
bool isSafePath() const;
275275
void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
276+
lanelet::ConstLanelets createDepartureCheckLanes() const;
276277

277278
// check if the goal is located behind the ego in the same route segment.
278279
bool isGoalBehindOfEgoInSameRouteSegment() const;

planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp

+7-4
Original file line numberDiff line numberDiff line change
@@ -30,9 +30,12 @@ namespace behavior_path_planner
3030
{
3131
using start_planner_utils::getPullOutLanes;
3232

33-
GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters)
33+
GeometricPullOut::GeometricPullOut(
34+
rclcpp::Node & node, const StartPlannerParameters & parameters,
35+
const std::shared_ptr<lane_departure_checker::LaneDepartureChecker> lane_departure_checker)
3436
: PullOutPlannerBase{node, parameters},
35-
parallel_parking_parameters_{parameters.parallel_parking_parameters}
37+
parallel_parking_parameters_{parameters.parallel_parking_parameters},
38+
lane_departure_checker_(lane_departure_checker)
3639
{
3740
planner_.setParameters(parallel_parking_parameters_);
3841
}
@@ -55,8 +58,8 @@ std::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const
5558
planner_.setTurningRadius(
5659
planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle);
5760
planner_.setPlannerData(planner_data_);
58-
const bool found_valid_path =
59-
planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start);
61+
const bool found_valid_path = planner_.planPullOut(
62+
start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_);
6063
if (!found_valid_path) {
6164
return {};
6265
}

0 commit comments

Comments
 (0)