map_loader package#
This package provides the features of loading various maps.
pointcloud_map_loader#
Feature#
pointcloud_map_loader
provides pointcloud maps to the other Autoware nodes in various configurations.
Currently, it supports the following two types:
- Publish raw pointcloud map
- Publish downsampled pointcloud map
- Send partial pointcloud map loading via ROS 2 service
- Send differential pointcloud map loading via ROS 2 service
NOTE: We strongly recommend to use divided maps when using large pointcloud map to enable the latter two features (partial and differential load). Please go through the prerequisites section for more details, and follow the instruction for dividing the map and preparing the metadata.
Prerequisites#
Prerequisites on pointcloud map file(s)#
You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules:
- The pointcloud map should be projected on the same coordinate defined in
map_projection_loader
, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to the readme ofmap_projection_loader
. - It must be divided by straight lines parallel to the x-axis and y-axis. The system does not support division by diagonal lines or curved lines.
- The division size along each axis should be equal.
- The division size should be about 20m x 20m. Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in ndt_scan_matcher and autoware_compare_map_segmentation.
- All the split maps should not overlap with each other.
- Metadata file should also be provided. The metadata structure description is provided below.
Metadata structure#
The metadata should look like this:
x_resolution: 20.0
y_resolution: 20.0
A.pcd: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520
B.pcd: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520
C.pcd: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540
D.pcd: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540
where,
x_resolution
andy_resolution
A.pcd
,B.pcd
, etc, are the names of PCD files.- List such as
[1200, 2500]
are the values indicate that for this PCD file, x coordinates are between 1200 and 1220 (x_resolution
+x_coordinate
) and y coordinates are between 2500 and 2520 (y_resolution
+y_coordinate
).
You may use pointcloud_divider for dividing pointcloud map as well as generating the compatible metadata.yaml.
Directory structure of these files#
If you only have one pointcloud map, Autoware will assume the following directory structure by default.
sample-map-rosbag
├── lanelet2_map.osm
├── pointcloud_map.pcd
If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files.
sample-map-rosbag
├── lanelet2_map.osm
├── pointcloud_map.pcd
│ ├── A.pcd
│ ├── B.pcd
│ ├── C.pcd
│ └── ...
├── map_projector_info.yaml
└── pointcloud_map_metadata.yaml
Specific features#
Publish raw pointcloud map (ROS 2 topic)#
The node publishes the raw pointcloud map loaded from the .pcd
file(s).
Publish downsampled pointcloud map (ROS 2 topic)#
The node publishes the downsampled pointcloud map loaded from the .pcd
file(s). You can specify the downsample resolution by changing the leaf_size
parameter.
Publish metadata of pointcloud map (ROS 2 topic)#
The node publishes the pointcloud metadata attached with an ID. Metadata is loaded from the .yaml
file. Please see the description of PointCloudMapMetaData.msg
for details.
Send partial pointcloud map (ROS 2 service)#
Here, we assume that the pointcloud maps are divided into grids.
Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area.
Please see the description of GetPartialPointCloudMap.srv
for details.
Send differential pointcloud map (ROS 2 service)#
Here, we assume that the pointcloud maps are divided into grids.
Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs.
Please see the description of GetDifferentialPointCloudMap.srv
for details.
Send selected pointcloud map (ROS 2 service)#
Here, we assume that the pointcloud maps are divided into grids.
Given IDs query from a client node, the node sends a set of pointcloud maps (each of which attached with unique ID) specified by query.
Please see the description of GetSelectedPointCloudMap.srv
for details.
Parameters#
Name | Type | Description | Default | Range |
---|---|---|---|---|
enable_whole_load | boolean | Enable raw pointcloud map publishing | True | N/A |
enable_downsampled_whole_load | boolean | Enable downsampled pointcloud map publishing | False | N/A |
enable_partial_load | boolean | Enable partial pointcloud map server | True | N/A |
enable_selected_load | boolean | Enable selected pointcloud map server | False | N/A |
leaf_size | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0 | N/A |
pcd_paths_or_directory | array | Path(s) to pointcloud map file or directory | [] | N/A |
pcd_metadata_path | string | Path to pointcloud metadata file | N/A |
Interfaces#
output/pointcloud_map
(sensor_msgs/msg/PointCloud2) : Raw pointcloud mapoutput/pointcloud_map_metadata
(autoware_map_msgs/msg/PointCloudMapMetaData) : Metadata of pointcloud mapoutput/debug/downsampled_pointcloud_map
(sensor_msgs/msg/PointCloud2) : Downsampled pointcloud mapservice/get_partial_pcd_map
(autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud mapservice/get_differential_pcd_map
(autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud mapservice/get_selected_pcd_map
(autoware_map_msgs/srv/GetSelectedPointCloudMap) : Selected pointcloud map- pointcloud map file(s) (.pcd)
- metadata of pointcloud map(s) (.yaml)
lanelet2_map_loader#
Feature#
lanelet2_map_loader
loads Lanelet2 file(s) and publishes the map data as autoware_map_msgs/LaneletMapBin
message.
The node projects lan/lon coordinates into arbitrary coordinates defined in /map/map_projector_info
from map_projection_loader
.
Please see tier4_autoware_msgs/msg/MapProjectorInfo.msg for supported projector types.
lanelet2_map_loader provides Lanelet2 maps in two different configurations:
- It loads single Lanelet2 file and publishes the map data as
autoware_map_msgs/LaneletMapBin
message. - It loads multiple Lanelet2 files differentially via ROS 2 service.
NOTE: If you work on large scale area, we recommend to use differential loading feature to avoid the time-consuming in various nodes that use the Lanelet2 map data.
Prerequisites#
Prerequisites on Lanelet2 map file(s)#
You may provide either a single .osm
file or multiple .osm
files. If you are using multiple OSM file,
it MUST obey the following rules:
- The
.osm
files should all be within the same MGRS grid. The system does not support loading multiple.osm
files from different MGRS grids. - It must be divided by straight lines parallel to the x-axis and y-axis. The system does not support division by diagonal lines or curved lines.
- Metadata file should also be provided. The metadata structure description is provided below.
Metadata structure#
The metadata should look like this:
x_resolution: 20.0
y_resolution: 20.0
A.osm: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520
B.osm: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520
C.osm: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540
D.osm: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540
where,
x_resolution
andy_resolution
are the resolution of the map in meters.A.osm
,B.osm
, etc, are the names of OSM files.- List such as
[1200, 2500]
are the values indicate that for this OSM file, x coordinates are between 1200 and 1220 (x_resolution
+x_coordinate
) and y coordinates are between 2500 and 2520 (y_resolution
+y_coordinate
).
You may use lanelet2_map_tile_generator from LeoDrive for dividing Lanelet2 map as well as generating the compatible metadata.yaml.
Directory structure of these files#
If you only have one Lanelet2 map, Autoware will assume the following directory structure by default.
sample-map-rosbag
├── lanelet2_map.osm
├── pointcloud_map.pcd
If you have multiple Lanelet2 maps, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple Lanelet2 map files.
sample-map-rosbag
├── pointcloud_map.pcd
├── lanelet2_map.osm
│ ├── A.osm
│ ├── B.osm
│ ├── C.osm
│ └── ...
├── map_projector_info.yaml
├── lanelet2_map_metadata.yaml
└── ...
How to run#
ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm
Subscribed Topics#
- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware
Published Topics#
If you use single Lanelet2 map file:
- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map
If you use multiple Lanelet2 map files:
- ~output/lanelet_map_meta_data (autoware_map_msgs/msg/LaneletMapMetaData) : Metadata of Lanelet2 Map
Services#
If you use multiple Lanelet2 map files:
- ~service/get_differential_lanelet_map (autoware_map_msgs/srv/GetSelectedLanelet2Map) : Differential loading of Lanelet2 Map
Parameters#
Name | Type | Description | Default | Range |
---|---|---|---|---|
enable_differential_map_loading | string | Flag to enable loading of differential map | true | N/A |
allow_unsupported_version | boolean | Flag to load unsupported format_version anyway. If true, just prints warning. | true | N/A |
center_line_resolution | float | Resolution of the Lanelet center line [m] | 5.0 | N/A |
use_waypoints | boolean | If true, centerline in the Lanelet2 map will be used as a waypoints tag. |
True | N/A |
lanelet2_map_path | string | The lanelet2 map path pointing to the .osm file | N/A | |
dummy_metadata.min_x | float | When the dynamic map loading is true and the map is a single file, this is the minimum x value of the map | 0.0 | N/A |
dummy_metadata.min_y | float | When the dynamic map loading is true and the map is a single file, this is the minimum y value of the map | 0.0 | N/A |
dummy_metadata.x_resolution | float | When the dynamic map loading is true and the map is a single file, this is the resolution of the x axis | 100000.0 | N/A |
dummy_metadata.y_resolution | float | When the dynamic map loading is true and the map is a single file, this is the resolution of the y axis | 100000.0 | N/A |
use_waypoints
decides how to handle a centerline.
This flag enables to use the overwriteLaneletsCenterlineWithWaypoints
function instead of overwriteLaneletsCenterline
. Please see the document of the autoware_lanelet2_extension package in detail.
lanelet2_map_visualization#
Feature#
lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray.
How to Run#
ros2 run map_loader lanelet2_map_visualization
Subscribed Topics#
- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map
Published Topics#
- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz