feat(lane_departure_checker): support path_with_lane_id

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Jul 7, 2022 · 78e85f8 · 78e85f8
1 parent bbe9420
commit 78e85f8
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 0 deletions.
diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp
@@ -22,6 +22,7 @@
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -41,6 +42,7 @@
 namespace lane_departure_checker
 {
 using autoware_auto_planning_msgs::msg::HADMapRoute;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::LinearRing2d;
@@ -95,6 +97,15 @@ class LaneDepartureChecker
 
   void setParam(const Param & param) { param_ = param; }
 
+  void setVehicleInfo(const vehicle_info_util::VehicleInfo vehicle_info)
+  {
+    vehicle_info_ptr_ = std::make_shared<vehicle_info_util::VehicleInfo>(vehicle_info);
+  }
+
+  bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path);
+
+  vehicle_info_util::VehicleInfo vehicle_info_public_;
+
 private:
   Param param_;
   std::shared_ptr<vehicle_info_util::VehicleInfo> vehicle_info_ptr_;
@@ -111,6 +122,7 @@ class LaneDepartureChecker
   std::vector<LinearRing2d> createVehicleFootprints(
     const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory,
     const Param & param);
+  std::vector<LinearRing2d> createVehicleFootprints(const PathWithLaneId & path);
 
   static std::vector<LinearRing2d> createVehiclePassingAreas(
     const std::vector<LinearRing2d> & vehicle_footprints);

diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp
@@ -144,6 +144,14 @@ Output LaneDepartureChecker::update(const Input & input)
   return output;
 }
 
+bool LaneDepartureChecker::checkPathWillLeaveLane(
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path)
+{
+  std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
+  lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
+  return willLeaveLane(candidate_lanelets, vehicle_footprints);
+}
+
 PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
   const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double yaw_threshold)
 {
@@ -234,6 +242,21 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
   return vehicle_footprints;
 }
 
+std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path)
+{
+  // Create vehicle footprint in base_link coordinate
+  const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_);
+
+  // Create vehicle footprint on each Path point
+  std::vector<LinearRing2d> vehicle_footprints;
+  for (const auto & p : path.points) {
+    vehicle_footprints.push_back(
+      transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(p.point.pose)));
+  }
+
+  return vehicle_footprints;
+}
+
 std::vector<LinearRing2d> LaneDepartureChecker::createVehiclePassingAreas(
   const std::vector<LinearRing2d> & vehicle_footprints)
 {