import lanelet2_map_preprocessor

Signed-off-by: a-maumau <[email protected]>

build_depends.repos

@@ -24,6 +24,10 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/autoware_internal_msgs.git
     version: main
+  core/autoware_lanelet2_extension:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git
+    version: 0.6.0
   # universe
   universe/autoware.universe:
     type: git

map/autoware_lanelet2_map_utils/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_lanelet2_map_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(PCL REQUIRED COMPONENTS common io kdtree)
+
+include_directories(
+  include
+  SYSTEM
+    ${PCL_INCLUDE_DIRS}
+)
+
+link_libraries(
+  ${PCL_LIBRARIES}
+)
+
+ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp)
+ament_auto_add_executable(transform_maps src/transform_maps.cpp)
+ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp)
+ament_auto_add_executable(merge_close_points src/merge_close_points.cpp)
+ament_auto_add_executable(remove_unreferenced_geometry src/remove_unreferenced_geometry.cpp)
+ament_auto_add_executable(fix_lane_change_tags src/fix_lane_change_tags.cpp)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  launch
+)

map/autoware_lanelet2_map_utils/README.md

@@ -0,0 +1,3 @@
+# autoware_lanelet2_map_utils
+
+This package is for preprocessing the lanelet map.

map/autoware_lanelet2_map_utils/config/fix_z_value_by_pcd.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    llt_map_path: $(var llt_map_path)
+    pcd_map_path: $(var pcd_map_path)
+    llt_output_path: $(var llt_output_path)

map/autoware_lanelet2_map_utils/config/transform_maps.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    llt_map_path: $(var llt_map_path)
+    pcd_map_path: $(var pcd_map_path)
+    llt_output_path: $(var llt_output_path)
+    pcd_output_path: $(var pcd_output_path)
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    roll: 0.0
+    pitch: 0.0
+    yaw: 0.0

map/autoware_lanelet2_map_utils/launch/fix_z_value_by_pcd.launch.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <node pkg="autoware_lanelet2_map_utils" exec="fix_z_value_by_pcd" name="fix_z_value_by_pcd" output="screen">
+    <param from="$(find-pkg-share autoware_lanelet2_map_utils)/config/fix_z_value_by_pcd.param.yaml" allow_substs="true"/>
+  </node>
+</launch>

map/autoware_lanelet2_map_utils/launch/transform_maps.launch.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <node pkg="autoware_lanelet2_map_utils" exec="transform_maps" name="transform_maps" output="screen">
+    <param from="$(find-pkg-share autoware_lanelet2_map_utils)/config/transform_maps.param.yaml" allow_substs="true"/>
+  </node>
+</launch>

map/autoware_lanelet2_map_utils/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_lanelet2_map_utils</name>
+  <version>0.1.0</version>
+  <description>The autoware_lanelet2_map_utils package</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Kento Yabuuchi</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Ryohsuke Mitsudome</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_lanelet2_extension</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

map/autoware_lanelet2_map_utils/schema/transform_maps.schema.json

@@ -0,0 +1,75 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Transforming Maps",
+  "type": "object",
+  "definitions": {
+    "transform_maps": {
+      "type": "object",
+      "properties": {
+        "llt_map_path": {
+          "type": "string",
+          "description": "Path pointing to the input lanelet2 file",
+          "default": ""
+        },
+        "pcd_map_path": {
+          "type": "string",
+          "description": "Path pointing to the input point cloud file",
+          "default": ""
+        },
+        "llt_output_path": {
+          "type": "string",
+          "description": "Path pointing to the output lanelet2 file",
+          "default": ""
+        },
+        "pcd_output_path": {
+          "type": "string",
+          "description": "Path pointing to the output point cloud file",
+          "default": ""
+        },
+        "x": {
+          "type": "number",
+          "default": 0.0,
+          "description": "x factor of Translation vector for transforming maps [m]"
+        },
+        "y": {
+          "type": "number",
+          "default": 0.0,
+          "description": "y factor of Translation vector for transforming maps [m]"
+        },
+        "z": {
+          "type": "number",
+          "default": 0.0,
+          "description": "z factor of Translation vector for transforming maps [m]"
+        },
+        "roll": {
+          "type": "number",
+          "default": 0.0,
+          "description": "roll factor of Rotation vector for transforming maps [rad]"
+        },
+        "pitch": {
+          "type": "number",
+          "default": 0.0,
+          "description": "pitch factor of Rotation vector for transforming maps [rad]"
+        },
+        "yaw": {
+          "type": "number",
+          "default": 0.0,
+          "description": "yaw factor of Rotation vector for transforming maps [rad]"
+        }
+      },
+      "required": ["x", "y", "z", "roll", "pitch", "yaw"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/transform_maps"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

map/autoware_lanelet2_map_utils/src/fix_lane_change_tags.cpp

@@ -0,0 +1,104 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
+#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
+#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/primitives/LaneletSequence.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_routing/RoutingGraph.h>
+
+#include <iostream>
+#include <unordered_set>
+#include <vector>
+
+namespace autoware::lanelet2_map_utils
+{
+bool load_lanelet_map(
+  const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr,
+  lanelet::Projector & projector)
+{
+  lanelet::LaneletMapPtr lanelet_map;
+  lanelet::ErrorMessages errors;
+  lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
+
+  for (const auto & error : errors) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
+  }
+  if (!errors.empty()) {
+    return false;
+  }
+  std::cout << "Loaded Lanelet2 map" << std::endl;
+  return true;
+}
+
+lanelet::Lanelets convert_to_vector(const lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  lanelet::Lanelets lanelets;
+  std::copy(
+    lanelet_map_ptr->laneletLayer.begin(), lanelet_map_ptr->laneletLayer.end(),
+    std::back_inserter(lanelets));
+  return lanelets;
+}
+void fix_tags(lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  auto lanelets = convert_to_vector(lanelet_map_ptr);
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
+    lanelet::traffic_rules::TrafficRulesFactory::create(
+      lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  lanelet::routing::RoutingGraphUPtr routing_graph =
+    lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *traffic_rules);
+
+  for (auto & llt : lanelets) {
+    if (!routing_graph->conflicting(llt).empty()) {
+      continue;
+    }
+    llt.attributes().erase("turn_direction");
+    if (!!routing_graph->adjacentRight(llt)) {
+      llt.rightBound().attributes()["lane_change"] = "yes";
+    }
+    if (!!routing_graph->adjacentLeft(llt)) {
+      llt.leftBound().attributes()["lane_change"] = "yes";
+    }
+  }
+}
+}  // namespace autoware::lanelet2_map_utils
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = rclcpp::Node::make_shared("fix_lane_change_tags");
+
+  const auto llt_map_path = node->declare_parameter<std::string>("llt_map_path");
+  const auto output_path = node->declare_parameter<std::string>("output_path");
+
+  lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
+  lanelet::projection::MGRSProjector projector;
+
+  if (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) {
+    return EXIT_FAILURE;
+  }
+
+  autoware::lanelet2_map_utils::fix_tags(llt_map_ptr);
+  lanelet::write(output_path, *llt_map_ptr, projector);
+
+  rclcpp::shutdown();
+
+  return 0;
+}