feat(lane_departure_checker): include neighbor lanelets as optional

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -3,6 +3,10 @@
     # Node
     update_rate: 10.0
     visualize_lanelet: false
+    include_right_lanes: false
+    include_left_lanes: false
+    include_opposite_lanes: false
+    include_conflicting_lanes: false
 
     # Core
     footprint_margin_scale: 1.0

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp

@@ -19,6 +19,7 @@
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/ros/processing_time_publisher.hpp>
@@ -35,6 +36,7 @@
 
 #include <lanelet2_core/LaneletMap.h>
 
+#include <map>
 #include <memory>
 #include <vector>
 
@@ -46,6 +48,10 @@ struct NodeParam
 {
   double update_rate;
   bool visualize_lanelet;
+  bool include_right_lanes;
+  bool include_left_lanes;
+  bool include_opposite_lanes;
+  bool include_conflicting_lanes;
 };
 
 class LaneDepartureCheckerNode : public rclcpp::Node
@@ -118,6 +124,27 @@ class LaneDepartureCheckerNode : public rclcpp::Node
 
   // Visualization
   visualization_msgs::msg::MarkerArray createMarkerArray() const;
+
+  // Lanelet Neighbor Search
+  lanelet::ConstLanelets getAllSharedLineStringLanelets(
+    const lanelet::ConstLanelet & current_lane, const bool is_right, const bool is_left,
+    const bool is_opposite, const bool is_conflicting, const bool & invert_opposite);
+
+  lanelet::ConstLanelets getAllRightSharedLinestringLanelets(
+    const lanelet::ConstLanelet & lane, const bool & include_opposite,
+    const bool & invert_opposite = false);
+
+  lanelet::ConstLanelets getAllLeftSharedLinestringLanelets(
+    const lanelet::ConstLanelet & lane, const bool & include_opposite,
+    const bool & invert_opposite = false);
+
+  boost::optional<lanelet::ConstLanelet> getLeftLanelet(const lanelet::ConstLanelet & lanelet);
+
+  lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet);
+  boost::optional<lanelet::ConstLanelet> getRightLanelet(
+    const lanelet::ConstLanelet & lanelet) const;
+
+  lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet);
 };
 }  // namespace lane_departure_checker
 

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -51,13 +51,13 @@ std::array<geometry_msgs::msg::Point, 3> triangle2points(
   return points;
 }
 
