From d3ccde7811737528eb3583b2bf06879d69ca7994 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Fri, 8 Jul 2022 15:46:31 +0900
Subject: [PATCH] feat(lane_departure_checker): support path_with_lane_id
 (#1279)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../lane_departure_checker.hpp                | 12 ++++++++++
 .../lane_departure_checker.cpp                | 23 +++++++++++++++++++
 2 files changed, 35 insertions(+)

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
index 3c16b4db06dec..369bc03144efe 100644
--- 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
index ad3fca6f8504e..a4bf3b7578ea4 100644
--- 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)
 {