watonomous.github.io
[ Software Division : HDM Service ]
Created by [ Henry Wang], last modified on May 14, 2020
This design document proposes the transmission of HDM information to the rest of the software system. This document assumes the reader has good knowledge of OSM and HERE HDM, as well as routing or graph traversal algorithm concepts. Feel free to review our other documentation, the HERE developer guide, and other concepts before continuing.
Previous Design
Currently, the vehicle's software system is designed as a single direction pipeline, based on the typical publisher-subscriber pattern supported by ROS. This leads to many anti-patterns in our codebase that don't always make a lot of sense. Prime examples of these anti-patterns are:
- continuously publish a monolithic data structure that represents the entire map
- continuously publish a large data structure that represents local map elements thresholded by an arbitrary radial distance from the ego vehicle as if it were detected by Perception
It should be obvious why these ideas aren't good. The second idea might seem like a good idea, but in the context of HDM, it's quite inappropriate. It's also the currently proposed plan. Historically, former junior members have treated HDM data as interchangeable with Perception data - they are not. And software architecture has been designed with HDM as generally supplementary - it is not. Let's do a thought experiment on how this proposed architecture proximity based publishing might look.
There are many reasons why I don't like this, but I don't want to write too much:
- Why mindless publish everything within some arbitrary threshold? For example, we don't care about the adjacent lane that travels in the opposite direction, why bother reporting it. Why do we arbitrarily cut off at some radial distance?
- [Why do we need to continuously publish all local lanes at a high frequency anyways? We're not Perception. In the absence of actions (e.g turns, lane changes etc) the vehicle is still on the same lane. Say if this lane is just a segment that's 100m long then there's really no need to republish that information until we've reached the end of the segment (in the absence of external forces). Generally, PP doesn't need new information unless some action needs to be made.]{.inline-comment-marker data-ref=”2cea8cb6-0219-4fdf-92b2-5a0e293602fe”}
[The point is, there was no realistic plan. So here's mine.]{.inline-comment-marker data-ref=”2cea8cb6-0219-4fdf-92b2-5a0e293602fe”}
HDM Service
ROS also supports the Client-Server pattern. Contrary to our usual Pub-Sub pattern, this allows messages to be exchanged with requests and responses, forming a bi-directional pipeline. Unfortunately, any usages of IPC requires intelligent definitions of message structures. We wouldn't really need IPC in the first place if the map was loaded in some PP node but who cares about IPC overhead anyways. Thankfully, the concept of lanelets is pretty easy to represent and can be derived from existing standards such as HERE or OSM.
Service ROS Messages
This is the proposed ROS message type used to succinctly represent the transmission of HDM lanelet info. This message type is derived from OSM Lanelets, with additional HERE context being embedded as necessary. The core message is the HDMLane, other relevant messages are HDMLaneLine and HDMLaneList.
Service API
The service is implemented as a python ROS node. It loads the map into program memory. It exposes an interface for client nodes to make queries. This service is effectively a database as it mostly responds to queries for HDMLanes. The syntax for a client making a request is just like a function invocation, but it's really better thought of as a remote procedure call. See the basic tutorial on ROS Services for more details.
The proposed queries are as follows.
int32t get_current_lane_id(pose)
HDMLane get_current_lane(pose)
The 2 variations are provided for better client flexibility, they can make 1 or 2 RPCs. This should be called once from Path Planning as it is an initialization (e.g when the entire system boots up the vehicle "localizes" itself on the HDM). Of course, it's also possible that the vehicle has become lost (e.g GPS issues) and re-localization is necessary. The implementation for this query is basically a nearest neighbour search, which can be implemented by a linear search or by rtrees. This is a work in progress.
HDMLane get_lane(int32_t lane_id)
This will probably be the most commonly invoked query and does exactly what you think is does.
HDMLaneList get_connected_lanes(int32_t lane_id)
Return all recursively adjacent lanes (think HERE LaneGroup), and all their next lanes. This query will probably not return lanes that drive in the opposite direction because that's useless. This query allows the client to receive local reachable lanes in a single query, otherwise subsequent invocations of get_lane would be required. A picture is a thousand words, see below. Given that the vehicle is on the red lane, this query would return all lanes except for the 2 parallel lanes that drive in the opposite direction. The limited depth of the search is quite arbitrary, and if we really wanted to, we could return all the lanes /s.
\
[
{.confluence-embedded-image
.confluence-thumbnail height=”250”}]{.confluence-embedded-file-wrapper
.confluence-embedded-manual-size}
Path Planning Routing Use Cases
The most likely client will be one of Path Planning's routing nodes. Let's think about how they would they use this query interface to route at various levels. First we need to understand the crux of the problem, going from point A to B. Apparently, for the competition, the inputs are given as Navigational Map (NM) link IDs (i.e street addresses), or their corresponding latitude and longitude coordinates. Currently, global mapping is proposed to route at the NM layer, by reusing Tony's project that performs Shortest Path First and continuously queries a massive SQL database. This generates a sequence of NM link IDs. I actually don't think that's the way to go because of the performance overhead from SQL queries for every node and link lookup, and the database represents an entire state, most of which we don't care about. But it saves work for now.
The NM links can be translated to HERE HDM Link Topology Geometry Layer links, so the path looks like a sequence of Link Topology link IDs.[ This is routing at a high level, but what about at a lower level? If the vehicle is making turns, then it needs to make appropriate lane changes first because we have multi-lane roads. ]{.inline-comment-marker data-ref=”c3f16ef0-6867-450e-9a14-dfd4195b4a0c”}This brings us to a more granular level routing. I think PP calls it the Behavioural Planner. Ultimately we want a sequence of lane change and branch (think left turn or something) actions to get from one lane to a destination lane. Thankfully, this should be do-able in the OSM Lanelet Map converted from HERE Lane Geometry Layer.
This is how I would break down this routing problem into logical steps. I assume our GPS data has sufficiently low error, otherwise nothing would work.
- Receive input destination in street address or latitude, longitude.
- Translate destination into Link Topology link ID and parametric range with HERE's translation layer and some nearest neighbour search. Do the same with the vehicle's current position. Now we have a source and destination link ID, coordinate, and maybe some parametric range on the links.
- Route from source link to destination link. If all lanes are bidirectional then this will be easy. Remember we don't care which side of the road we are on for the destinations. Now we have a path, which is represented by a sequence of links. Great, that's global routing, but for we need more granular local routing if we want to be changing lanes and using correct turning lanes. Solution, use the context rich HDM layers.
- PP calls get_current_lane(pose) and now knows which lane it's on.
- [PP does BFS starting from the current lane to recursively explore all possible "adjacent" and "next" lanes and so on, until a lane with the next-in-path link ID is found. If you're confused here, think of a lane as a node in a digraph, think of adjacent and next lanes as children of that node. This produces a sequence of lanes that allows the vehicle to traverse from a source link ID to a destination link ID. Let's call this lane level routing.]{.inline-comment-marker data-ref=”b8483fde-084b-434f-8ca6-de08e6b47f0e”}
This lane level routing was originally intended to be used for getting from the current link to a connected[ (intermediate) destination link. However we could route to the ultimate destination link to produce a complete of lanes the vehicle must traverse through. That's pretty elegant. Be careful because the BFS expansion tree gets really wide the deeper you go. But if PP were to actually execute lane level routing to the ultimate destination link, then that would be a really big waste of IPC. The node that loaded the map can more efficiently execute lane level routing. [But no one cares about IPC so whatever.]{.inline-comment-marker data-ref=”d7e0832f-ac5a-415c-a4ea-701a8d342a98”}]
This is what PP pseudo code might look like for working with the service. Honestly, that lane level routing can be done in our module as well. It's pretty light work so hopefully it doesn't matter where it happens. The only caveat here is that we don't PP explore the entire lane graph because that incurs a lot of IPC. The BFS should have a depth limit.
# Core routing function implements BFS.
# Inputs:
# src_lane_id: current HDMLane.lane_id that the vehicle is on.
# dest_link_id: destination Link Topology Geometry link ID.
# Outputs:
# path: list of HDMLane.lane_id or HDMLane.
route_lanes(src_lane_id, dest_link_id):
found = False
queue = []
parents =
visited =
path = []
queue.enq(src_lane_id)
parents[src_lane_id] = None
while True:
current_lane_id = stack.deq()
visited[current_lane_id] = True
current_lane = service.get_lane(lane_id)
if current_lane.start_link_id == dest_link_id:
found = True
path = [current_lane_id]
break
for lane_id in [current_lane.left, current_lane.right, current_lane.next]:
if visited[lane_id]:
continue
stack.push(lane_id)
parents[lane_id] = current_lane_id
# I used lane_id, but this could just be the lane as well.
# It's a memory trade-off to store all examined lane objects.
# However, if working with lane_ids, an RPC is needed to fetch
# the contents of the lane data structure.
if found:
while True:
parent_lane_id = parents[path[-1]]
if parent_lane_id == None:
break
path = [parent_lane_id] + path
else:
throw Error("Path not found to ")
return path
# Implemented as some callback on obstacle detection.
swerve(current_lane_id, path):
left_or_right_lane_id = dodge_left_or_right(current_lane_id)
path.enq_at_head(left_or_right_lane_id)
# In some main loop.
path = route_lanes()
current_lane = service.get_lane(path[0])
while path:
next_lane_id = path.deq()
next_lane = service.get_lane(next_lane_id):
if is_lane_change(current_lane, next_lane):
change_lane(next_lane):
else:
follow_extended_trajectory(current_lane, next_lane)
current_lane = next_lane
One of the main challenges is [[dynamic rerouting]{.inline-comment-marker data-ref=”b1b024d2-1701-4345-9a05-8d0121d1184d”}]{.inline-comment-marker data-ref=”c34783e6-8c4b-4ae9-b590-bd2123faddec”}. How do we do this? If an intersection is blocked, make note of the source link ID and destination link ID, and re-route at the Topology Geometry Layer (or NM layer) based on those restrictions to produce a new sequence of link IDs. Nothing new here.
Justin's Design
Very similar with the exception that only stitched lanes are published. This is WIP.
===============================================
# HDMStichedLane
-----------------------------------------------
Header header
# Id used to categorize stitched lanes by starting lane.
int32 lane_id
# Polyline representing the center trajectory of this lane.
# Direction of travel is implied by the ordering of points.
geometry_msgs/Point[] trajectory
# We need another data type for this.
# Polylines representing the left and right boundaries of this lane.
# Ordering of points should correspond to trajectory.
HDMLaneLine left_boundary
HDMLaneLine right_boundary
uint32[] lane_ids
\
Traffic Light Association
Unlike the previous lane query functions, traffic light data should be published.
===============================================
# AssociatedTrafficLight
-----------------------------------------------
Header header
# Lane id as defined in Justin / Henry's Design
int32 lane_id
# Traffic Light State Enums
string TL_ST_NON=NON
string TL_ST_RED=RED
string TL_ST_YEL=YELLOW
string TL_ST_GRE=GREEN
string TL_ST_FLA=FLASHING_RED
string colour
HDMLaneLine left_boundary
HDMLaneLine right_boundary
uint32[] lane_ids
===============================================
# AssociatedTrafficLightList
-----------------------------------------------
Header header
AssociatedTrafficLight lights
Attachments:
Screen Shot
2020-02-29 at 12.20.37 AM.png
(image/png)\
Comments:
+———————————————————————–+
| [] |
| |
| Henry |
| Wang{.conf |
| luence-userlink |
| .user-mention} Can you double-check the type of the IDs? I remember |
| from the navigation map, Link IDs are 64 bit unsigned integers. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| 
|
| Posted by cy9zhang at Feb 26, 2020 17:04 |
| |
+———————————————————————–+
| [] |
| |
| Good catch. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Feb 26, 2020 18:46 |
| |
+———————————————————————–+
| [] |
| |
| Link ID is uint32 |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Feb 27, 2020 06:24 |
| |
+———————————————————————–+
| [] |
| |
| > ==============================================={.text .plain |
| > style=”text-align: left;”}\ |
| > # HDMLaneLine\ |
| > -----------------------------------------------{.text .plain |
| > style=”text-align: left;”}\ |
| > Header header\ |
| > \ |
| > # Unique ID for this lane line.{.text .plain |
| > style=”text-align: left;”}\ |
| > int32 lane_line_id |
| |
| 1. Does pp really need this? I feel like they would only be |
| concerned with which lane they are on and the boundary lines |
| associated with it, but I see no reason to provide an identifier. |
| Although I do accept that we will need to internally have an id |
| for theses lanelets in the OSM maps. |
| 2. It begs the question, how are we generating these ids? The HERE |
| map protobuf developer guide did not mention giving lane |
| boundaries an id. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by f58wu at Mar 01, 2020 06:02 |
| |
+———————————————————————–+
| [] |
| |
| > # Unique ID for this lane.\ |
| > int32 lane_id |
| |
| <div> |
| |
| Clarification: |
| |
| </div> |
| |
| <div> |
| |
| We will use HERE lane ids defined in the lane level topology-geo |
| layer? |
| |
| </div> |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by f58wu at Mar 01, 2020 06:11 |
| |
+———————————————————————–+
| [] |
| |
| > HDMLaneLine left_boundary\ |
| > HDMLaneLine right_boundary |
| |
| <div> |
| |
| We could actually use the lane_line_id in HDMLaneLine here. Going |
| by the assumption that adjacent lanes will share 1 lanelet, we can |
| avoid sending the each inner set of lanelet points twice by sending a |
| lane_line_id to reference the polyline instead. This would of |
| course require us to send an array of HDMLaneLine as well. |
| |
| </div> |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by f58wu at Mar 01, 2020 06:15 |
| |
+———————————————————————–+
| [] |
| |
| > # Additional HERE specific context.\ |
| > # HERE Topology Geometry Link IDs.\ |
| > # Intersection lanes typically have different start and end link |
| > IDs.\ |
| > # Otherwise, start and end IDs are generally the same.\ |
| > uint32 start_link_id\ |
| > uint32 end_link_id |
| |
| <div> |
| |
| More clarification here, why does pp need these ids? |
| |
| </div> |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by f58wu at Mar 01, 2020 06:25 |
| |
+———————————————————————–+
| [] |
| |
| 1. It might be a little redundant, I don't quite remember |
| now. Charles |
| Zhang{.confl |
| uence-userlink |
| .user-mention}, did we have a use case for including this?\ |
| 2. Converting lane boundaries to OSM Ways is really easy, we just |
| generate a unique ID during the conversion. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Mar 01, 2020 06:38 |
| |
+———————————————————————–+
| [] |
| |
| HERE does not have explicit lane_ids, they have lane_group_ids, |
| and you reference a lane by an index. This is an OSM concept, this |
| lane_id is not related to Topology Geometry Link ID. It's used to |
| uniquely identify and query relevant lanes. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Mar 01, 2020 06:40 |
| |
+———————————————————————–+
| [] |
| |
| We chose to embed it so that a single HDMLane structure on its own |
| provides enough environmental context for navigation. We totally |
| could have created a wrapper class that contained HDMLane[] and |
| HDMLaneLine[]. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Mar 01, 2020 06:42 |
| |
+———————————————————————–+
| [] |
| |
| I guess my documentation wasn't very clear. We need this for |
| routing. Their global routing generates a list of connected Link |
| Topology Geometry Link IDs. At a lower level of routing they need a |
| list of connected HDMLanes to traverse. Recall that lanes and links |
| can be mapped between each other. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Mar 01, 2020 06:46 |
| |
+———————————————————————–+
| [] |
| |
| Will this lane_id just be the index of the lane in the lane group |
| then? |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by f58wu at Mar 01, 2020 06:48 |
| |
+———————————————————————–+
| [] |
| |
| No, it's a uniquely generated ID. OSM lanelet concept. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Mar 01, 2020 06:49 |
| |
+———————————————————————–+
| [] |
| |
| Yup, for referencing between navigation and HD map |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by cy9zhang at Mar 01, 2020 15:19 |
| |
+———————————————————————–+
| [] |
| |
| Right now no use cases. IMO it's a good idea to give them IDs since |
| they're duplicated between stitched lanes. This way we can easily |
| tell which roadlines are identical |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by cy9zhang at Mar 01, 2020 15:20 |
| |
+———————————————————————–+
| [] |
| |
| ["I assume our GPS data has sufficiently low |
| error"] |
| |
| [We had an issue with this in the previous competition - we'll need |
| more information from other sensors to corroborate the GPS |
| reading.] |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by a293pate at Mar 04, 2020 00:01 |
| |
+———————————————————————–+
| [] |
| |
| "[PP does BFS"] |
| |
| [I imagine we can compute some heuristics based on (e.g. the number |
| of levels in the rtree you have to backtrack to get to a common |
| ancestor) so they're not doing BFS] |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by a293pate at Mar 04, 2020 00:13 |
| |
+———————————————————————–+
| [] |
| |
| Don't need rtrees for this, lanes have pointers to each other. |
| |
| {.smallfont align=”left” style=”color: #666666; width: 98%; margi |
| n-bottom: 10px;”} |
| |
| Posted by henry.wang at Mar 04, 2020 04:34 |
| |
+———————————————————————–+
Document generated by Confluence on Dec 10, 2021 04:02