Merge pull request #809 from tier4/sync-upstream

chore: sync upstream

control/lane_departure_checker/README.md

@@ -63,15 +63,30 @@ 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         |
-| 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         |
+#### General Parameters
+
+| Name                       | Type   | Description                                                                                                 | Default value |
+| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ |
+| will_out_of_lane_checker   | bool   | Enable checker whether ego vehicle footprint will depart from lane                                          | True          |
+| out_of_lane_checker        | bool   | Enable checker whether ego vehicle footprint is out of lane                                                 | True          |
+| boundary_departure_checker | bool   | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False         |
+| update_rate                | double | Frequency for publishing [Hz]                                                                               | 10.0          |
+| visualize_lanelet          | bool   | Flag for visualizing lanelet                                                                                | False         |
+
+#### Parameters For Lane Departure
+
+| Name                      | Type | Description                                       | Default value |
+| :------------------------ | :--- | :------------------------------------------------ | :------------ |
+| 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         |
+
+#### Parameters For Road Border Departure
+
+| Name                     | Type                       | Description                                                 | Default value |
+| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ |
+| boundary_types_to_detect | std::vector\<std::string\> | line_string types to detect with boundary_departure_checker | [road_border] |
 
 ### Core Parameters
 

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -1,13 +1,19 @@
 /**:
   ros__parameters:
+    # Enable feature
+    will_out_of_lane_checker: true
+    out_of_lane_checker: true
+    boundary_departure_checker: false
+
     # Node
     update_rate: 10.0
     visualize_lanelet: false
     include_right_lanes: false
     include_left_lanes: false
     include_opposite_lanes: false
     include_conflicting_lanes: false
-    road_border_departure_checker: false
+    boundary_types_to_detect: [road_border]
+
 
     # Core
     footprint_margin_scale: 1.0

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -72,14 +72,15 @@ struct Input
   lanelet::ConstLanelets shoulder_lanelets{};
   Trajectory::ConstSharedPtr reference_trajectory{};
   Trajectory::ConstSharedPtr predicted_trajectory{};
+  std::vector<std::string> boundary_types_to_detect{};
 };
 
 struct Output
 {
   std::map<std::string, double> processing_time_map{};
   bool will_leave_lane{};
   bool is_out_of_lane{};
-  bool will_cross_road_border{};
+  bool will_cross_boundary{};
   PoseDeviation trajectory_deviation{};
   lanelet::ConstLanelets candidate_lanelets{};
   TrajectoryPoints resampled_trajectory{};
@@ -136,9 +137,10 @@ class LaneDepartureChecker
     const lanelet::ConstLanelets & candidate_lanelets,
     const std::vector<LinearRing2d> & vehicle_footprints);
 
-  static bool willCrossRoadBorder(
+  static bool willCrossBoundary(
     const lanelet::ConstLanelets & candidate_lanelets,
-    const std::vector<LinearRing2d> & vehicle_footprints);
+    const std::vector<LinearRing2d> & vehicle_footprints,
+    const std::vector<std::string> & boundary_types_to_detects);
 
   static bool isCrossingRoadBorder(
     const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints);

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp

@@ -37,6 +37,7 @@
 
 #include <map>
 #include <memory>
+#include <string>
 #include <vector>
 
 namespace lane_departure_checker
@@ -45,13 +46,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin;
 
 struct NodeParam
 {
+  bool will_out_of_lane_checker;
+  bool out_of_lane_checker;
+  bool boundary_departure_checker;
+
   double update_rate;
   bool visualize_lanelet;
   bool include_right_lanes;
   bool include_left_lanes;
   bool include_opposite_lanes;
   bool include_conflicting_lanes;
-  bool road_border_departure_checker;
+  std::vector<std::string> boundary_types_to_detect;
 };
 
 class LaneDepartureCheckerNode : public rclcpp::Node

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -67,19 +67,19 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2
   return false;
 }
 
-bool isCrossingWithRoadBorder(
-  const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints)
+bool isCrossingWithBoundary(
+  const lanelet::BasicLineString2d & boundary, 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);
+      for (size_t i = 0; i < boundary.size() - 1; ++i) {
+        auto boundary1 = boundary.at(i);
+        auto boundary2 = boundary.at(i + 1);
         if (tier4_autoware_utils::intersect(
               tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2),
-              fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) {
+              fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) {
           return true;
         }
       }
@@ -172,9 +172,9 @@ 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);
+  output.will_cross_boundary = willCrossBoundary(
+    output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect);
+  output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true);
 
   return output;
 }
@@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane(
   return false;
 }
 
-bool LaneDepartureChecker::willCrossRoadBorder(
+bool LaneDepartureChecker::willCrossBoundary(
   const lanelet::ConstLanelets & candidate_lanelets,
-  const std::vector<LinearRing2d> & vehicle_footprints)
+  const std::vector<LinearRing2d> & vehicle_footprints,
+  const std::vector<std::string> & boundary_types_to_detect)
 {
   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(
+    if (
+      std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) !=
+      boundary_types_to_detect.end()) {
+      if (isCrossingWithBoundary(
             candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) {
         // std::cerr << "The crossed road_border's line string id: "
         //           << candidate_lanelet.rightBound().id() << std::endl;
@@ -351,8 +354,10 @@ bool LaneDepartureChecker::willCrossRoadBorder(
     }
     const std::string l_type =
       candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none");
-    if (l_type == "road_border") {
-      if (isCrossingWithRoadBorder(
+    if (
+      std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) !=
+      boundary_types_to_detect.end()) {
+      if (isCrossingWithBoundary(
             candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) {
         // std::cerr << "The crossed road_border's line string id: "
         //           << candidate_lanelet.leftBound().id() << std::endl;

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -125,15 +125,22 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
 {
   using std::placeholders::_1;
 
+  // Enable feature
+  node_param_.will_out_of_lane_checker = declare_parameter<bool>("will_out_of_lane_checker");
+  node_param_.out_of_lane_checker = declare_parameter<bool>("out_of_lane_checker");
+  node_param_.boundary_departure_checker = declare_parameter<bool>("boundary_departure_checker");
+
   // Node Parameter
   node_param_.update_rate = declare_parameter<double>("update_rate");
   node_param_.visualize_lanelet = declare_parameter<bool>("visualize_lanelet");
   node_param_.include_right_lanes = declare_parameter<bool>("include_right_lanes");
   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");
+
+  // Boundary_departure_checker
+  node_param_.boundary_types_to_detect =
+    declare_parameter<std::vector<std::string>>("boundary_types_to_detect");
 
   // Vehicle Info
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -338,6 +345,7 @@ void LaneDepartureCheckerNode::onTimer()
   input_.shoulder_lanelets = shoulder_lanelets_;
   input_.reference_trajectory = reference_trajectory_;
   input_.predicted_trajectory = predicted_trajectory_;
+  input_.boundary_types_to_detect = node_param_.boundary_types_to_detect;
   processing_time_map["Node: setInputData"] = stop_watch.toc(true);
 
   output_ = lane_departure_checker_->update(input_);
@@ -377,12 +385,19 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
   result.reason = "success";
 
   try {
+    // Enable feature
+    update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker);
+    update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker);
+    update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker);
+
     // Node
     update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet);
     update_param(parameters, "include_right_lanes", node_param_.include_right_lanes);
     update_param(parameters, "include_left_lanes", node_param_.include_left_lanes);
     update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes);
     update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes);
+    update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker);
+    update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect);
 
     // Core
     update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale);
@@ -409,19 +424,19 @@ void LaneDepartureCheckerNode::checkLaneDeparture(
   int8_t level = DiagStatus::OK;
   std::string msg = "OK";
 
-  if (output_.will_leave_lane) {
+  if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) {
     level = DiagStatus::WARN;
     msg = "vehicle will leave lane";
   }
 
-  if (output_.is_out_of_lane) {
+  if (output_.is_out_of_lane && node_param_.out_of_lane_checker) {
     level = DiagStatus::ERROR;
     msg = "vehicle is out of lane";
   }
 
-  if (output_.will_cross_road_border && node_param_.road_border_departure_checker) {
+  if (output_.will_cross_boundary && node_param_.boundary_departure_checker) {
     level = DiagStatus::ERROR;
-    msg = "vehicle will cross road border";
+    msg = "vehicle will cross boundary";
   }
 
   stat.summary(level, msg);