TF Tree for Autoware.Auto
Description
Autoware.Auto doesn't have a pre-defined TF tree. This issue takes a first pass at defining one for at least the AVP demo. This proposal adds one additional frame to those defined in REP-105 which is here called nav_base
but it does not have a child frame. Here is my proposal:
Frames
All frames as defined in REP-105 plus nav_base
and one frame per sensor. With regard to base_link
and nav_base
, the following definitions would apply to their locations with respect to the mobile robot's chassis:
- For road-ready vehicles which have a mounting location for a license plate, the
base_link
frame will be tied to a location on the exterior of the chassis of the vehicle exactly half-way between the top two mounting holes for the rear license plate. - For vehicles which do not have a license plate mounting location, the
base_link
frame will be tied to a location exactly half-way between the chassis frame rails in the Y axis (RHR convention, Y left), as far in the negative X axis as both frame rails extend, and in the vertical center of both frame rails in the Z axis. - The
nav_base
frame will be a robot-fixed frame withbase_link
as its parent. The origin of thenav_base
frame would be in the center of the axis of rotation of the mobile robot with 0 Z offset frombase_link
. Since thebase_link
frame is no longer attached to the rear axle of the vehicle, usingbase_link
as the frame for calculating local navigation becomes difficult. Thenav_base
frame attempts to mitigate this problem.
Autoware.Auto Static Maps Tree
Assumptions
- Static, globally-fixed map for localization (sensor map)
- Static, globally-fixed map for traffic rules and some stationary objects (semantic map)
- Locally-referenced odometry data are available from one or more sources
- Locally-referenced environmental data are available from one or more sources
- Globally-referenced position data are available from one or more sources
Nodes
- Odometry Localizer:
- Input: Locally-referenced sensor data with no fixed, external localization environment (e.g. wheel encoders, camera odometry, accelerometer, gyroscope, etc.)
- Output: Estimated pose and twist in the
odom
frame - Output:
odom
->base_link
Transform
- Environmental Sensor Localizer:
- Input: Locally-referenced sensor data with fixed, external localization environment (e.g. lidar, radar, etc.)
- Input: Globally-referenced sensor map
- Input:
earth
->map
transform - Input: Estimated pose and twist of
base_link
in theearth
frame- Estimate is translated to
map
frame withearth
->map
transform - Used for initial pose estimate
- Estimate is translated to
- Output: Estimated pose of
base_link
in themap
frame
- Global Sensor Localizer:
- Input: Globally-referenced sensor data (e.g. GPS, magnetometer, etc.)
- Output: Estimated pose and twist of
base_link
in theearth
frame
- Map Localizer:
- Input: Estimated pose and twist of
base_link
in theodom
frame - Input: Estimated pose and twist of
base_link
in themap
frame - Input:
earth
->map
transform - Input: Estimated pose and twist of
base_link
in theearth
frame- Estimate is translated to
map
frame withearth
->map
transform
- Estimate is translated to
- Output:
map
->odom
Transform
- Input: Estimated pose and twist of
Transforms
-
base_link
-><sensor_frame>
: Static. Published bytf2::static_transform_publisher
orrobot_description
when using a URDF file.-
Explanation: It is assumed that all sensors which are attached to the body of the mobile robot are done so rigidly and are not free to move. Being as
base_link
is a frame that is defined as being tied to a fixed point on the mobile robot's body, any transform between a sensor andbase_link
would be static.
-
Explanation: It is assumed that all sensors which are attached to the body of the mobile robot are done so rigidly and are not free to move. Being as
-
odom
->base_link
: Dynamic. Published by Odometry Localizer.-
Explanation: While localization to the local map provides a globally-fixed location estimate, no sensor or algorithm to localize to the local map can be considered infallible and can not be trusted to always provide a continuous, error-free position. In addition, an Environmental Sensor Localizer can not be expected to provide an accurate localization estimate when an initial position in the local map has not yet been computed. Since navigation is based on the full
map
->base_link
transform, this estimate can be used for navigation even when a globally-relative or map-relative position is not yet available.
-
Explanation: While localization to the local map provides a globally-fixed location estimate, no sensor or algorithm to localize to the local map can be considered infallible and can not be trusted to always provide a continuous, error-free position. In addition, an Environmental Sensor Localizer can not be expected to provide an accurate localization estimate when an initial position in the local map has not yet been computed. Since navigation is based on the full
-
map
->odom
: Dynamic. Published by Map Localizer.-
Explanation: The Map Localizer takes the position estimate provided by the Environmental Sensor Localizer (a
map
->base_link
relation) as well as the position estimate provided by the Global Sensor Localizer (anearth
->base_link
relation which is translated to amap
->base_link
relation using a lookup of the fixedearth
->map
transform) and fuses them into a singlemap
->base_link
estimate. It then relates this estimate to the origin of theodom
frame as amap
->odom
transform, which it publishes. This allows integration of both the odometry localization data and environmental sensor localization data into a transform tree ofmap
->odom
->base_link
which should be considered a single transform (i.e. ephemeralmap
->base_link
) by navigation nodes.
-
Explanation: The Map Localizer takes the position estimate provided by the Environmental Sensor Localizer (a
-
map
->semantic_map
: Static. Published bytf2::static_transform_publisher
-
Explanation: Since there often exists an offset between the origin of the Semantic Map and the origin of the Sensor Map, different frames for each map are necessary. Also, since localization is usually more time-sensitive than functions which utilize the Semantic Map, this proposal ties the typical
map
frame to the Sensor Map which avoids a transform lookup in localization but necessitates one in functions that use the Semantic Map such as path planning.
-
Explanation: Since there often exists an offset between the origin of the Semantic Map and the origin of the Sensor Map, different frames for each map are necessary. Also, since localization is usually more time-sensitive than functions which utilize the Semantic Map, this proposal ties the typical
-
earth
->map
: If a map server is used, dynamic but non-contiguous and published by the map server. If no map server is used, static and published bytf2::static_transform_publisher
-
Explanation: Since the Sensor Map (published in the
map
frame) is defined as being globally-fixed, this transform relates the origin of the Sensor Map to the origin of theearth
frame which is referenced as ECEF. The publication of this transform is necessary for the Environmental Sensor Localizer to create an initial position estimate using the output from the Global Sensor Localizer. This transform is also necessary for the Map Localizer to be able to utilize the output from the Global Sensor Localizer as a fusion input.
-
Explanation: Since the Sensor Map (published in the
Diagram
Definition of Done
-
ASWG Members Agree on Nodes and TF Tree -
Documentation Updated with Decision
Edited by Joshua Whitley