feat(map_projection_loader): add map_projection_loader (autowarefound…

…ation#3986) * feat(map_projection_loader): add map_projection_loader Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * Update default algorithm Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix test Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix launch file and fix map_loader Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update lanelet2 Signed-off-by: kminoda <koji.minoda@tier4.jp> * fill yaml file path Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * minor fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix test Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add include guard Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update test Signed-off-by: kminoda <koji.minoda@tier4.jp> * update map_loader Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update docs * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * add dependency Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * remove unnecessary parameter Signed-off-by: kminoda <koji.minoda@tier4.jp> * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * update test Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add url Signed-off-by: kminoda <koji.minoda@tier4.jp> * enable python tests Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * small fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix grammar Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove transverse mercator Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add rule in map Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix readme of map loader Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove transverse mercator for now Signed-off-by: kminoda <koji.minoda@tier4.jp> * add utm Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove altitude from current projection loader Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix pre-commit Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix build error Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix: remove package.xml Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix clang-tidy Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
leo-drive · Aug 16, 2023 · 02aad76 · 02aad76
1 parent b4f9f53
commit 02aad76
Show file tree

Hide file tree

Showing 22 changed files with 935 additions and 62 deletions.
diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py
@@ -12,13 +12,18 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import os
+
+from ament_index_python import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import GroupAction
+from launch.actions import IncludeLaunchDescription
 from launch.actions import OpaqueFunction
 from launch.actions import SetLaunchConfiguration
 from launch.conditions import IfCondition
 from launch.conditions import UnlessCondition
+from launch.launch_description_sources import AnyLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import Node
@@ -57,7 +62,10 @@ def launch_setup(context, *args, **kwargs):
         package="map_loader",
         plugin="Lanelet2MapLoaderNode",
         name="lanelet2_map_loader",
-        remappings=[("output/lanelet2_map", "vector_map")],
+        remappings=[
+            ("output/lanelet2_map", "vector_map"),
+            ("input/map_projector_info", "map_projector_type"),
+        ],
         parameters=[
             {
                 "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"),
@@ -110,6 +118,19 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    map_projection_loader_launch_file = os.path.join(
+        get_package_share_directory("map_projection_loader"),
+        "launch",
+        "map_projection_loader.launch.xml",
+    )
+    map_projection_loader = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(map_projection_loader_launch_file),
+        launch_arguments={
+            "map_projector_info_path": LaunchConfiguration("map_projector_info_path"),
+            "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"),
+        }.items(),
+    )
+
     container = ComposableNodeContainer(
         name="map_container",
         namespace="",
@@ -129,6 +150,7 @@ def launch_setup(context, *args, **kwargs):
             PushRosNamespace("map"),
             container,
             map_hash_generator,
+            map_projection_loader,
         ]
     )
 
@@ -159,6 +181,11 @@ def add_launch_arg(name: str, default_value=None, description=None):
         [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"],
         "path to pointcloud map metadata file",
     ),
+    add_launch_arg(
+        "map_projector_info_path",
+        [LaunchConfiguration("map_path"), "/map_projector_info.yaml"],
+        "path to map projector info yaml file",
+    ),
     add_launch_arg(
         "lanelet2_map_loader_param_path",
         [

diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml
@@ -13,6 +13,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <exec_depend>map_loader</exec_depend>
+  <exec_depend>map_projection_loader</exec_depend>
   <exec_depend>map_tf_generator</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

diff --git a/map/map_loader/README.md b/map/map_loader/README.md
@@ -20,11 +20,12 @@ Currently, it supports the following two types:
 
 You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data and either of `enable_partial_load`, `enable_differential_load` or `enable_selected_load` are set true, it MUST obey the following rules:
 
-1. **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.
-2. **The division size along each axis should be equal.**
-3. **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](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation).
-4. **All the split maps should not overlap with each other.**
-5. **Metadata file should also be provided.** The metadata structure description is provided below.
+1. **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 of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md).
+2. **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.
+3. **The division size along each axis should be equal.**
+4. **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](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation).
+5. **All the split maps should not overlap with each other.**
+6. **Metadata file should also be provided.** The metadata structure description is provided below.
 
 Note that these rules are not applicable when `enable_partial_load`, `enable_differential_load` and `enable_selected_load` are all set false. In this case, however, you also need to disable dynamic map loading mode for other nodes as well ([ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation) as of June 2023).
 
@@ -69,6 +70,7 @@ sample-map-rosbag
 │ ├── B.pcd
 │ ├── C.pcd
 │ └── ...
+├── map_projector_info.yaml
 └── pointcloud_map_metadata.yaml
 ```
 
@@ -138,27 +140,39 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co
 ### Feature
 
 lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message.
-The node projects lan/lon coordinates into MGRS coordinates.
+The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_type` from `map_projection_loader`.
+The node supports the following three types of coordinate systems:
+
+- MGRS
+- UTM
+- local
 
 ### 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
 
 - ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map
 
+### Parameters
+
+| Name                   | Type        | Description                                      | Default value |
+| :--------------------- | :---------- | :----------------------------------------------- | :------------ |
+| center_line_resolution | double      | Define the resolution of the lanelet center line | 5.0           |
+| lanelet2_map_path      | std::string | The lanelet2 map path                            | None          |
+
 ---
 
 ## lanelet2_map_visualization
 
 ### Feature
 
-lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. There are 3 types of map can be loaded in autoware. Please make sure you selected the correct lanelet2_map_projector_type when you launch this package.
-
-- MGRS
-- UTM
-- local
+lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray.
 
 ### How to Run
 
@@ -171,13 +185,3 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa
 ### Published Topics
 
 - ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz
-
-### Parameters
-
-| Name                        | Type        | Description                                                  | Default value |
-| :-------------------------- | :---------- | :----------------------------------------------------------- | :------------ |
-| lanelet2_map_projector_type | std::string | The type of the map projector using, can be MGRS, UTM, local | MGRS          |
-| latitude                    | double      | Latitude of map_origin, only using in UTM map projector      | 0.0           |
-| longitude                   | double      | Longitude of map_origin, only using in UTM map projector     | 0.0           |
-| center_line_resolution      | double      | Define the resolution of the lanelet center line             | 5.0           |
-| lanelet2_map_path           | std::string | The lanelet2 map path                                        | None          |
diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml
@@ -1,7 +1,3 @@
 /**:
   ros__parameters:
-    lanelet2_map_projector_type: MGRS   # Options: MGRS, UTM, local
-    latitude: 40.816617984672746        # Latitude of map_origin, using in UTM
-    longitude: 29.360491808334285       # Longitude of map_origin, using in UTM
-
     center_line_resolution: 5.0         # [m]
diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp
@@ -36,16 +36,15 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
   static lanelet::LaneletMapPtr load_map(
     const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
     const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0);
-  static const MapProjectorInfo get_map_projector_type(
-    const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
-    const double & map_origin_lat, const double & map_origin_lon);
   static HADMapBin create_map_bin_msg(
     const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
     const rclcpp::Time & now);
 
 private:
+  void on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg);
+
+  rclcpp::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_type_;
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
-  rclcpp::Publisher<MapProjectorInfo>::SharedPtr pub_map_projector_type_;
 };
 
 #endif  // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
@@ -36,6 +36,7 @@
 #include <ament_index_cpp/get_package_prefix.hpp>
 #include <lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/projection/transverse_mercator_projector.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -49,16 +50,21 @@
 
 Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options)
 : Node("lanelet2_map_loader", options)
+{
+  // subscription
+  sub_map_projector_type_ = create_subscription<MapProjectorInfo>(
+    "input/map_projector_info", rclcpp::QoS{1}.transient_local(),
+    [this](const MapProjectorInfo::ConstSharedPtr msg) { on_map_projector_info(msg); });
+}
+
+void Lanelet2MapLoaderNode::on_map_projector_info(const MapProjectorInfo::ConstSharedPtr msg)
 {
   const auto lanelet2_filename = declare_parameter("lanelet2_map_path", "");
-  const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS");
   const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0);
-  const double map_origin_lat = declare_parameter("latitude", 0.0);
-  const double map_origin_lon = declare_parameter("longitude", 0.0);
 
   // load map from file
   const auto map =
-    load_map(lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon);
+    load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude);
   if (!map) {
     return;
   }
@@ -69,16 +75,10 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
   // create map bin msg
   const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now());
 
-  const auto map_projector_type_msg = get_map_projector_type(
-    lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon);
   // create publisher and publish
   pub_map_bin_ =
     create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
   pub_map_bin_->publish(map_bin_msg);
-  // create publisher and publish
-  pub_map_projector_type_ =
-    create_publisher<MapProjectorInfo>("map_projector_type", rclcpp::QoS{1}.transient_local());
-  pub_map_projector_type_->publish(map_projector_type_msg);
 }
 
 lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
@@ -96,7 +96,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
     lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
     lanelet::Origin origin{position};
     lanelet::projection::UtmProjector projector{origin};
-
     const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
     if (errors.empty()) {
       return map;
@@ -137,27 +136,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
   return nullptr;
 }
 
-const MapProjectorInfo Lanelet2MapLoaderNode::get_map_projector_type(
-  const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
-  const double & map_origin_lat, const double & map_origin_lon)
-{
-  lanelet::ErrorMessages errors{};
-  MapProjectorInfo map_projector_type_msg;
-  if (lanelet2_map_projector_type == "MGRS") {
-    lanelet::projection::MGRSProjector projector{};
-    const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
-    map_projector_type_msg.type = "MGRS";
-    map_projector_type_msg.mgrs_grid = projector.getProjectedMGRSGrid();
-  } else if (lanelet2_map_projector_type == "UTM") {
-    map_projector_type_msg.type = "UTM";
-    map_projector_type_msg.map_origin.latitude = map_origin_lat;
-    map_projector_type_msg.map_origin.longitude = map_origin_lon;
-  } else {
-    map_projector_type_msg.type = "local";
-  }
-  return map_projector_type_msg;
-}
-
 HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
   const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now)
 {

diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt
@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.14)
+project(map_projection_loader)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(map_projection_loader_lib SHARED
+  src/map_projection_loader.cpp
+  src/load_info_from_lanelet2_map.cpp
+)
+
+target_link_libraries(map_projection_loader_lib yaml-cpp)
+
+ament_auto_add_executable(map_projection_loader src/map_projection_loader_node.cpp)
+
+target_compile_options(map_projection_loader PUBLIC -g -Wall -Wextra -Wpedantic -Werror)
+
+target_link_libraries(map_projection_loader map_projection_loader_lib)
+target_include_directories(map_projection_loader PUBLIC include)
+
+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}" map_projection_loader_lib)
+  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_utm_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
+)