Closed msmcconnell closed 1 year ago
Well there is the doxygen doc. Essentially, the RoutingGraph
has an underlying Graph
object that manages the boost::graph
object which represents the actual graph. Adding an edge is basically as easy as doing routingGraph->graph_.addEdge(fromLanelet, toLanelet, edgeInfo);
. However so far there has not been any need for modifications to the graph so there is no functionality to remove or modify an existing edge. Also there is nothing to prevent someone from adding multiple edges for the same vertices. So basically what you could do is:
RoutingGraph
Graph
to modify or remove edgesRoutingGraph
to create something like a MutableRoutingGraph
that has member functions that allow to adding or modifying neighbour relations.Alternatively you could also create something like a RoutingGraphTemplate
that has the superset of all edges that might be there (basically as if initialized with a dummy TrafficRule object where everything is passable) and then offers a function to create an actual RoutingGraph
using the actual TrafficRules
(and RoutingCosts
) by copying the underlying graph object and removing all unpassable edges. Since this second step should be very fast compared to the first, this can be used to quickly create a new graph once the rules have changed while still making sure the resulting graph is consistent with the rules.
Hope this helps. Should you have something running, think about creating a PR, maybe others might find this useful too.
@poggenhans Thanks, this is very helpful! If it works out, I will certainly put up a PR.
@poggenhans We recently implemented our own solution for this (based on steps 1-3 you provided) and thought you might be interested in seeing the implementation we used.
We implemented a RoutingGraph.msg which is effectively a representation of the boost adjacency list used to store the graph internally in lanelet2.
To construct the message we extended from RoutingGraph after making the underlying RoutingGraphGraph member protected. https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/carma_wm_ctrl/src/RoutingGraphAccessor.h
To reconstruct the graph on a receiving node we implemented this function https://github.com/usdot-fhwa-stol/carma-platform/blob/283cfdf8e2e88ffb97ef1d7ff3d8add88fbcb4cc/carma_wm/src/WMListenerWorker.cpp#L447
If, you wanted to mainline these implementations I'll try to assist but we would need to identify where such things could live in the core lanelet2 libraries.
This issue is stale because it has been open for 90 days with no activity. Is this issue still work in progress? If yes, mark it as "WIP" or comment, otherwise it will be closed in 30 days.
This issue was closed because it has been stalled for 30 days with no activity.
I have a system using a particularly large lanelet2 map and a mechanism for dynamic updates to regulatory elements. Sometimes these regulatory element updates require that the RoutingGraph be recomputed. However, this process can take a very long time 30s or more which is not acceptable for my use case. I would like to investigate the possibility of either limiting the graph update to only the impacted lanelets, and or creating a portable format for the routing graph which I can communicate to other system components, so only one component would need to do the long computation. The code structure for the RoutingGraph is quite complex, so I am wondering if there is any additional documentation that could be provided such as Class, Activity, or Sequence Diagrams? Having such documentation would be a big first step in identifying how to make the modifications.