From 31d39b539cbadfc8a325d5b7e85755123138a6e2 Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Tue, 17 Sep 2024 14:48:19 +0900 Subject: [PATCH] fix(lanelet2_map_preprocessor): remove lanelet2_map_preprocessor package (#8801) * move lanelet2_map_preprocessor into autoware_tools Signed-off-by: a-maumau * place to correct dir Signed-off-by: a-maumau * fix typo Co-authored-by: Yamato Ando Signed-off-by: a-maumau --------- Signed-off-by: a-maumau Co-authored-by: Yamato Ando --- .../lanelet2_map_preprocessor/CMakeLists.txt | 34 ---- map/util/lanelet2_map_preprocessor/README.md | 1 + .../config/fix_z_value_by_pcd.param.yaml | 5 - .../config/transform_maps.param.yaml | 12 -- .../launch/fix_z_value_by_pcd.launch.xml | 6 - .../launch/transform_maps.launch.xml | 6 - .../lanelet2_map_preprocessor/package.xml | 30 --- .../schema/transform_maps.schema.json | 75 ------- .../src/fix_lane_change_tags.cpp | 101 ---------- .../src/fix_z_value_by_pcd.cpp | 165 --------------- .../src/merge_close_lines.cpp | 188 ------------------ .../src/merge_close_points.cpp | 117 ----------- .../src/remove_unreferenced_geometry.cpp | 95 --------- .../src/transform_maps.cpp | 150 -------------- 14 files changed, 1 insertion(+), 984 deletions(-) delete mode 100644 map/util/lanelet2_map_preprocessor/CMakeLists.txt create mode 100644 map/util/lanelet2_map_preprocessor/README.md delete mode 100644 map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml delete mode 100644 map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml delete mode 100644 map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml delete mode 100644 map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml delete mode 100644 map/util/lanelet2_map_preprocessor/package.xml delete mode 100644 map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json delete mode 100644 map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp delete mode 100644 map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp delete mode 100644 map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp delete mode 100644 map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp delete mode 100644 map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp delete mode 100644 map/util/lanelet2_map_preprocessor/src/transform_maps.cpp diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt deleted file mode 100644 index 982a262455efa..0000000000000 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -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 - config - launch -) diff --git a/map/util/lanelet2_map_preprocessor/README.md b/map/util/lanelet2_map_preprocessor/README.md new file mode 100644 index 0000000000000..49a66973305e5 --- /dev/null +++ b/map/util/lanelet2_map_preprocessor/README.md @@ -0,0 +1 @@ +This package has been moved to . diff --git a/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml b/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml deleted file mode 100644 index de86ef7936d74..0000000000000 --- a/map/util/lanelet2_map_preprocessor/config/fix_z_value_by_pcd.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - llt_map_path: $(var llt_map_path) - pcd_map_path: $(var pcd_map_path) - llt_output_path: $(var llt_output_path) diff --git a/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml b/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml deleted file mode 100644 index 548c28055c834..0000000000000 --- a/map/util/lanelet2_map_preprocessor/config/transform_maps.param.yaml +++ /dev/null @@ -1,12 +0,0 @@ -/**: - 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 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 deleted file mode 100644 index 5b2aa2e425184..0000000000000 --- a/map/util/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml b/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml deleted file mode 100644 index 950593771878c..0000000000000 --- a/map/util/lanelet2_map_preprocessor/launch/transform_maps.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml deleted file mode 100644 index b0c371ac9eeed..0000000000000 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - lanelet2_map_preprocessor - 0.1.0 - The lanelet2_map_preprocessor package - Yamato Ando - Kento Yabuuchi - Masahiro Sakamoto - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - Ryu Yamamoto - Apache License 2.0 - Ryohsuke Mitsudome - - ament_cmake_auto - autoware_cmake - - autoware_lanelet2_extension - libpcl-all-dev - rclcpp - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json b/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json deleted file mode 100644 index 78fb3952d44ce..0000000000000 --- a/map/util/lanelet2_map_preprocessor/schema/transform_maps.schema.json +++ /dev/null @@ -1,75 +0,0 @@ -{ - "$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": ["/**"] -} 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 deleted file mode 100644 index 571c2e91c53a5..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// 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 -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -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"; - } - } -} - -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("llt_map_path"); - const auto output_path = node->declare_parameter("output_path"); - - lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); - lanelet::projection::MGRSProjector projector; - - if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { - return EXIT_FAILURE; - } - - fix_tags(llt_map_ptr); - lanelet::write(output_path, *llt_map_ptr, projector); - - rclcpp::shutdown(); - - return 0; -} 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 deleted file mode 100644 index af565208c5f71..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ /dev/null @@ -1,165 +0,0 @@ -// 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 -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -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; -} - -bool load_pcd_map( - 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 - 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 get_min_height_around_point( - const pcl::PointCloud::Ptr & pcd_map_ptr, - const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, - const double search_radius3d, const double search_radius2d) -{ - std::vector point_idx_radius_search; - std::vector point_radius_squared_distance; - if ( - kdtree.radiusSearch( - search_pt, search_radius3d, point_idx_radius_search, point_radius_squared_distance) <= 0) { - std::cout << "no points found within 3d radius " << search_radius3d << std::endl; - return search_pt.z; - } - - double min_height = std::numeric_limits::max(); - bool found = false; - - for (auto pt_idx : point_idx_radius_search) { - 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; -} - -void adjust_height( - const pcl::PointCloud::Ptr & pcd_map_ptr, - const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - std::unordered_set done; - double search_radius2d = 0.5; - double search_radius3d = 10; - - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(pcd_map_ptr); - - for (lanelet::Lanelet & llt : lanelet_map_ptr->laneletLayer) { - for (lanelet::Point3d & pt : llt.leftBound()) { - if (done.find(pt.id()) != done.end()) { - continue; - } - pcl::PointXYZ pcl_pt; - pcl_pt.x = static_cast(pt.x()); - pcl_pt.y = static_cast(pt.y()); - pcl_pt.z = static_cast(pt.z()); - double min_height = - get_min_height_around_point(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 (done.find(pt.id()) != done.end()) { - continue; - } - pcl::PointXYZ pcl_pt; - pcl_pt.x = static_cast(pt.x()); - pcl_pt.y = static_cast(pt.y()); - pcl_pt.z = static_cast(pt.z()); - double min_height = - get_min_height_around_point(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("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; - - pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - - if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { - return EXIT_FAILURE; - } - if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { - return EXIT_FAILURE; - } - - adjust_height(pcd_map_ptr, llt_map_ptr); - lanelet::write(llt_output_path, *llt_map_ptr, projector); - - rclcpp::shutdown(); - - return 0; -} diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp deleted file mode 100644 index 0e40d04a2cec3..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ /dev/null @@ -1,188 +0,0 @@ -// 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 -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -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::LineStrings3d convert_line_layer_to_line_strings( - const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - lanelet::LineStrings3d lines; - std::copy( - lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), - std::back_inserter(lines)); - return lines; -} - -lanelet::ConstPoint3d get3d_point_from2d_arc_length( - const lanelet::ConstLineString3d & line, const double s) -{ - double accumulated_distance2d = 0; - if (line.size() < 2) { - return lanelet::Point3d(); - } - auto prev_pt = line.front(); - for (size_t i = 1; i < line.size(); i++) { - const auto & pt = line[i]; - double distance2d = - lanelet::geometry::distance2d(lanelet::utils::to2D(prev_pt), lanelet::utils::to2D(pt)); - if (accumulated_distance2d + distance2d >= s) { - double ratio = (s - accumulated_distance2d) / distance2d; - auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; - std::cout << interpolated_pt << std::endl; - return lanelet::ConstPoint3d{ - lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()}; - } - accumulated_distance2d += distance2d; - prev_pt = pt; - } - RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed"); - return {}; -} - -bool are_lines_same( - const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) -{ - bool same_ends = false; - if (line1.front() == line2.front() && line1.back() == line2.back()) { - same_ends = true; - } - if (line1.front() == line2.back() && line1.back() == line2.front()) { - same_ends = true; - } - if (!same_ends) { - return false; - } - - double sum_distance = - std::accumulate(line1.begin(), line1.end(), 0.0, [&line2](double sum, const auto & pt) { - return sum + boost::geometry::distance(pt.basicPoint(), line2); - }); - sum_distance += - std::accumulate(line2.begin(), line2.end(), 0.0, [&line1](double sum, const auto & pt) { - return sum + boost::geometry::distance(pt.basicPoint(), line1); - }); - - double avg_distance = sum_distance / static_cast(line1.size() + line2.size()); - std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; - return avg_distance < 1.0; -} - -lanelet::BasicPoint3d get_closest_point_on_line( - const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) -{ - auto arc_coordinate = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(line), lanelet::utils::to2D(search_point)); - std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; - return get3d_point_from2d_arc_length(line, arc_coordinate.length).basicPoint(); -} - -lanelet::LineString3d merge_two_lines( - const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) -{ - lanelet::Points3d new_points; - for (const auto & p1 : line1) { - const lanelet::BasicPoint3d & p1_basic_point = p1.basicPoint(); - lanelet::BasicPoint3d p2_basic_point = get_closest_point_on_line(p1, line2); - lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; - lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); - new_points.push_back(new_point); - } - return lanelet::LineString3d{lanelet::utils::getId(), new_points}; -} - -void copy_data(lanelet::LineString3d & dst, const lanelet::LineString3d & src) -{ - dst.clear(); - for (const lanelet::ConstPoint3d & pt : src) { - dst.push_back(static_cast(pt)); - } -} - -void merge_lines(lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - auto lines = convert_line_layer_to_line_strings(lanelet_map_ptr); - - for (size_t i = 0; i < lines.size(); i++) { - auto line_i = lines.at(i); - for (size_t j = 0; j < i; j++) { - auto line_j = lines.at(j); - if (are_lines_same(line_i, line_j)) { - auto merged_line = merge_two_lines(line_i, line_j); - copy_data(line_i, merged_line); - copy_data(line_j, merged_line); - line_i.setId(line_j.id()); - std::cout << line_j << " " << line_i << std::endl; - // lanelet_map_ptr->add(merged_line); - for (lanelet::Point3d & pt : merged_line) { - lanelet_map_ptr->add(pt); - } - break; - } - } - } -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - auto node = rclcpp::Node::make_shared("merge_close_lines"); - - 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; - - if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { - return EXIT_FAILURE; - } - - merge_lines(llt_map_ptr); - lanelet::write(output_path, *llt_map_ptr, projector); - - rclcpp::shutdown(); - - return 0; -} diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp deleted file mode 100644 index 3cbbffc702019..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// 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 -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -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::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - lanelet::Points3d points; - std::copy( - lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), - std::back_inserter(points)); - return points; -} - -// lanelet::LineString3d mergeClosePoints(const lanelet::ConstLineString3d& line1, const -// lanelet::ConstLineString3d& line2) -// { -// lanelet::Points3d new_points; -// for (const auto& p1 : line1) -// { -// p1_basic_point = p1.basicPoint(); -// lanelet::BasicPoint3d p2 = getClosestPointOnLine(line2, p1); -// lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point)/2; -// lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); -// new_points.push_back(new_point); -// } -// return lanelet::LineString3d(lanelet::utils::getId(), new_points); -// } - -void merge_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - const auto & points = convert_points_layer_to_points(lanelet_map_ptr); - - for (size_t i = 0; i < points.size(); i++) { - auto point_i = points.at(i); - for (size_t j = 0; j < i; j++) { - auto point_j = points.at(j); - - double distance = boost::geometry::distance(point_i, point_j); - if (distance < 0.1) { - const auto new_point = (point_i.basicPoint() + point_j.basicPoint()) / 2; - // const auto new_pt3d = lanelet::Point3d(lanelet::utils::getId(), new_point); - point_i.x() = new_point.x(); - point_i.y() = new_point.y(); - point_i.z() = new_point.z(); - point_j.x() = new_point.x(); - point_j.y() = new_point.y(); - point_j.z() = new_point.z(); - point_i.setId(point_j.id()); - } - } - } -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - auto node = rclcpp::Node::make_shared("merge_close_points"); - - 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; - - if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { - return EXIT_FAILURE; - } - - merge_points(llt_map_ptr); - lanelet::write(output_path, *llt_map_ptr, projector); - - rclcpp::shutdown(); - - return 0; -} diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp deleted file mode 100644 index 3ab10551e36f9..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// 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 -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -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::Points3d convert_points_layer_to_points(const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - lanelet::Points3d points; - std::copy( - lanelet_map_ptr->pointLayer.begin(), lanelet_map_ptr->pointLayer.end(), - std::back_inserter(points)); - return points; -} - -lanelet::LineStrings3d convert_line_layer_to_line_strings( - const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - lanelet::LineStrings3d lines; - std::copy( - lanelet_map_ptr->lineStringLayer.begin(), lanelet_map_ptr->lineStringLayer.end(), - std::back_inserter(lines)); - return lines; -} - -void remove_unreferenced_geometry(lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); - for (const auto & llt : lanelet_map_ptr->laneletLayer) { - new_map->add(llt); - } - lanelet_map_ptr = new_map; -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - auto node = rclcpp::Node::make_shared("remove_unreferenced_geometry"); - - 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; - - if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { - return EXIT_FAILURE; - } - remove_unreferenced_geometry(llt_map_ptr); - lanelet::write(output_path, *llt_map_ptr, projector); - - rclcpp::shutdown(); - - return 0; -} diff --git a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp b/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp deleted file mode 100644 index d55e03c7faee5..0000000000000 --- a/map/util/lanelet2_map_preprocessor/src/transform_maps.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// 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 -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -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; -} - -bool load_pcd_map( - 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 - 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; -} - -void transform_maps( - const pcl::PointCloud::Ptr & pcd_map_ptr, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const Eigen::Affine3d & affine) -{ - { - for (lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { - Eigen::Vector3d eigen_pt(pt.x(), pt.y(), pt.z()); - auto transformed_pt = affine * eigen_pt; - pt.x() = transformed_pt.x(); - pt.y() = transformed_pt.y(); - pt.z() = transformed_pt.z(); - } - } - - { - for (auto & pt : pcd_map_ptr->points) { - Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); - auto transformed_pt = affine * eigen_pt; - pt.x = static_cast(transformed_pt.x()); - pt.y = static_cast(transformed_pt.y()); - pt.z = static_cast(transformed_pt.z()); - } - } -} - -Eigen::Affine3d create_affine_matrix_from_xyzrpy( - const double x, const double y, const double z, const double roll, const double pitch, - const double yaw) -{ - double roll_rad = roll * M_PI / 180.0; - double pitch_rad = pitch * M_PI / 180.0; - double yaw_rad = yaw * M_PI / 180.0; - - Eigen::Translation trans(x, y, z); - Eigen::Matrix3d rot; - rot = Eigen::AngleAxisd(yaw_rad, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(pitch_rad, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(roll_rad, Eigen::Vector3d::UnitX()); - return trans * rot; -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - 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"); - const auto y = node->declare_parameter("y"); - const auto z = node->declare_parameter("z"); - const auto roll = node->declare_parameter("roll"); - const auto pitch = node->declare_parameter("pitch"); - const auto yaw = node->declare_parameter("yaw"); - - std::cout << "transforming maps with following parameters" << std::endl - << "x " << x << std::endl - << "y " << y << std::endl - << "z " << z << std::endl - << "roll " << roll << std::endl - << "pitch " << pitch << std::endl - << "yaw " << yaw << std::endl; - - lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); - lanelet::projection::MGRSProjector projector; - - pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); - - if (!load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { - return EXIT_FAILURE; - } - if (!load_pcd_map(pcd_map_path, pcd_map_ptr)) { - return EXIT_FAILURE; - } - Eigen::Affine3d affine = create_affine_matrix_from_xyzrpy(x, y, z, roll, pitch, yaw); - - const auto mgrs_grid = - node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); - std::cout << "using mgrs grid: " << mgrs_grid << std::endl; - - transform_maps(pcd_map_ptr, llt_map_ptr, affine); - lanelet::write(llt_output_path, *llt_map_ptr, projector); - pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); - - rclcpp::shutdown(); - - return 0; -}