feat(lanelet2_map_preprocessor): port lanelet2_map_preprocessor from …

…Autoware.Universe

Signed-off-by: mitsudome-r <[email protected]>

lanelet2_map_preprocessor/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.14)
+project(lanelet2_map_preprocessor)
+
+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
+  launch
+)

lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="llt_map_path" default=""/>
+  <arg name="pcd_map_path" default=""/>
+  <arg name="llt_output_path" default=""/>
+
+  <node pkg="lanelet2_map_preprocessor" exec="fix_z_value_by_pcd" name="fix_z_value_by_pcd" output="screen">
+    <param name="llt_map_path" value="$(var llt_map_path)"/>
+    <param name="pcd_map_path" value="$(var pcd_map_path)"/>
+    <param name="llt_output_path" value="$(var llt_output_path)"/>
+  </node>
+</launch>

lanelet2_map_preprocessor/launch/transform_maps.launch.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="llt_map_path" default=""/>
+  <arg name="pcd_map_path" default=""/>
+  <arg name="llt_output_path" default=""/>
+  <arg name="pcd_output_path" default=""/>
+  <arg name="x" default="0.0"/>
+  <arg name="y" default="0.0"/>
+  <arg name="z" default="0.0"/>
+  <arg name="roll" default="0.0"/>
+  <arg name="pitch" default="0.0"/>
+  <arg name="yaw" default="0.0"/>
+
+  <node pkg="lanelet2_map_preprocessor" exec="transform_maps" name="transform_maps" output="screen">
+    <param name="llt_map_path" value="$(var llt_map_path)"/>
+    <param name="pcd_map_path" value="$(var pcd_map_path)"/>
+    <param name="llt_output_path" value="$(var llt_output_path)"/>
+    <param name="pcd_output_path" value="$(var pcd_output_path)"/>
+    <param name="x" value="$(var x)"/>
+    <param name="y" value="$(var y)"/>
+    <param name="z" value="$(var z)"/>
+    <param name="roll" value="$(var roll)"/>
+    <param name="pitch" value="$(var pitch)"/>
+    <param name="yaw" value="$(var yaw)"/>
+  </node>
+</launch>

lanelet2_map_preprocessor/package.xml

@@ -0,0 +1,23 @@
+<?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>lanelet2_map_preprocessor</name>
+  <version>0.1.0</version>
+  <description>The lanelet2_map_preprocessor package</description>
+  <maintainer email="[email protected]">Ryohsuke Mitsudome</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>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>

lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp

@@ -0,0 +1,109 @@
+// 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 <lanelet2_extension/io/autoware_osm_parser.hpp>
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <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>
+
+using lanelet::utils::getId;
+using lanelet::utils::to2D;
+
+bool loadLaneletMap(
+  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;
+}
+
+bool exists(std::unordered_set<lanelet::Id> & set, lanelet::Id element)
+{
+  return std::find(set.begin(), set.end(), element) != set.end();
+}
+
+lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  lanelet::Lanelets lanelets;
+  for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) {
+    lanelets.push_back(lanelet);
+  }
+  return lanelets;
+}
+void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  auto lanelets = convertToVector(lanelet_map_ptr);
+  lanelet::traffic_rules::TrafficRulesPtr trafficRules =
+    lanelet::traffic_rules::TrafficRulesFactory::create(
+      lanelet::Locations::Germany, lanelet::Participants::Vehicle);
+  lanelet::routing::RoutingGraphUPtr routingGraph =
+    lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules);
+
+  for (auto & llt : lanelets) {
+    if (!routingGraph->conflicting(llt).empty()) {
+      continue;
+    }
+    llt.attributes().erase("turn_direction");
+    if (!!routingGraph->adjacentRight(llt)) {
+      llt.rightBound().attributes()["lane_change"] = "yes";
+    }
+    if (!!routingGraph->adjacentLeft(llt)) {
+      llt.leftBound().attributes()["lane_change"] = "yes";
+    }
+  }
+}
+
+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 (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) {
+    return EXIT_FAILURE;
+  }
+
+  fixTags(llt_map_ptr);
+  lanelet::write(output_path, *llt_map_ptr, projector);
+
+  rclcpp::shutdown();
+
+  return 0;
+}

lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp

@@ -0,0 +1,169 @@
+// 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 <lanelet2_extension/io/autoware_osm_parser.hpp>
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_io/Io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/point_types.h>
+
+#include <iostream>
+#include <unordered_set>
+#include <vector>
+
+bool loadLaneletMap(
+  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;
+}
+
+bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd_map_ptr)
+{
+  if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_map_path, *pcd_map_ptr) == -1) {  //* load the file
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path);
+    return false;
+  }
+  std::cout << "Loaded " << pcd_map_ptr->width * pcd_map_ptr->height << " data points."
+            << std::endl;
+  return true;
+}
+
+double getMinHeightAroundPoint(
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd_map_ptr,
+  const pcl::KdTreeFLANN<pcl::PointXYZ> & kdtree, const pcl::PointXYZ & search_pt,
+  const double search_radius3d, const double search_radius2d)
+{
+  std::vector<int> pointIdxRadiusSearch;
+  std::vector<float> pointRadiusSquaredDistance;
+  if (
+    kdtree.radiusSearch(
+      search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) {
+    std::cout << "no points found within 3d radius " << search_radius3d << std::endl;
+    return search_pt.z;
+  }
+
+  double min_height = std::numeric_limits<double>::max();
+  bool found = false;
+
+  for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) {
+    std::size_t pt_idx = pointIdxRadiusSearch.at(i);
+    const auto pt = pcd_map_ptr->points.at(pt_idx);
+    if (pt.z > min_height) {
+      continue;
+    }
+    double distance2d = std::hypot(pt.x - search_pt.x, pt.y - search_pt.y);
+    if (distance2d < search_radius2d) {
+      found = true;
+      min_height = pt.z;
+    }
+  }
+  if (!found) {
+    min_height = search_pt.z;
+  }
+  return min_height;
+}
+
+bool exists(std::unordered_set<lanelet::Id> & set, lanelet::Id element)
+{
+  return std::find(set.begin(), set.end(), element) != set.end();
+}
+
+void adjustHeight(
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  std::unordered_set<lanelet::Id> done;
+  double search_radius2d = 0.5;
+  double search_radius3d = 10;
+
+  pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
+  kdtree.setInputCloud(pcd_map_ptr);
+
+  for (lanelet::Lanelet & llt : lanelet_map_ptr->laneletLayer) {
+    for (lanelet::Point3d & pt : llt.leftBound()) {
+      if (exists(done, pt.id())) {
+        continue;
+      }
+      pcl::PointXYZ pcl_pt;
+      pcl_pt.x = pt.x();
+      pcl_pt.y = pt.y();
+      pcl_pt.z = pt.z();
+      double min_height =
+        getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d);
+      std::cout << "moving from " << pt.z() << " to " << min_height << std::endl;
+      pt.z() = min_height;
+      done.insert(pt.id());
+    }
+    for (lanelet::Point3d & pt : llt.rightBound()) {
+      if (exists(done, pt.id())) {
+        continue;
+      }
+      pcl::PointXYZ pcl_pt;
+      pcl_pt.x = pt.x();
+      pcl_pt.y = pt.y();
+      pcl_pt.z = pt.z();
+      double min_height =
+        getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d);
+      std::cout << "moving from " << pt.z() << " to " << min_height << std::endl;
+      pt.z() = min_height;
+      done.insert(pt.id());
+    }
+  }
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto node = rclcpp::Node::make_shared("lanelet_map_height_adjuster");
+
+  const auto llt_map_path = node->declare_parameter<std::string>("llt_map_path");
+  const auto pcd_map_path = node->declare_parameter<std::string>("pcd_map_path");
+  const auto llt_output_path = node->declare_parameter<std::string>("llt_output_path");
+
+  lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
+  lanelet::projection::MGRSProjector projector;
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_map_ptr(new pcl::PointCloud<pcl::PointXYZ>);
+
+  if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) {
+    return EXIT_FAILURE;
+  }
+  if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) {
+    return EXIT_FAILURE;
+  }
+
+  adjustHeight(pcd_map_ptr, llt_map_ptr);
+  lanelet::write(llt_output_path, *llt_map_ptr, projector);
+
+  rclcpp::shutdown();
+
+  return 0;
+}