From fccf8570c8a237e9969f0a5187dbab4c4053db96 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 4 May 2022 17:29:52 +0900 Subject: [PATCH] feat(lanelet2_map_preprocessor): port lanelet2_map_preprocessor from ROS1 to ROS2 (#458) * feat(lanelet2_map_preprocessor): port lanelet2_map_preprocessor from ROS1 to ROS2 Signed-off-by: mitsudome-r Signed-off-by: Kenji Miyake * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../lanelet2_map_preprocessor/CMakeLists.txt | 9 ++- .../launch/fix_z_value_by_pcd.launch.xml | 12 ++++ .../launch/transform_maps.launch.xml | 26 ++++++++ .../lanelet2_map_preprocessor/package.xml | 3 + .../src/fix_lane_change_tags.cpp | 24 ++----- .../src/fix_z_value_by_pcd.cpp | 35 ++-------- .../src/merge_close_lines.cpp | 24 ++----- .../src/merge_close_points.cpp | 24 ++----- .../src/remove_unreferenced_geometry.cpp | 24 ++----- .../src/transform_maps.cpp | 64 +++++-------------- 10 files changed, 88 insertions(+), 157 deletions(-) create mode 100644 map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml create mode 100644 map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt index 0319b17f625a5..13d17586553f9 100644 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ b/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 +) diff --git a/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml b/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml new file mode 100644 index 0000000000000..82021ff25d240 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml new file mode 100644 index 0000000000000..902ca982dace1 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 0b93ec4846e6d..41b4243e56ba6 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -13,6 +13,9 @@ pcl_ros rclcpp + ament_lint_auto + autoware_lint_common + ament_cmake diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index 87fcba3c10914..601e8dfa0117f 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -26,13 +26,6 @@ #include #include -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("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index 128c7ae58cc1a..b0eca472ee1f3 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -27,14 +27,6 @@ #include #include -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::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(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("llt_map_path"); + const auto pcd_map_path = node->declare_parameter("pcd_map_path"); + const auto llt_output_path = node->declare_parameter("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(); diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index 69305049d4f79..a9e45b3b31785 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -26,13 +26,6 @@ #include #include -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("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index 6a47a420ca942..ab77b18493120 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -25,13 +25,6 @@ #include #include -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("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index 392824efd8840..ca488b3acb790 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -25,13 +25,6 @@ #include #include -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("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); lanelet::projection::MGRSProjector projector; diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp index a1decc717ee35..987f559be1e8e 100644 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -27,15 +27,6 @@ #include #include -void printUsage() -{ - std::cerr << "Please set following private parameters:" << std::endl - << "llt_map_path" << std::endl - << "pcd_map_path" << std::endl - << "llt_output_path" << std::endl - << "pcd_output_path" << std::endl; -} - bool loadLaneletMap( const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, lanelet::Projector & projector) @@ -45,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("transform_maps"), error); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); } if (!errors.empty()) { return false; @@ -57,7 +48,7 @@ bool loadLaneletMap( bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) { if (pcl::io::loadPCDFile(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." @@ -109,35 +100,19 @@ Eigen::Affine3d createAffineMatrixFromXYZRPY( int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::Node node("transform_maps"); - 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("llt_output_path")) { - printUsage(); - return EXIT_FAILURE; - } - if (!node.has_parameter("pcd_output_path")) { - printUsage(); - return EXIT_FAILURE; - } - std::string llt_map_path, pcd_map_path, llt_output_path, pcd_output_path; - node.get_parameter("llt_map_path", llt_map_path); - node.get_parameter("pcd_map_path", pcd_map_path); - node.get_parameter("llt_output_path", llt_output_path); - node.get_parameter("pcd_output_path", pcd_output_path); - const double x = node.declare_parameter("x", 0.0); - const double y = node.declare_parameter("y", 0.0); - const double z = node.declare_parameter("z", 0.0); - const double roll = node.declare_parameter("roll", 0.0); - const double pitch = node.declare_parameter("pitch", 0.0); - const double yaw = node.declare_parameter("yaw", 0.0); + auto node = rclcpp::Node::make_shared("transform_maps"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto pcd_map_path = node->declare_parameter("pcd_map_path"); + const auto llt_output_path = node->declare_parameter("llt_output_path"); + const auto pcd_output_path = node->declare_parameter("pcd_output_path"); + const auto x = node->declare_parameter("x", 0.0); + const auto y = node->declare_parameter("y", 0.0); + const auto z = node->declare_parameter("z", 0.0); + const auto roll = node->declare_parameter("roll", 0.0); + const auto pitch = node->declare_parameter("pitch", 0.0); + const auto yaw = node->declare_parameter("yaw", 0.0); std::cout << "transforming maps with following parameters" << std::endl << "x " << x << std::endl @@ -160,15 +135,8 @@ int main(int argc, char * argv[]) } Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); - std::string mgrs_grid; - if (node.has_parameter("mgrs_grid")) { - node.get_parameter("mgrs_grid", mgrs_grid); - projector.setMGRSCode(mgrs_grid); - } else { - std::cout << "no mgrs code set. using last projected grid instead" << std::endl; - mgrs_grid = projector.getProjectedMGRSGrid(); - } - + const auto mgrs_grid = + node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); std::cout << "using mgrs grid: " << mgrs_grid << std::endl; transformMaps(pcd_map_ptr, llt_map_ptr, affine);