Merge pull request #729 from tier4/feature/add-road-boarder-deparcher…

…-checker

feat(lane_departure_checker): add road boarder deparcher checker

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

@@ -770,6 +770,28 @@ inline bool isTwistCovarianceValid(
   return false;
 }
 
+// NOTE: much faster than boost::geometry::intersects()
+inline std::optional<geometry_msgs::msg::Point> intersect(
+  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
+  const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
+{
+  // calculate intersection point
+  const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y);
+  if (det == 0.0) {
+    return std::nullopt;
+  }
+
+  const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det;
+  const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det;
+  if (t < 0 || 1 < t || s < 0 || 1 < s) {
+    return std::nullopt;
+  }
+
+  geometry_msgs::msg::Point intersect_point;
+  intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
+  intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
+  return intersect_point;
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_

common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

@@ -1689,3 +1689,108 @@ TEST(geometry, isTwistCovarianceValid)
   twist_with_covariance.covariance.at(0) = 1.0;
   EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), true);
 }
+
+TEST(geometry, intersect)
+{
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::intersect;
+
+  {  // Normally crossing
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(-1.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_TRUE(result);
+    EXPECT_NEAR(result->x, 0.0, epsilon);
+    EXPECT_NEAR(result->y, 0.0, epsilon);
+    EXPECT_NEAR(result->z, 0.0, epsilon);
+  }
+
+  {  // No crossing
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(1.0, 0.0, 0.0);
+    const auto p4 = createPoint(3.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // One segment is the point on the other's segment
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, 0.0, 0.0);
+    const auto p4 = createPoint(0.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // One segment is the point not on the other's segment
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(1.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // Both segments are the points which are the same position
+    const auto p1 = createPoint(0.0, 0.0, 0.0);
+    const auto p2 = createPoint(0.0, 0.0, 0.0);
+    const auto p3 = createPoint(0.0, 0.0, 0.0);
+    const auto p4 = createPoint(0.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // Both segments are the points which are different position
+    const auto p1 = createPoint(0.0, 1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(1.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // Segments are the same
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, -1.0, 0.0);
+    const auto p4 = createPoint(0.0, 1.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // One's edge is on the other's segment
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_TRUE(result);
+    EXPECT_NEAR(result->x, 0.0, epsilon);
+    EXPECT_NEAR(result->y, 0.0, epsilon);
+    EXPECT_NEAR(result->z, 0.0, epsilon);
+  }
+
+  {  // One's edge is the same as the other's edge.
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, -1.0, 0.0);
+    const auto p4 = createPoint(2.0, -1.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_TRUE(result);
+    EXPECT_NEAR(result->x, 0.0, epsilon);
+    EXPECT_NEAR(result->y, -1.0, epsilon);
+    EXPECT_NEAR(result->z, 0.0, epsilon);
+  }
+}

control/lane_departure_checker/README.md

@@ -8,6 +8,7 @@ This package includes the following features:
 
 - **Lane Departure**: Check if ego vehicle is going to be out of lane boundaries based on output from control module (predicted trajectory).
 - **Trajectory Deviation**: Check if ego vehicle's pose does not deviate from the trajectory. Checking lateral, longitudinal and yaw deviation.
+- **Road Border Departure**: Check if ego vehicle's footprint, generated from the control's output, extends beyond the road border.
 
 ## Inner-workings / Algorithms
 
@@ -62,10 +63,15 @@ This package includes the following features:
 
 ### Node Parameters
 
-| Name              | Type   | Description                   | Default value |
-| :---------------- | :----- | :---------------------------- | :------------ |
-| update_rate       | double | Frequency for publishing [Hz] | 10.0          |
-| visualize_lanelet | bool   | Flag for visualizing lanelet  | False         |
+| Name                          | Type   | Description                                                   | Default value |
+| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ |
+| update_rate                   | double | Frequency for publishing [Hz]                                 | 10.0          |
+| visualize_lanelet             | bool   | Flag for visualizing lanelet                                  | False         |
+| include_right_lanes           | bool   | Flag for including right lanelet in borders                   | False         |
+| include_left_lanes            | bool   | Flag for including left lanelet in borders                    | False         |
+| include_opposite_lanes        | bool   | Flag for including opposite lanelet in borders                | False         |
+| include_conflicting_lanes     | bool   | Flag for including conflicting lanelet in borders             | False         |
+| road_border_departure_checker | bool   | Flag for checking if the vehicle will departs the road border | False         |
 
 ### Core Parameters
 

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -7,6 +7,7 @@
     include_left_lanes: false
     include_opposite_lanes: false
     include_conflicting_lanes: false
+    road_border_departure_checker: false
 
     # Core
     footprint_margin_scale: 1.0

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -80,6 +80,7 @@ struct Output
   std::map<std::string, double> processing_time_map{};
   bool will_leave_lane{};
   bool is_out_of_lane{};
+  bool will_cross_road_border{};
   PoseDeviation trajectory_deviation{};
   lanelet::ConstLanelets candidate_lanelets{};
   TrajectoryPoints resampled_trajectory{};
@@ -135,6 +136,13 @@ class LaneDepartureChecker
   static bool willLeaveLane(
     const lanelet::ConstLanelets & candidate_lanelets,
     const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static bool willCrossRoadBorder(
+    const lanelet::ConstLanelets & candidate_lanelets,
+    const std::vector<LinearRing2d> & vehicle_footprints);
+
+  static bool isCrossingRoadBorder(
+    const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints);
 };
 }  // namespace lane_departure_checker
 

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp

@@ -51,6 +51,7 @@ struct NodeParam
   bool include_left_lanes;
   bool include_opposite_lanes;
   bool include_conflicting_lanes;
+  bool road_border_departure_checker;
 };
 
 class LaneDepartureCheckerNode : public rclcpp::Node

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -39,6 +39,16 @@ namespace
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using geometry_msgs::msg::Point;
+
+Point fromVector2dToMsg(const Eigen::Vector2d & point)
+{
+  Point msg{};
+  msg.x = point.x();
+  msg.y = point.y();
+  msg.z = 0.0;
+  return msg;
+}
 
 double calcBrakingDistance(
   const double abs_velocity, const double max_deceleration, const double delay_time)
@@ -57,6 +67,27 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2
   return false;
 }
 
+bool isCrossingWithRoadBorder(
+  const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints)
+{
+  for (auto & footprint : footprints) {
+    for (size_t i = 0; i < footprint.size() - 1; ++i) {
+      auto footprint1 = footprint.at(i).to_3d();
+      auto footprint2 = footprint.at(i + 1).to_3d();
+      for (size_t i = 0; i < road_border.size() - 1; ++i) {
+        auto road_border1 = road_border.at(i);
+        auto road_border2 = road_border.at(i + 1);
+        if (tier4_autoware_utils::intersect(
+              tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2),
+              fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) {
+          return true;
+        }
+      }
+    }
+  }
+  return false;
+}
+
 LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprints)
 {
   MultiPoint2d combined;
@@ -141,6 +172,10 @@ Output LaneDepartureChecker::update(const Input & input)
   output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front());
   output.processing_time_map["isOutOfLane"] = stop_watch.toc(true);
 
+  output.will_cross_road_border =
+    willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints);
+  output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true);
+
   return output;
 }
 
