feat(intersection): generate detection lane using RightOfWay and traffic light arrow

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -26,6 +26,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include <map>
 #include <memory>
 #include <optional>
 #include <string>
@@ -47,11 +48,10 @@ bool getDuplicatedPointIdx(
 /**
  * @brief get objective polygons for detection area
  */
-bool getObjectiveLanelets(
+bool getDetectionLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const int lane_id, const double detection_area_length,
-  std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
-  lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger);
+  lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on = false);
 
 struct StopLineIdx
 {
@@ -149,6 +149,10 @@ std::vector<int> extendedAdjacentDirectionLanes(
 std::optional<Polygon2d> getIntersectionArea(
   lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);
 
+bool isTrafficLightArrowActivated(
+  lanelet::ConstLanelet lane,
+  const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos);
+
 }  // namespace util
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -91,19 +91,18 @@ bool IntersectionModule::modifyPathVelocity(
   const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
   const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
-  /* get detection area and conflicting area */
-  lanelet::ConstLanelets detection_area_lanelets;
-  std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
+  const auto & assigned_lanelet =
+    planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_);
 
-  util::getObjectiveLanelets(
+  /* get detection area*/
+  lanelet::ConstLanelets detection_area_lanelets;
+  const bool tl_arrow_solid_on =
+    util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
+  util::getDetectionLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
-  std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
-    conflicting_area_lanelets, planner_param_.detection_area_length);
+    &detection_area_lanelets, tl_arrow_solid_on);
   std::vector<lanelet::CompoundPolygon3d> detection_areas =
     util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
-  std::vector<int> conflicting_area_lanelet_ids =
-    util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets);
   std::vector<int> detection_area_lanelet_ids =
     util::getLaneletIdsFromLaneletsVec({detection_area_lanelets});
 
@@ -116,8 +115,6 @@ bool IntersectionModule::modifyPathVelocity(
   debug_data_.detection_area = detection_areas;
 
   /* get intersection area */
-  const auto & assigned_lanelet =
-    planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_);
   const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr);
   if (intersection_area) {
     const auto intersection_area_2d = intersection_area.value();
@@ -136,7 +133,7 @@ bool IntersectionModule::modifyPathVelocity(
   /* set stop-line and stop-judgement-line for base_link */
   util::StopLineIdx stop_line_idxs;
   if (!util::generateStopLine(
-        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin,
+        lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin,
         planner_param_.keep_detection_line_margin, path, *path, &stop_line_idxs,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -67,25 +67,23 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
 
   /* get detection area */
   lanelet::ConstLanelets detection_area_lanelets;
-  std::vector<lanelet::ConstLanelets> conflicting_area_lanelets;
-
-  util::getObjectiveLanelets(
+  util::getDetectionLanelets(
     lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length,
-    &conflicting_area_lanelets, &detection_area_lanelets, logger_);
-  std::vector<lanelet::CompoundPolygon3d> conflicting_areas = util::getPolygon3dFromLaneletsVec(
-    conflicting_area_lanelets, planner_param_.detection_area_length);
-  if (conflicting_areas.empty()) {
+    &detection_area_lanelets);
+  std::vector<lanelet::CompoundPolygon3d> detection_areas =
+    util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length);
+  if (detection_areas.empty()) {
     RCLCPP_DEBUG(logger_, "no detection area. skip computation.");
     return true;
   }
-  debug_data_.detection_area = conflicting_areas;
+  debug_data_.detection_area = detection_areas;
 
   /* set stop-line and stop-judgement-line for base_link */
   util::StopLineIdx stop_line_idxs;
   const auto private_path =
     extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m);
   if (!util::generateStopLine(
-        lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin,
+        lane_id_, detection_areas, planner_data_, planner_param_.stop_line_margin,
         0.0 /* unnecessary in merge_from_private */, path, private_path, &stop_line_idxs,
         logger_.get_child("util"))) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail");

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -309,14 +309,23 @@ bool getStopPoseIndexFromMap(
   return true;
 }
 
-bool getObjectiveLanelets(
+bool getDetectionLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const int lane_id, const double detection_area_length,
-  std::vector<lanelet::ConstLanelets> * conflicting_lanelets_result,
-  lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger)
+  lanelet::ConstLanelets * detection_lanelets_result, const bool tl_arrow_solid_on)
 {
   const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);
 
+  const auto tl_regelems = assigned_lanelet.regulatoryElementsAs<lanelet::TrafficLight>();
+  bool has_tl = false;
+  if (tl_regelems.size() != 0) {
+    const auto tl_regelem = tl_regelems.front();
+    const auto stop_line_opt = tl_regelem->stopLine();
+    if (!!stop_line_opt) has_tl = true;
+  }
+
+  const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");
+
   lanelet::ConstLanelets yield_lanelets;
 
   // for low priority lane
@@ -351,72 +360,49 @@ bool getObjectiveLanelets(
   const auto & conflicting_lanelets =
     lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet);
 
-  std::vector<lanelet::ConstLanelets>                      // conflicting lanes with "lane_id"
-    conflicting_lanelets_ex_yield_ego;                     // excluding ego lanes and yield lanes
-  std::vector<lanelet::ConstLanelets> objective_lanelets;  // final objective lanelets
+  lanelet::ConstLanelets detection_lanelets;  // final objective lanelets
 
-  // exclude yield lanelets and ego lanelets from objective_lanelets
-  for (const auto & conflicting_lanelet : conflicting_lanelets) {
-    if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) {
-      continue;
-    }
-    if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) {
-      continue;
+  // exclude yield lanelets and ego lanelets from detection_lanelets
+  // if assigned lanelet is "straight" with traffic light, detection area is not necessary
+  if (turn_direction == std::string("straight") && has_tl) {
+  } else {
+    // otherwise we need to know the priority from RightOfWay
+    for (const auto & conflicting_lanelet : conflicting_lanelets) {
+      if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) {
+        continue;
+      }
+      if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) {
+        continue;
+      }
+      detection_lanelets.push_back(conflicting_lanelet);
     }
-    conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet});
-    objective_lanelets.push_back({conflicting_lanelet});
   }
 
   // get possible lanelet path that reaches conflicting_lane longer than given length
