feat(behavior_path_planner): visualize drivable areas

The visualization visualize lanelet that share same linestrings

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -126,6 +126,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
     const PathWithLaneId & path) const;  // (TODO) move to util
 
   void clipPathLength(PathWithLaneId & path) const;  // (TODO) move to util
+  void visualizeDrivableAreasWithSharedLanelets();
 
   /**
    * @brief Execute behavior tree and publish planned data.
@@ -156,6 +157,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
 private:
   rclcpp::Publisher<OccupancyGrid>::SharedPtr debug_drivable_area_publisher_;
+  rclcpp::Publisher<OccupancyGrid>::SharedPtr debug_drivable_area_lanelets_publisher_;
   rclcpp::Publisher<Path>::SharedPtr debug_path_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
   void publishDebugMarker(const std::vector<MarkerArray> & debug_markers);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -71,6 +71,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     create_publisher<TurnIndicatorsCommand>("~/output/turn_indicators_cmd", 1);
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
   debug_drivable_area_publisher_ = create_publisher<OccupancyGrid>("~/debug/drivable_area", 1);
+  debug_drivable_area_lanelets_publisher_ =
+    create_publisher<OccupancyGrid>("~/drivable_area_lanelet", 1);
   debug_path_publisher_ = create_publisher<Path>("~/debug/path_for_visualize", 1);
 
   // For remote operation
@@ -499,6 +501,7 @@ void BehaviorPathPlannerNode::run()
 
   publishDebugMarker(bt_manager_->getDebugMarkers());
 
+  visualizeDrivableAreasWithSharedLanelets();
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
@@ -702,6 +705,80 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
   return refined_path;
 }
 
+void BehaviorPathPlannerNode::visualizeDrivableAreasWithSharedLanelets()
+{
+  [[maybe_unused]] const auto & route_handler = planner_data_->route_handler;
+  const auto ego_pose = planner_data_->self_pose->pose;
+  lanelet::ConstLanelet current_lane;
+  if (!route_handler->getClosestLaneletWithinRoute(ego_pose, &current_lane)) {
+    return;
+  }
+
+  lanelet::ConstLanelets current_lanes =
+    route_handler->getLaneletSequence(current_lane, ego_pose, 0.5, 3.0);
+
+  [[maybe_unused]] const auto searchLeftLaneletsAndAppendToDrivableAreas =
+    [&route_handler](
+      const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended,
+      bool extend_to_opposite_lane = true) noexcept {
+      auto lanelet_at_left = route_handler->getLeftLanelet(current_lanelet);
+      auto lanelet_at_left_opposite = route_handler->getLeftOppositeLanelets(current_lanelet);
+      while (lanelet_at_left) {
+        lanelet_to_be_extended.push_back(lanelet_at_left.get());
+        lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
+        lanelet_at_left_opposite = route_handler->getLeftOppositeLanelets(lanelet_at_left.get());
+      }
+
+      if (!lanelet_at_left_opposite.empty() && extend_to_opposite_lane) {
+        auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left_opposite.front());
+        while (lanelet_at_right) {
+          lanelet_to_be_extended.push_back(lanelet_at_right.get());
+          lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
+        }
+      }
+    };
+
+  [[maybe_unused]] const auto searchRightLaneletsAndAppendToDrivableAreas =
+    [&route_handler](
+      const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended,
+      bool extend_to_opposite_lane = true) noexcept {
+      auto lanelet_at_right = route_handler->getRightLanelet(current_lanelet);
+      auto lanelet_at_right_opposite = route_handler->getRightOppositeLanelets(current_lanelet);
+      while (lanelet_at_right) {
+        lanelet_to_be_extended.push_back(lanelet_at_right.get());
+        lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get());
+        lanelet_at_right_opposite = route_handler->getRightOppositeLanelets(lanelet_at_right.get());
+      }
+
+      if (!lanelet_at_right_opposite.empty() && extend_to_opposite_lane) {  // means lanelets in the
+                                                                            // opposite
+                                                                            // direction exist
+        lanelet_to_be_extended.push_back(lanelet_at_right_opposite.front());
+        auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right_opposite.front());
+        while (lanelet_at_left) {
+          lanelet_to_be_extended.push_back(lanelet_at_left.get());
+          lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get());
+        }
+      }
+    };
+
+  lanelet::ConstLanelets debug_connected;
+  for (auto & lane : current_lanes) {
+    debug_connected.push_back(lane);
+    searchRightLaneletsAndAppendToDrivableAreas(lane, debug_connected);
+
+    constexpr bool isRightHandDriving = false;
+    searchLeftLaneletsAndAppendToDrivableAreas(lane, debug_connected, !isRightHandDriving);
+  }
+
+  constexpr double area_resolution = 0.1;
+  const auto & vehicle_length = planner_data_->parameters.vehicle_length;
+  std::cout << debug_connected.size() << '\n';
+
+  const auto drivabless =
+    util::generateDrivableArea(debug_connected, area_resolution, vehicle_length, planner_data_);
+  debug_drivable_area_lanelets_publisher_->publish(drivabless);
+}
 }  // namespace behavior_path_planner
 
 #include <rclcpp_components/register_node_macro.hpp>