@@ -298,4 +333,34 @@ bool LaneDepartureChecker::isOutOfLane(
 
   return false;
 }
+
+bool LaneDepartureChecker::willCrossRoadBorder(
+  const lanelet::ConstLanelets & candidate_lanelets,
+  const std::vector<LinearRing2d> & vehicle_footprints)
+{
+  for (const auto & candidate_lanelet : candidate_lanelets) {
+    const std::string r_type =
+      candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none");
+    if (r_type == "road_border") {
+      if (isCrossingWithRoadBorder(
+            candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) {
+        // std::cerr << "The crossed road_border's line string id: "
+        //           << candidate_lanelet.rightBound().id() << std::endl;
+        return true;
+      }
+    }
+    const std::string l_type =
+      candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none");
+    if (l_type == "road_border") {
+      if (isCrossingWithRoadBorder(
+            candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) {
+        // std::cerr << "The crossed road_border's line string id: "
+        //           << candidate_lanelet.leftBound().id() << std::endl;
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
 }  // namespace lane_departure_checker

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -132,6 +132,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   node_param_.include_left_lanes = declare_parameter<bool>("include_left_lanes");
   node_param_.include_opposite_lanes = declare_parameter<bool>("include_opposite_lanes");
   node_param_.include_conflicting_lanes = declare_parameter<bool>("include_conflicting_lanes");
+  node_param_.road_border_departure_checker =
+    declare_parameter<bool>("road_border_departure_checker");
 
   // Vehicle Info
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -417,6 +419,11 @@ void LaneDepartureCheckerNode::checkLaneDeparture(
     msg = "vehicle is out of lane";
   }
 
+  if (output_.will_cross_road_border && node_param_.road_border_departure_checker) {
+    level = DiagStatus::ERROR;
+    msg = "vehicle will cross road border";
+  }
+
   stat.summary(level, msg);
 }