feat: add traffic rules to enable driving for areas (#104)

Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>
Signed-off-by: emmeyteja <emmeyteja@gmail.com>

Author: emmeyteja

autoware_lanelet2_extension/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(tf2 REQUIRED)
 autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME}_lib SHARED
+  lib/autoware_traffic_rules.cpp
   lib/autoware_osm_parser.cpp
   lib/autoware_traffic_light.cpp
   lib/crosswalk.cpp

autoware_lanelet2_extension/include/autoware_lanelet2_extension/traffic_rules/autoware_traffic_rules.hpp

@@ -0,0 +1,42 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE_LANELET2_EXTENSION__TRAFFIC_RULES__AUTOWARE_TRAFFIC_RULES_HPP_
+#define AUTOWARE_LANELET2_EXTENSION__TRAFFIC_RULES__AUTOWARE_TRAFFIC_RULES_HPP_
+
+#include <lanelet2_traffic_rules/GermanTrafficRules.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+namespace lanelet::autoware
+{
+
+/// @brief Autoware-specific location identifier for traffic rules registration.
+static constexpr const char DefaultLocation[] = "autoware";
+
+/// @brief Vehicle traffic rules for Autoware based on GermanVehicle, but allowing areas in normal
+/// driving mode. GermanVehicle unconditionally disallows canPass(ConstArea), which prevents routing
+/// through areas. This class restores the GenericTrafficRules behavior that checks participant
+/// tags.
+class AutowareVehicle : public traffic_rules::GermanVehicle
+{
+public:
+  using GermanVehicle::GermanVehicle;
+
+  // Allow passing through areas based on participant tags (GenericTrafficRules default behavior)
+  bool canPass(const ConstArea & area) const override { return GenericTrafficRules::canPass(area); }
+};
+
+}  // namespace lanelet::autoware
+
+#endif  // AUTOWARE_LANELET2_EXTENSION__TRAFFIC_RULES__AUTOWARE_TRAFFIC_RULES_HPP_

autoware_lanelet2_extension/include/autoware_lanelet2_extension/utility/route_checker.hpp

@@ -25,7 +25,11 @@ namespace lanelet::utils::route
 {
 using autoware_planning_msgs::msg::LaneletRoute;
 
-bool isRouteValid(const LaneletRoute & route, const lanelet::LaneletMapPtr lanelet_map_ptr_);
+/// @param allow_area If false, routes containing any primitive with primitive_type "area" are
+/// invalid (lane-only validation). If true, area ids are resolved on areaLayer.
+bool isRouteValid(
+  const LaneletRoute & route, const lanelet::LaneletMapPtr lanelet_map_ptr_,
+  bool allow_area = false);
 }  // namespace lanelet::utils::route
 
 // NOLINTEND(readability-identifier-naming)

autoware_lanelet2_extension/include/autoware_lanelet2_extension/visualization/visualization.hpp

@@ -31,6 +31,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Area.h>
 #include <lanelet2_core/primitives/Lanelet.h>
 
 #include <string>
