Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: move autoware_map_projection_loader package from Autoware Universe #125

Draft
wants to merge 6 commits into
base: main
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 63 additions & 0 deletions map/autoware_map_projection_loader/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
cmake_minimum_required(VERSION 3.14)
project(autoware_map_projection_loader)

find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_find_build_dependencies()

ament_auto_add_library(${PROJECT_NAME} SHARED
src/map_projection_loader.cpp
src/load_info_from_lanelet2_map.cpp
)

rclcpp_components_register_node(${PROJECT_NAME}
PLUGIN "autoware::map_projection_loader::MapProjectionLoader"
EXECUTABLE ${PROJECT_NAME}_node
EXECUTOR SingleThreadedExecutor
)

target_link_libraries(${PROJECT_NAME} yaml-cpp)

function(add_testcase filepath)
get_filename_component(filename ${filepath} NAME)
string(REGEX REPLACE ".cpp" "" test_name ${filename})
ament_add_gmock(${test_name} ${filepath})
target_link_libraries("${test_name}" ${PROJECT_NAME})
ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
endfunction()

if(BUILD_TESTING)
# Test python scripts
add_launch_test(
test/test_node_load_mgrs_from_yaml.test.py
TIMEOUT "30"
)
add_launch_test(
test/test_node_load_local_from_yaml.test.py
TIMEOUT "30"
)
add_launch_test(
test/test_node_load_local_cartesian_utm_from_yaml.test.py
TIMEOUT "30"
)
add_launch_test(
test/test_node_load_transverse_mercator_from_yaml.test.py
TIMEOUT "30"
)
install(DIRECTORY
test/data/
DESTINATION share/${PROJECT_NAME}/test/data/
)

# Test c++ scripts
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
add_testcase(test/test_load_info_from_lanelet2_map.cpp)
endif()

ament_auto_package(
INSTALL_TO_SHARE
launch
config
)
95 changes: 95 additions & 0 deletions map/autoware_map_projection_loader/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
# autoware_map_projection_loader

## Feature

`autoware_map_projection_loader` is responsible for publishing `map_projector_info` that defines in which kind of coordinate Autoware is operating.
This is necessary information especially when you want to convert from global (geoid) to local coordinate or the other way around.

- If `map_projector_info_path` DOES exist, this node loads it and publishes the map projection information accordingly.
- If `map_projector_info_path` does NOT exist, the node assumes that you are using the `MGRS` projection type, and loads the lanelet2 map instead to extract the MGRS grid.
- **DEPRECATED WARNING: This interface that uses the lanelet2 map is not recommended. Please prepare the YAML file instead.**

## Map projector info file specification

You need to provide a YAML file, namely `map_projector_info.yaml` under the `map_path` directory. For `pointcloud_map_metadata.yaml`, please refer to the Readme of `autoware_map_loader`.

```bash
sample-map-rosbag
├── lanelet2_map.osm
├── pointcloud_map.pcd
├── map_projector_info.yaml
└── pointcloud_map_metadata.yaml
```

There are three types of transformations from latitude and longitude to XYZ coordinate system as shown in the figure below. Please refer to the following details for the necessary parameters for each projector type.

![node_diagram](docs/map_projector_type.svg)

### Using local coordinate

```yaml
# map_projector_info.yaml
projector_type: Local
```

#### Limitation

The functionality that requires latitude and longitude will become unavailable.

The currently identified unavailable functionalities are:

- GNSS localization
- Sending the self-position in latitude and longitude using ADAPI

### Using MGRS

If you want to use MGRS, please specify the MGRS grid as well.

```yaml
# map_projector_info.yaml
projector_type: MGRS
vertical_datum: WGS84
mgrs_grid: 54SUE
```

#### Limitation

It cannot be used with maps that span across two or more MGRS grids. Please use it only when it falls within the scope of a single MGRS grid.

### Using LocalCartesianUTM

If you want to use local cartesian UTM, please specify the map origin as well.

```yaml
# map_projector_info.yaml
projector_type: LocalCartesianUTM
vertical_datum: WGS84
map_origin:
latitude: 35.6762 # [deg]
longitude: 139.6503 # [deg]
altitude: 0.0 # [m]
```

### Using TransverseMercator

If you want to use Transverse Mercator projection, please specify the map origin as well.

```yaml
# map_projector_info.yaml
projector_type: TransverseMercator
vertical_datum: WGS84
map_origin:
latitude: 35.6762 # [deg]
longitude: 139.6503 # [deg]
altitude: 0.0 # [m]
```

## Published Topics

- `~/map_projector_info` (autoware_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information

## Parameters

Note that these parameters are assumed to be passed from launch arguments, and it is not recommended to directly write them in `map_projection_loader.param.yaml`.

{{ json_to_markdown("map/autoware_map_projection_loader/schema/map_projection_loader.schema.json") }}
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
/**:
ros__parameters:
map_projector_info_path: $(var map_projector_info_path)
lanelet2_map_path: $(var lanelet2_map_path)
Loading
Loading