-  const double length = detection_area_length;
-  lanelet::ConstLanelets objective_and_preceding_lanelets;
-  std::set<lanelet::Id> objective_ids;
-  for (const auto & ll : objective_lanelets) {
-    // Preceding lanes does not include objective_lane so add them at the end
-    for (const auto & l : ll) {
-      const auto & inserted = objective_ids.insert(l.id());
-      if (inserted.second) objective_and_preceding_lanelets.push_back(l);
-    }
-    // get preceding lanelets without ego_lanelets
-    // to prevent the detection area from including the ego lanes and its' preceding lanes.
-    const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences(
-      routing_graph_ptr, ll.front(), length, ego_lanelets);
-    for (const auto & ls : lanelet_sequences) {
-      for (const auto & l : ls) {
-        const auto & inserted = objective_ids.insert(l.id());
-        if (inserted.second) objective_and_preceding_lanelets.push_back(l);
+  // if traffic light arrow is active, this process is unnecessary
+  if (!tl_arrow_solid_on) {
+    const double length = detection_area_length;
+    lanelet::ConstLanelets detection_and_preceding_lanelets;
+    std::set<lanelet::Id> detection_ids;
+    for (const auto & ll : detection_lanelets) {
+      // Preceding lanes does not include detection_lane so add them at the end
+      const auto & inserted = detection_ids.insert(ll.id());
+      if (inserted.second) detection_and_preceding_lanelets.push_back(ll);
+      // get preceding lanelets without ego_lanelets
+      // to prevent the detection area from including the ego lanes and its' preceding lanes.
+      const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences(
+        routing_graph_ptr, ll, length, ego_lanelets);
+      for (const auto & ls : lanelet_sequences) {
+        for (const auto & l : ls) {
+          const auto & inserted = detection_ids.insert(l.id());
+          if (inserted.second) detection_and_preceding_lanelets.push_back(l);
+        }
       }
     }
+    *detection_lanelets_result = detection_and_preceding_lanelets;
+  } else {
+    *detection_lanelets_result = detection_lanelets;
   }
-
-  *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego;
-  *objective_lanelets_result = objective_and_preceding_lanelets;
-
-  // set this flag true when debugging
-  const bool is_debug = false;
-  if (!is_debug) return true;
-  std::stringstream ss_c, ss_y, ss_e, ss_o, ss_os;
-  for (const auto & l : conflicting_lanelets) {
-    ss_c << l.id() << ", ";
-  }
-  for (const auto & l : yield_lanelets) {
-    ss_y << l.id() << ", ";
-  }
-  for (const auto & l : ego_lanelets) {
-    ss_e << l.id() << ", ";
-  }
-  for (const auto & l : objective_lanelets) {
-    ss_o << l.front().id() << ", ";
-  }
-  for (const auto & l : objective_and_preceding_lanelets) {
-    ss_os << l.id() << ", ";
-  }
-  RCLCPP_INFO(
-    logger, "getObjectiveLanelets() conflict = %s yield = %s ego = %s", ss_c.str().c_str(),
-    ss_y.str().c_str(), ss_e.str().c_str());
-  RCLCPP_INFO(
-    logger, "getObjectiveLanelets() object = %s object_sequences = %s", ss_o.str().c_str(),
-    ss_os.str().c_str());
   return true;
 }
 
@@ -676,5 +662,40 @@ std::optional<Polygon2d> getIntersectionArea(
   return std::make_optional(poly);
 }
 
+bool isTrafficLightArrowActivated(
+  lanelet::ConstLanelet lane,
+  const std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> & tl_infos)
+{
+  const auto & turn_direction = lane.attributeOr("turn_direction", "else");
+  boost::optional<int> tl_id = boost::none;
+  for (auto && tl_regelem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
+    tl_id = tl_regelem->id();
+    break;
+  }
+  if (!tl_id) {
+    // this lane has no traffic light
+    return false;
+  }
+  const auto tl_info_it = tl_infos.find(tl_id.get());
+  if (tl_info_it == tl_infos.end()) {
+    // the info of this traffic light is not available
+    return false;
+  }
+  const auto & tl_info = tl_info_it->second;
+  for (auto && tl_light : tl_info.signal.lights) {
+    if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue;
+    if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue;
+    if (
+      turn_direction == std::string("left") &&
+      tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)
+      return true;
+    if (
+      turn_direction == std::string("right") &&
+      tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)
+      return true;
+  }
+  return false;
+}
+
 }  // namespace util
 }  // namespace behavior_velocity_planner