@@ -236,6 +237,17 @@ visualization_msgs::msg::MarkerArray hatchedRoadMarkingsAreaAsMarkerArray(
  */
 visualization_msgs::msg::MarkerArray obstacleRemovalAreaAsMarkerArray(
   const lanelet::ConstPolygons3d & obstacle_removal_area, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [laneletAreasAsMarkerArray creates marker array to visualize Lanelet2 Area layer primitives
+ * (e.g. routing through lane-area-lane connections).]
+ * @param areas         [Lanelet2 ConstArea from map areaLayer]
+ * @param fill_color    [TRIANGLE_LIST fill color; namespace lanelet_routing_area]
+ * @param outline_color [LINE_STRIP boundary color; namespace lanelet_routing_area_outline]
+ */
+visualization_msgs::msg::MarkerArray laneletAreasAsMarkerArray(
+  const std::vector<lanelet::ConstArea> & areas, const std_msgs::msg::ColorRGBA & fill_color,
+  const std_msgs::msg::ColorRGBA & outline_color);
 }  // namespace format_v2
 
 /**

autoware_lanelet2_extension/lib/autoware_traffic_rules.cpp

@@ -0,0 +1,24 @@
+// Copyright 2025 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware_lanelet2_extension/traffic_rules/autoware_traffic_rules.hpp"
+
+namespace lanelet::autoware
+{
+
+// Register AutowareVehicle for the Autoware location + Vehicle participant.
+static traffic_rules::RegisterTrafficRules<AutowareVehicle> reg(
+  DefaultLocation, Participants::Vehicle);
+
+}  // namespace lanelet::autoware

autoware_lanelet2_extension/lib/route_checker.cpp

@@ -26,13 +26,21 @@
 namespace lanelet::utils
 {
 bool route::isRouteValid(
-  const LaneletRoute & route_msg, const lanelet::LaneletMapPtr lanelet_map_ptr_)
+  const LaneletRoute & route_msg, const lanelet::LaneletMapPtr lanelet_map_ptr_, bool allow_area)
 {
   for (const auto & route_section : route_msg.segments) {
     for (const auto & primitive : route_section.primitives) {
+      // If area is not allowed, and the primitive is an area, return false
+      if (primitive.primitive_type == "area" && !allow_area) {
+        return false;
+      }
       const auto id = primitive.id;
       try {
-        lanelet_map_ptr_->laneletLayer.get(id);
+        if (primitive.primitive_type == "area") {
+          lanelet_map_ptr_->areaLayer.get(id);
+        } else {
+          lanelet_map_ptr_->laneletLayer.get(id);
+        }
       } catch (const std::exception & e) {
         std::cerr
           << e.what()

autoware_lanelet2_extension/lib/visualization.cpp

@@ -31,10 +31,12 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <algorithm>
+#include <cmath>
 #include <iostream>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
+#include <utility>
 #include <vector>
 
 namespace
@@ -388,6 +390,62 @@ void pushPolygonMarker(
   }
 }
 
+bool areaOuterToPolygon3d(const lanelet::ConstArea & area, lanelet::Polygon3d * llt_poly)
+{
+  if (llt_poly == nullptr) {
+    return false;
+  }
+  llt_poly->clear();
+  for (const auto & ls : area.outerBound()) {
+    for (const auto & pt : ls) {
+      llt_poly->push_back(
+        lanelet::Point3d(
+          lanelet::InvalId, pt.basicPoint().x(), pt.basicPoint().y(), pt.basicPoint().z()));
+    }
+  }
+  if (llt_poly->size() >= 2U) {
+    const auto & f = llt_poly->front();
+    const auto & b = llt_poly->back();
+    if (
+      std::abs(f.basicPoint().x() - b.basicPoint().x()) < 1e-6 &&
+      std::abs(f.basicPoint().y() - b.basicPoint().y()) < 1e-6) {
+      llt_poly->pop_back();
+    }
+  }
+  return llt_poly->size() >= 3U;
+}
+
+visualization_msgs::msg::Marker makeLaneletRoutingAreaOutlineMarker(
+  const int id, const lanelet::Polygon3d & poly, const std_msgs::msg::ColorRGBA & outline_color)
+{
+  visualization_msgs::msg::Marker line;
+  line.header.frame_id = "map";
+  line.header.stamp = rclcpp::Time();
+  line.ns = "lanelet_routing_area_outline";
+  line.id = id;
+  line.action = visualization_msgs::msg::Marker::ADD;
+  line.type = visualization_msgs::msg::Marker::LINE_STRIP;
+  line.scale.x = 0.05;
+  line.scale.y = 0.0;
+  line.scale.z = 0.0;
+  line.color = outline_color;
+  line.pose.orientation.w = 1.0;
+  line.lifetime = rclcpp::Duration(0, 0);
+  line.frame_locked = false;
+  line.points.reserve(poly.size() + 1U);
+  for (const auto & pt : poly) {
+    geometry_msgs::msg::Point p;
+    p.x = pt.x();
+    p.y = pt.y();
+    p.z = pt.z();
+    line.points.push_back(p);
+  }
+  if (!line.points.empty()) {
+    line.points.push_back(line.points.front());
+  }
+  return line;
+}
+
 }  // anonymous namespace
 
 namespace lanelet
@@ -1210,6 +1268,40 @@ visualization_msgs::msg::MarkerArray obstacleRemovalAreaAsMarkerArray(
   }
   return marker_array;
 }
+
+visualization_msgs::msg::MarkerArray laneletAreasAsMarkerArray(
+  const std::vector<lanelet::ConstArea> & areas, const std_msgs::msg::ColorRGBA & fill_color,
+  const std_msgs::msg::ColorRGBA & outline_color)
+{
+  visualization_msgs::msg::MarkerArray marker_array;
+  if (areas.empty()) {
+    return marker_array;
+  }
+
+  visualization_msgs::msg::Marker fill_marker =
+    createPolygonMarker("lanelet_routing_area", fill_color);
+  std::vector<visualization_msgs::msg::Marker> outline_markers;
+  outline_markers.reserve(areas.size());
+  int outline_id = 0;
+
+  for (const auto & area : areas) {
+    lanelet::Polygon3d llt_poly;
+    if (!areaOuterToPolygon3d(area, &llt_poly)) {
+      continue;
+    }
+    pushPolygonMarker(&fill_marker, llt_poly, fill_color);
+    outline_markers.push_back(
+      makeLaneletRoutingAreaOutlineMarker(outline_id++, llt_poly, outline_color));
+  }
+
+  if (!fill_marker.points.empty()) {
+    marker_array.markers.push_back(std::move(fill_marker));
+  }
+  for (auto & om : outline_markers) {
+    marker_array.markers.push_back(std::move(om));
+  }
+  return marker_array;
+}
 }  // namespace format_v2
 }  // namespace visualization
 

autoware_lanelet2_extension/test/src/test_route_checker.cpp

@@ -22,11 +22,14 @@
 
 #include <gtest/gtest.h>
 #include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/Area.h>
 
 #include <memory>
 
+using lanelet::Area;
 using lanelet::Lanelet;
 using lanelet::LineString3d;
+using lanelet::LineStrings3d;
 using lanelet::Point3d;
 using lanelet::utils::getId;
 
@@ -88,6 +91,60 @@ TEST_F(TestSuite, isRouteValid)  // NOLINT for gtest
        "one";
 }
 
+TEST_F(TestSuite, isRouteValid_rejects_area_when_allow_area_false)  // NOLINT for gtest
+{
+  autoware_planning_msgs::msg::LaneletRoute route_with_area;
+  autoware_planning_msgs::msg::LaneletSegment segment;
+  autoware_planning_msgs::msg::LaneletPrimitive prim;
+  prim.primitive_type = "area";
+  prim.id = 99999;  // id is irrelevant when allow_area is false
+  segment.primitives.push_back(prim);
+  route_with_area.segments.push_back(segment);
+
+  ASSERT_FALSE(lanelet::utils::route::isRouteValid(route_with_area, sample_map_ptr, false))
+    << "Route with area primitive must be invalid when allow_area is false";
+  ASSERT_FALSE(lanelet::utils::route::isRouteValid(route_with_area, sample_map_ptr))
+    << "Default allow_area should be false for mixed routes";
+}
+
+TEST_F(TestSuite, isRouteValid_unknown_area_id_fails_when_allow_area_true)  // NOLINT for gtest
+{
+  autoware_planning_msgs::msg::LaneletRoute route_with_area;
+  autoware_planning_msgs::msg::LaneletSegment segment;
+  autoware_planning_msgs::msg::LaneletPrimitive prim;
+  prim.primitive_type = "area";
+  prim.id = 99999;  // not on map
+  segment.primitives.push_back(prim);
+  route_with_area.segments.push_back(segment);
+
+  ASSERT_FALSE(lanelet::utils::route::isRouteValid(route_with_area, sample_map_ptr, true))
+    << "Missing area id on map should fail validation";
+}
+
+TEST_F(TestSuite, isRouteValid_resolves_area_when_allow_area_true)  // NOLINT for gtest
+{
+  const lanelet::Id area_id = getId();
+  const Point3d a1(getId(), 10.0, 0.0, 0.0);
+  const Point3d a2(getId(), 12.0, 0.0, 0.0);
+  const Point3d a3(getId(), 12.0, 2.0, 0.0);
+  const Point3d a4(getId(), 10.0, 2.0, 0.0);
+  const LineString3d ring(getId(), {a1, a2, a3, a4, a1});
+  const LineStrings3d outer{ring};
+  const Area test_area(area_id, outer);
+  sample_map_ptr->add(test_area);
+
+  autoware_planning_msgs::msg::LaneletRoute route_with_area;
+  autoware_planning_msgs::msg::LaneletSegment segment;
+  autoware_planning_msgs::msg::LaneletPrimitive prim;
+  prim.primitive_type = "area";
+  prim.id = area_id;
+  segment.primitives.push_back(prim);
+  route_with_area.segments.push_back(segment);
+
+  ASSERT_TRUE(lanelet::utils::route::isRouteValid(route_with_area, sample_map_ptr, true))
+    << "Valid area id on map should pass when allow_area is true";
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);