-lanelet::ConstLanelets getRouteLanelets(
+std::map<int, lanelet::ConstLanelet> getRouteLanelets(
   const lanelet::LaneletMapPtr & lanelet_map,
   const lanelet::routing::RoutingGraphPtr & routing_graph,
   const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr,
   const double vehicle_length)
 {
-  lanelet::ConstLanelets route_lanelets;
+  std::map<int, lanelet::ConstLanelet> route_lanelets;
 
   bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map);
   if (!is_route_valid) {
@@ -73,7 +73,7 @@ lanelet::ConstLanelets getRouteLanelets(
       for (const auto & lanelet_sequence : lanelet::utils::query::getPrecedingLaneletSequences(
              routing_graph, lanelet_map->laneletLayer.get(lane_id), extension_length)) {
         for (const auto & preceding_lanelet : lanelet_sequence) {
-          route_lanelets.push_back(preceding_lanelet);
+          route_lanelets[preceding_lanelet.id()] = preceding_lanelet;
         }
       }
     }
@@ -82,7 +82,7 @@ lanelet::ConstLanelets getRouteLanelets(
   for (const auto & route_section : route_ptr->segments) {
     for (const auto & primitive : route_section.primitives) {
       const auto lane_id = primitive.id;
-      route_lanelets.push_back(lanelet_map->laneletLayer.get(lane_id));
+      route_lanelets[lane_id] = lanelet_map->laneletLayer.get(lane_id);
     }
   }
 
@@ -95,7 +95,7 @@ lanelet::ConstLanelets getRouteLanelets(
       for (const auto & lanelet_sequence : lanelet::utils::query::getSucceedingLaneletSequences(
              routing_graph, lanelet_map->laneletLayer.get(lane_id), extension_length)) {
         for (const auto & succeeding_lanelet : lanelet_sequence) {
-          route_lanelets.push_back(succeeding_lanelet);
+          route_lanelets[succeeding_lanelet.id()] = succeeding_lanelet;
         }
       }
     }
@@ -128,13 +128,16 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   // Node Parameter
   node_param_.update_rate = declare_parameter("update_rate", 10.0);
   node_param_.visualize_lanelet = declare_parameter("visualize_lanelet", false);
-
-  // Core Parameter
+  node_param_.include_right_lanes = declare_parameter("include_right_lanes", false);
+  node_param_.include_left_lanes = declare_parameter("include_left_lanes", false);
+  node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes", false);
+  node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes", false);
 
   // Vehicle Info
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
   vehicle_length_m_ = vehicle_info.vehicle_length_m;
 
+  // Core Parameter
   param_.footprint_margin_scale = declare_parameter("footprint_margin_scale", 1.0);
   param_.resample_interval = declare_parameter("resample_interval", 0.3);
   param_.max_deceleration = declare_parameter("max_deceleration", 3.0);
@@ -307,7 +310,27 @@ void LaneDepartureCheckerNode::onTimer()
 
   // In order to wait for both of map and route will be ready, write this not in callback but here
   if (last_route_ != route_ && !route_->segments.empty()) {
-    route_lanelets_ = getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_);
+    std::map<int, lanelet::ConstLanelet>::iterator itr;
+
+    auto map_route_lanelets_ =
+      getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_);
+
+    lanelet::ConstLanelets shared_lanelets_tmp;
+
+    for (itr = map_route_lanelets_.begin(); itr != map_route_lanelets_.end(); ++itr) {
+      const auto shared_lanelet = getAllSharedLineStringLanelets(
+        itr->second, node_param_.include_right_lanes, node_param_.include_left_lanes,
+        node_param_.include_opposite_lanes, node_param_.include_conflicting_lanes, true);
+      shared_lanelets_tmp.insert(
+        shared_lanelets_tmp.end(), shared_lanelet.begin(), shared_lanelet.end());
+    }
+    for (const auto & lanelet : shared_lanelets_tmp) {
+      map_route_lanelets_[lanelet.id()] = lanelet;
+    }
+    route_lanelets_.clear();
+    for (itr = map_route_lanelets_.begin(); itr != map_route_lanelets_.end(); ++itr) {
+      route_lanelets_.push_back(itr->second);
+    }
     last_route_ = route_;
   }
   processing_time_map["Node: getRouteLanelets"] = stop_watch.toc(true);
@@ -361,6 +384,10 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(
   try {
     // 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);
 
     // Core
     update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale);
@@ -553,6 +580,165 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray
 
   return marker_array;
 }
+
+lanelet::ConstLanelets LaneDepartureCheckerNode::getAllSharedLineStringLanelets(
+  const lanelet::ConstLanelet & current_lane, const bool is_right, const bool is_left,
+  const bool is_opposite, const bool is_conflicting, const bool & invert_opposite)
+{
+  lanelet::ConstLanelets shared{current_lane};
+
+  if (is_right) {
+    const lanelet::ConstLanelets all_right_lanelets =
+      getAllRightSharedLinestringLanelets(current_lane, is_opposite, invert_opposite);
+    shared.insert(shared.end(), all_right_lanelets.begin(), all_right_lanelets.end());
+  }
+
+  if (is_left) {
+    const lanelet::ConstLanelets all_left_lanelets =
+      getAllLeftSharedLinestringLanelets(current_lane, is_opposite, invert_opposite);
+    shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end());
+  }
+
+  if (is_conflicting) {
+    const auto conflicting_lanelets =
+      lanelet::utils::getConflictingLanelets(routing_graph_, current_lane);
+    shared.insert(shared.end(), conflicting_lanelets.begin(), conflicting_lanelets.end());
+  }
+  return shared;
+}
+
+lanelet::ConstLanelets LaneDepartureCheckerNode::getAllRightSharedLinestringLanelets(
+  const lanelet::ConstLanelet & lane, const bool & include_opposite, const bool & invert_opposite)
+{
+  lanelet::ConstLanelets linestring_shared;
+  auto lanelet_at_right = getRightLanelet(lane);
+  auto lanelet_at_right_opposite = getRightOppositeLanelets(lane);
+  while (lanelet_at_right) {
+    linestring_shared.push_back(lanelet_at_right.get());
+    lanelet_at_right = getRightLanelet(lanelet_at_right.get());
+    if (!lanelet_at_right) {
+      break;
+    }
+    lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get());
+  }
+
+  if (!lanelet_at_right_opposite.empty() && include_opposite) {
+    if (invert_opposite) {
+      linestring_shared.push_back(lanelet_at_right_opposite.front().invert());
+    } else {
+      linestring_shared.push_back(lanelet_at_right_opposite.front());
+    }
+    auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front());
+    while (lanelet_at_left) {
+      if (invert_opposite) {
+        linestring_shared.push_back(lanelet_at_left.get().invert());
+      } else {
+        linestring_shared.push_back(lanelet_at_left.get());
+      }
+      lanelet_at_left = getLeftLanelet(lanelet_at_left.get());
+    }
+  }
+  return linestring_shared;
+}
+
+lanelet::ConstLanelets LaneDepartureCheckerNode::getAllLeftSharedLinestringLanelets(
+  const lanelet::ConstLanelet & lane, const bool & include_opposite, const bool & invert_opposite)
+{
+  lanelet::ConstLanelets linestring_shared;
+  auto lanelet_at_left = getLeftLanelet(lane);
+  auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane);
+  while (lanelet_at_left) {
+    linestring_shared.push_back(lanelet_at_left.get());
+    lanelet_at_left = getLeftLanelet(lanelet_at_left.get());
+    if (!lanelet_at_left) {
+      break;
+    }
+    lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get());
+  }
+
+  if (!lanelet_at_left_opposite.empty() && include_opposite) {
+    if (invert_opposite) {
+      linestring_shared.push_back(lanelet_at_left_opposite.front().invert());
+    } else {
+      linestring_shared.push_back(lanelet_at_left_opposite.front());
+    }
+    auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front());
+    while (lanelet_at_right) {
+      if (invert_opposite) {
+        linestring_shared.push_back(lanelet_at_right.get().invert());
+      } else {
+        linestring_shared.push_back(lanelet_at_right.get());
+      }
+      lanelet_at_right = getRightLanelet(lanelet_at_right.get());
+    }
+  }
+  return linestring_shared;
+}
+
+boost::optional<lanelet::ConstLanelet> LaneDepartureCheckerNode::getLeftLanelet(
+  const lanelet::ConstLanelet & lanelet)
+{
+  // routable lane
+  const auto & left_lane = routing_graph_->left(lanelet);
+  if (left_lane) {
+    return left_lane;
+  }
+
+  // non-routable lane (e.g. lane change infeasible)
+  const auto & adjacent_left_lane = routing_graph_->adjacentLeft(lanelet);
+  return adjacent_left_lane;
+}
+
+lanelet::Lanelets LaneDepartureCheckerNode::getLeftOppositeLanelets(
+  const lanelet::ConstLanelet & lanelet)
+{
+  const auto opposite_candidate_lanelets =
+    lanelet_map_->laneletLayer.findUsages(lanelet.leftBound().invert());
+
+  lanelet::Lanelets opposite_lanelets;
+  for (const auto & candidate_lanelet : opposite_candidate_lanelets) {
+    if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) {
+      continue;
+    }
+
+    opposite_lanelets.push_back(candidate_lanelet);
+  }
+
+  return opposite_lanelets;
+}
+
+boost::optional<lanelet::ConstLanelet> LaneDepartureCheckerNode::getRightLanelet(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  // routable lane
+  const auto & right_lane = routing_graph_->right(lanelet);
+  if (right_lane) {
+    return right_lane;
+  }
+
+  // non-routable lane (e.g. lane change infeasible)
+  const auto & adjacent_right_lane = routing_graph_->adjacentRight(lanelet);
+  return adjacent_right_lane;
+}
+
+lanelet::Lanelets LaneDepartureCheckerNode::getRightOppositeLanelets(
+  const lanelet::ConstLanelet & lanelet)
+{
+  const auto opposite_candidate_lanelets =
+    lanelet_map_->laneletLayer.findUsages(lanelet.rightBound().invert());
+
+  lanelet::Lanelets opposite_lanelets;
+  for (const auto & candidate_lanelet : opposite_candidate_lanelets) {
+    if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) {
+      continue;
+    }
+
+    opposite_lanelets.push_back(candidate_lanelet);
+  }
+
+  return opposite_lanelets;
+}
+
 }  // namespace lane_departure_checker
 
 #include <rclcpp_components/register_node_macro.hpp>