# Mitigating Urban Congestion: A Cooperative Reservation Framework for Automated Vehicles
## 4 Infrastructure Tracking Node
The sequence executes each action consecutively; in this case, the _BeforeABP_ marks the distance at which to start the next action. Then, the fallback section follows, which executes actions until one is successful. The first action, _RequestConnection_, tries to request a connection to the infrastructure management system. The management system receives this request through the services provided in the data package and starts searching for a _Survey Cluster_ to assign to the vehicle. If the search is successful, the infrastructure container assigns the _Cluster_ found to the vehicle that requested the connection, and the behavior finishes. The _Tracking Node_ stores a reference to the _Cluster_ that is used to track the vehicle and concurrently manages the resources of the traffic emplacement. If the _Cluster_ cannot be found with current information, it is not assigned, the action fails, and instead, the behavior executes the fallback that defines a prudent approach to the ABP (_ApproachABP_ action) and a wait-and-retry connection execution (_WaitRetry_ action that listen to the topic _stop_ to release the wait flag). The search process to find a suitable _Cluster_ given the planning and mission information retrieved from the vehicle container can be seen in Algorithm 2. This algorithm requests the _road_plan_ from the vehicle planning layer and then filters with the \(type\_filter\) parameter only relevant lane types. Then, it computes the middle point of the current road processed and compares each of its lanes with each _Survey Area_ lane of each _Survey Cluster_. The closest point matched belongs to the _Survey Cluster_ that will be assigned to the _Tracking Node_. The same procedure can be used with Mission information; the only difference is that, instead of using planning roads, the algorithms use ABP poses, for which it converts vehicle coordinate ABP poses into infrastructure local coordinates. After this process, the vehicle is tracked by the _Tracking Node_, starting to retrieve navigation information from the vehicle container. Afterward, the arbitration can take place by executing the _Tracking Node_ associated with the vehicle and the _Cluster_ selected. ```
1:\(type\_filter\leftarrow[\mathit{Intersections},\mathit{Entry},\mathit{Exit}]\)
2:\(road\_plan\gets node\_requestPlan(type\_filter)\)
3:procedureFindClusterwithPlan(\(road\_plan\))
4:\(contacts\_mid\leftarrow[]\)
5:\(coord\_transform\leftarrow\mathit{node\_requestMapOrigin}()\)
6:for each\(road\in road\_plan\)do
7:for each\(contact\in\mathit{road\_contacts}\)do
8:\(contacts\_mid\gets coord\_transform.toENU(contact\_mid)\)
9:endfor
10:for each\(cluster\in\mathit{clusters}\)do
11:if\(contacts\_mid\in\mathit{clusters.control\_areas}\)then
12:\(return\gets cluster\)
13:endif
14:endfor
15:endfor
16:\(return\gets nullptr\)
17:endprocedure
```  
**Algorithm 2** Find cluster with plan  
This last algorithm reports an average processing latency of \(\approx 3\) ms with an \(\mathcal{O}(n)\) space complExity to find the _Survey Cluster_ set from the provided cooperative information, which, in the best case, is extracted directly from the Road Plan. These results may depend on the hardware used and the configuration of the _Survey Cluster_ set stored in memory, but they were consistent with the test cases proposed in Section 7. Finally, in Figure 6, a visual representation of this process to find a _Cluster_ can be found. In this figure, a vehicle is approaching the intersection; yellow lanes represent the road plan the vehicle is following, which is shared with the infrastructure to find a suitable _Cluster_. Due to differing vehicle and infrastructure map origins, a coordinate conversion occurs when comparing midpoints. Based on matched Entry, Exit, and survey lanes, the infrastructure assigns a _Cluster_ and places an ABP using transformed Entry and Exit coordinates, which are then communicated to the vehicle in its coordinate system.