Merge pull request autowarefoundation#6 from Robicarr-Karsan/test/bnk…

…/outdoor-avoidance

feat(behavior_path_planner): execute avoidance module with avoidance tag

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp

@@ -92,6 +92,14 @@ lanelet::ConstLanelets getExtendLanes(
   const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
   const std::shared_ptr<const PlannerData> & planner_data);
 
+boost::optional<int64_t> getNearestLaneId(
+  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
+  const geometry_msgs::msg::Pose & current_pose);
+
+std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
+  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
+  const geometry_msgs::msg::Pose & current_pose);
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   std::optional<Pose> & p_out);

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -1293,6 +1293,77 @@ lanelet::ConstLanelets getExtendLanes(
   return extend_lanelets;
 }
 
+std::vector<int64_t> getSortedLaneIdsFromPath(const PathWithLaneId & path)
+{
+  std::vector<int64_t> sorted_lane_ids;
+  for (const auto & path_points : path.points) {
+    for (const auto lane_id : path_points.lane_ids)
+      if (
+        std::find(sorted_lane_ids.begin(), sorted_lane_ids.end(), lane_id) ==
+        sorted_lane_ids.end()) {
+        sorted_lane_ids.emplace_back(lane_id);
+      }
+  }
+  return sorted_lane_ids;
+}
+std::vector<int64_t> getSubsequentLaneIdsSetOnPath(
+  const PathWithLaneId & path, int64_t base_lane_id)
+{
+  const auto all_lane_ids = getSortedLaneIdsFromPath(path);
+  const auto base_index = std::find(all_lane_ids.begin(), all_lane_ids.end(), base_lane_id);
+
+  // cannot find base_index in all_lane_ids
+  if (base_index == all_lane_ids.end()) {
+    return std::vector<int64_t>();
+  }
+
+  std::vector<int64_t> subsequent_lane_ids;
+
+  std::copy(base_index, all_lane_ids.end(), std::back_inserter(subsequent_lane_ids));
+  return subsequent_lane_ids;
+}
+
+boost::optional<int64_t> getNearestLaneId(
+  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
+  const geometry_msgs::msg::Pose & current_pose)
+{
+  lanelet::ConstLanelets lanes;
+  const auto lane_ids = getSortedLaneIdsFromPath(path);
+  for (const auto & lane_id : lane_ids) {
+    lanes.push_back(lanelet_map->laneletLayer.get(lane_id));
+  }
+
+  lanelet::Lanelet closest_lane;
+  if (lanelet::utils::query::getClosestLanelet(lanes, current_pose, &closest_lane)) {
+    return closest_lane.id();
+  }
+  return boost::none;
+}
+
+std::vector<lanelet::ConstLanelet> getLaneletsOnPath(
+  const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map,
+  const geometry_msgs::msg::Pose & current_pose)
+{
+  const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose);
+
+  std::vector<int64_t> unique_lane_ids;
+  if (nearest_lane_id) {
+    // Add subsequent lane_ids from nearest lane_id
+    unique_lane_ids = avoidance::getSubsequentLaneIdsSetOnPath(
+      path, *nearest_lane_id);
+  } else {
+    // Add all lane_ids in path
+    unique_lane_ids = avoidance::getSortedLaneIdsFromPath(path);
+  }
+
+  std::vector<lanelet::ConstLanelet> lanelets;
+  for (const auto lane_id : unique_lane_ids) {
+    lanelets.push_back(lanelet_map->laneletLayer.get(lane_id));
+  }
+
+  return lanelets;
+}
+
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   std::optional<Pose> & p_out)
@@ -1608,6 +1679,24 @@ void compensateDetectionLost(
     }
   }
 }
+void push_target_object(const std::shared_ptr<const PlannerData>& planner_data, ObjectData & object, AvoidancePlanningData & data)
+{
+  const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();
+
+  const auto routing_graph = planner_data->route_handler->getRoutingGraphPtr();
+  const auto lanelet_map = planner_data->route_handler->getLaneletMapPtr();
+
+  const auto target_object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
+  const auto target_object_lanelets = utils::avoidance::getLaneletsOnPath(data.reference_path, lanelet_map, target_object_pose);
+  const auto object_lane_id = behavior_path_planner::utils::avoidance::getNearestLaneId(data.reference_path, lanelet_map, target_object_pose);
+  for (const auto &object_lane : target_object_lanelets) {
+    const std::string is_avoidance_enable = object_lane.attributeOr("avoidance", "else");
+    if (is_avoidance_enable == "yes" && object_lane_id.get() == object_lane.id()) {
+      object.last_seen = now;
+      data.target_objects.push_back(object);
+    }
+  }
+}
 
 void filterTargetObjects(
   ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
@@ -1618,12 +1707,6 @@ void filterTargetObjects(
     return;
   }
 
-  const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();
-  const auto push_target_object = [&data, &now](auto & object) {
-    object.last_seen = now;
-    data.target_objects.push_back(object);
-  };
-
   for (auto & o : objects) {
     if (!filtering_utils::isSatisfiedWithCommonCondition(
           o, data, forward_detection_range, planner_data, parameters)) {
@@ -1663,8 +1746,7 @@ void filterTargetObjects(
         continue;
       }
     }
-
-    push_target_object(o);
+    push_target_object(planner_data, o, data);
   }
 }