Merge pull request #216 from usdot-fhwa-stol/feature/add_routing_grap…

…h_to_map_updates

Add RoutingGraph to map updates

lanelet2/lanelet2_routing/include/lanelet2_routing/RoutingGraph.h

@@ -490,7 +490,7 @@ class RoutingGraph {
    */
   RoutingGraph(std::unique_ptr<internal::RoutingGraphGraph>&& graph, lanelet::LaneletSubmapConstPtr&& passableMap);
 
- private:
+ protected:
   //! Documentation to be found in the cpp file.
   std::unique_ptr<internal::RoutingGraphGraph> graph_;  ///< Wrapper of the routing graph
   LaneletSubmapConstPtr passableLaneletSubmap_;         ///< Lanelet map of all passable lanelets

lanelet2/lanelet2_routing/src/RoutingGraphBuilder.cpp

@@ -3,7 +3,6 @@
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Area.h>
 #include <lanelet2_core/geometry/Lanelet.h>
-
 #include <unordered_map>
 
 #include "lanelet2_routing/Exceptions.h"
@@ -92,6 +91,7 @@ RoutingGraphBuilder::RoutingGraphBuilder(const traffic_rules::TrafficRules& traf
       config_{config} {}
 
 RoutingGraphUPtr RoutingGraphBuilder::build(const LaneletMapLayers& laneletMapLayers) {
+
   auto passableLanelets = getPassableLanelets(laneletMapLayers.laneletLayer, trafficRules_);
   auto passableAreas = getPassableAreas(laneletMapLayers.areaLayer, trafficRules_);
   auto passableMap = utils::createConstSubmap(passableLanelets, passableAreas);
@@ -100,6 +100,7 @@ RoutingGraphUPtr RoutingGraphBuilder::build(const LaneletMapLayers& laneletMapLa
   addAreasToGraph(passableAreas);
   addEdges(passableLanelets, passableMap->laneletLayer);
   addEdges(passableAreas, passableMap->laneletLayer, passableMap->areaLayer);
+
   return std::make_unique<RoutingGraph>(std::move(graph_), std::move(passableMap));
 }
 

messages/autoware_lanelet2_msgs/msg/MapBin.msg

@@ -23,3 +23,11 @@ uint32 route_version
 
 # route flag to set the route invalidation status
 bool invalidates_route
+
+# Description of the Routing Graph following this update.
+# This field will only be populated if has_routing_graph == true
+autoware_lanelet2_msgs/RoutingGraph routing_graph
+
+# If true then this update contains a new routing_graph
+# In standard usage this would only be set if invalidates_route==true or if this is the first map broadcast of a new map version
+bool has_routing_graph

messages/autoware_lanelet2_msgs/msg/RoutingGraph.msg

@@ -0,0 +1,18 @@
+# Message describing the contents of a lanelet2 routing graph such that it can be reconstructed by the recieving component
+
+
+# A list of lanelets representing vertices in the graph and their associated edges
+# This structure mirrors an adjacency list representation of a directed graph 
+RoutingGraphVertexAndEdges[] lanelet_vertices
+
+# A list of areass representing vertices in the graph and their associated edges
+RoutingGraphVertexAndEdges[] area_vertices
+
+# The lanelet2 participant type used to generate this graph
+# The lanelets and areas in this graph will only be present if they are passable by this participant type
+string participant_type
+
+# The maximum number of routing cost ids used to compute edge costs in this graph
+# This field is an optimization to prevent the need to count the number of unique routing cost ids in the edges
+int64 num_unique_routing_cost_ids
+

messages/autoware_lanelet2_msgs/msg/RoutingGraphVertexAndEdges.msg

@@ -0,0 +1,23 @@
+# This message is a description of a single vertex (lanelet or area) and its edges
+# in a Lanelet2 Routing Graph. The edges are stored as a collection of equally sized areas where each index corresponds to one edge
+# The reason edges are stored as multiple array fields instead of independent message objects
+# is because ROS2 DDS implementations have known performance issues with heavily nested data structures. 
+
+int64 lanelet_or_area # Lanelet or area of interest (the vertex)
+
+# The following 4 arrays must have the same size and ordering such that each index corresponds to the same routing graph edge 
+int64[] lanelet_or_area_ids # The other vertices connected to the vertex of interest in a lanelet_or_area->lanelet_or_area_ids[i] direction
+float64[] edge_routing_costs # The routing costs of the edges
+uint16[] edge_routing_cost_source_ids # The ids of the routing cost generators used to compute the routing cost of each edge
+uint8[] edge_relations # The relation type of the edge
+
+# Enum of edge relation types
+# See the enum class RelationType in Lanelet2 for relation descriptions
+uint8 RELATION_NONE=0
+uint8 RELATION_SUCCESSOR=1
+uint8 RELATION_LEFT=2
+uint8 RELATION_RIGHT=3
+uint8 RELATION_ADJACENT_LEFT=4
+uint8 RELATION_ADJACENT_RIGHT=5
+uint8 RELATION_CONFLICTING=6
+uint8 RELATION_AREA=7