feat(lanelet2_map_preprocessor): port lanelet2_map_preprocessor from …

…ROS1 to ROS2 (tier4#458)

* feat(lanelet2_map_preprocessor): port lanelet2_map_preprocessor from ROS1 to ROS2

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

map/util/lanelet2_map_preprocessor/CMakeLists.txt

@@ -26,4 +26,11 @@ 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)
 
-ament_auto_package()
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

map/util/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>

map/util/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>

map/util/lanelet2_map_preprocessor/package.xml

@@ -13,6 +13,9 @@
   <depend>pcl_ros</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>

map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp

@@ -26,13 +26,6 @@
 #include <unordered_set>
 #include <vector>
 
-void printUsage()
-{
-  std::cerr << "Please set following private parameters:" << std::endl
-            << "llt_map_path" << std::endl
-            << "output_path" << std::endl;
-}
-
 using lanelet::utils::getId;
 using lanelet::utils::to2D;
 
@@ -45,7 +38,7 @@ bool loadLaneletMap(
   lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
 
   for (const auto & error : errors) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("fix_lane_change_tags"), error);
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
   }
   if (!errors.empty()) {
     return false;
@@ -93,20 +86,11 @@ void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr)
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::Node node("fix_lane_change_tags");
 
-  if (!node.has_parameter("llt_map_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
-  if (!node.has_parameter("output_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
+  auto node = rclcpp::Node::make_shared("fix_lane_change_tags");
 
-  std::string llt_map_path, output_path;
-  node.get_parameter("llt_map_path", llt_map_path);
-  node.get_parameter("output_path", output_path);
+  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;

map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp

@@ -27,14 +27,6 @@
 #include <unordered_set>
 #include <vector>
 
-void printUsage()
-{
-  std::cerr << "Please set following private parameters:" << std::endl
-            << "llt_map_path" << std::endl
-            << "pcd_map_path" << std::endl
-            << "output_path" << std::endl;
-}
-
 bool loadLaneletMap(
   const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr,
   lanelet::Projector & projector)
@@ -44,7 +36,7 @@ bool loadLaneletMap(
   lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
 
   for (const auto & error : errors) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("fix_z_value_by_pcd"), error);
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
   }
   if (!errors.empty()) {
     return false;
@@ -56,7 +48,7 @@ bool loadLaneletMap(
 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
-    PCL_ERROR("Couldn't read file test_pcd.pcd \n");
+    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."
@@ -149,25 +141,12 @@ void adjustHeight(
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::Node node("fix_z_value_by_pcd");
 
-  if (!node.has_parameter("llt_map_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
-  if (!node.has_parameter("pcd_map_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
-  if (!node.has_parameter("output_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
+  auto node = rclcpp::Node::make_shared("lanelet_map_height_adjuster");
 
-  std::string llt_map_path, pcd_map_path, output_path;
-  node.get_parameter("llt_map_path", llt_map_path);
-  node.get_parameter("pcd_map_path", pcd_map_path);
-  node.get_parameter("output_path", output_path);
+  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;
@@ -182,7 +161,7 @@ int main(int argc, char * argv[])
   }
 
   adjustHeight(pcd_map_ptr, llt_map_ptr);
-  lanelet::write(output_path, *llt_map_ptr, projector);
+  lanelet::write(llt_output_path, *llt_map_ptr, projector);
 
   rclcpp::shutdown();
 

map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp

@@ -26,13 +26,6 @@
 #include <unordered_set>
 #include <vector>
 
-void printUsage()
-{
-  std::cerr << "Please set following private parameters:" << std::endl
-            << "llt_map_path" << std::endl
-            << "output_path" << std::endl;
-}
-
 using lanelet::utils::getId;
 using lanelet::utils::to2D;
 
@@ -45,7 +38,7 @@ bool loadLaneletMap(
   lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
 
   for (const auto & error : errors) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("merge_close_lines"), error);
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
   }
   if (!errors.empty()) {
     return false;
@@ -187,20 +180,11 @@ void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr)
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::Node node("merge_close_lines");
 
-  if (!node.has_parameter("llt_map_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
-  if (!node.has_parameter("output_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
+  auto node = rclcpp::Node::make_shared("merge_close_lines");
 
-  std::string llt_map_path, output_path;
-  node.get_parameter("llt_map_path", llt_map_path);
-  node.get_parameter("output_path", output_path);
+  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;

map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp

@@ -25,13 +25,6 @@
 #include <unordered_set>
 #include <vector>
 
-void printUsage()
-{
-  std::cerr << "Please set following private parameters:" << std::endl
-            << "llt_map_path" << std::endl
-            << "output_path" << std::endl;
-}
-
 bool loadLaneletMap(
   const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr,
   lanelet::Projector & projector)
@@ -41,7 +34,7 @@ bool loadLaneletMap(
   lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
 
   for (const auto & error : errors) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("merge_close_points"), error);
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
   }
   if (!errors.empty()) {
     return false;
@@ -107,20 +100,11 @@ void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr)
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::Node node("merge_close_points");
 
-  if (!node.has_parameter("llt_map_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
-  if (!node.has_parameter("output_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
+  auto node = rclcpp::Node::make_shared("merge_close_points");
 
-  std::string llt_map_path, pcd_map_path, output_path;
-  node.get_parameter("llt_map_path", llt_map_path);
-  node.get_parameter("output_path", output_path);
+  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;

map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp

@@ -25,13 +25,6 @@
 #include <unordered_set>
 #include <vector>
 
-void printUsage()
-{
-  std::cerr << "Please set following private parameters:" << std::endl
-            << "llt_map_path" << std::endl
-            << "output_path" << std::endl;
-}
-
 bool loadLaneletMap(
   const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr,
   lanelet::Projector & projector)
@@ -41,7 +34,7 @@ bool loadLaneletMap(
   lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
 
   for (const auto & error : errors) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger("remove_unreferenced_geometry"), error);
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
   }
   if (!errors.empty()) {
     return false;
@@ -85,20 +78,11 @@ void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr)
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::Node node("remove_unreferenced_geometry");
 
-  if (!node.has_parameter("llt_map_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
-  if (!node.has_parameter("output_path")) {
-    printUsage();
-    return EXIT_FAILURE;
-  }
+  auto node = rclcpp::Node::make_shared("remove_unreferenced_geometry");
 
-  std::string llt_map_path, pcd_map_path, output_path;
-  node.get_parameter("llt_map_path", llt_map_path);
-  node.get_parameter("output_path", output_path);
+  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;