diff --git a/build_depends.repos b/build_depends.repos index 60bc6944..5ff343a0 100644 --- a/build_depends.repos +++ b/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.1 # universe universe/autoware.universe: type: git diff --git a/map/autoware_lanelet2_map_utils/CMakeLists.txt b/map/autoware_lanelet2_map_utils/CMakeLists.txt new file mode 100644 index 00000000..0785647d --- /dev/null +++ b/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 +) diff --git a/map/autoware_lanelet2_map_utils/README.md b/map/autoware_lanelet2_map_utils/README.md new file mode 100644 index 00000000..6f4fc063 --- /dev/null +++ b/map/autoware_lanelet2_map_utils/README.md @@ -0,0 +1,3 @@ +# autoware_lanelet2_map_utils + +This package is for preprocessing the lanelet map. diff --git a/map/autoware_lanelet2_map_utils/config/fix_z_value_by_pcd.param.yaml b/map/autoware_lanelet2_map_utils/config/fix_z_value_by_pcd.param.yaml new file mode 100644 index 00000000..de86ef79 --- /dev/null +++ b/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) diff --git a/map/autoware_lanelet2_map_utils/config/transform_maps.param.yaml b/map/autoware_lanelet2_map_utils/config/transform_maps.param.yaml new file mode 100644 index 00000000..548c2805 --- /dev/null +++ b/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 diff --git a/map/autoware_lanelet2_map_utils/launch/fix_z_value_by_pcd.launch.xml b/map/autoware_lanelet2_map_utils/launch/fix_z_value_by_pcd.launch.xml new file mode 100644 index 00000000..cab1ee9e --- /dev/null +++ b/map/autoware_lanelet2_map_utils/launch/fix_z_value_by_pcd.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/map/autoware_lanelet2_map_utils/launch/transform_maps.launch.xml b/map/autoware_lanelet2_map_utils/launch/transform_maps.launch.xml new file mode 100644 index 00000000..8976eeec --- /dev/null +++ b/map/autoware_lanelet2_map_utils/launch/transform_maps.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/map/autoware_lanelet2_map_utils/package.xml b/map/autoware_lanelet2_map_utils/package.xml new file mode 100644 index 00000000..f66897da --- /dev/null +++ b/map/autoware_lanelet2_map_utils/package.xml @@ -0,0 +1,29 @@ + + + + autoware_lanelet2_map_utils + 0.1.0 + The autoware_lanelet2_map_utils package + Yamato Ando + 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/autoware_lanelet2_map_utils/schema/transform_maps.schema.json b/map/autoware_lanelet2_map_utils/schema/transform_maps.schema.json new file mode 100644 index 00000000..78fb3952 --- /dev/null +++ b/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": ["/**"] +} diff --git a/map/autoware_lanelet2_map_utils/src/fix_lane_change_tags.cpp b/map/autoware_lanelet2_map_utils/src/fix_lane_change_tags.cpp new file mode 100644 index 00000000..be9ec44a --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +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("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 (!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; +} diff --git a/map/autoware_lanelet2_map_utils/src/fix_z_value_by_pcd.cpp b/map/autoware_lanelet2_map_utils/src/fix_z_value_by_pcd.cpp new file mode 100644 index 00000000..faedd513 --- /dev/null +++ b/map/autoware_lanelet2_map_utils/src/fix_z_value_by_pcd.cpp @@ -0,0 +1,168 @@ +// 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 + +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; +} + +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()); + } + } +} +} // namespace autoware::lanelet2_map_utils + +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 (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + if (!autoware::lanelet2_map_utils::load_pcd_map(pcd_map_path, pcd_map_ptr)) { + return EXIT_FAILURE; + } + + autoware::lanelet2_map_utils::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/autoware_lanelet2_map_utils/src/merge_close_lines.cpp b/map/autoware_lanelet2_map_utils/src/merge_close_lines.cpp new file mode 100644 index 00000000..e963ff88 --- /dev/null +++ b/map/autoware_lanelet2_map_utils/src/merge_close_lines.cpp @@ -0,0 +1,191 @@ +// 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 + +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::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; + } + } + } +} +} // namespace autoware::lanelet2_map_utils + +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 (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + autoware::lanelet2_map_utils::merge_lines(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/map/autoware_lanelet2_map_utils/src/merge_close_points.cpp b/map/autoware_lanelet2_map_utils/src/merge_close_points.cpp new file mode 100644 index 00000000..46ced8a2 --- /dev/null +++ b/map/autoware_lanelet2_map_utils/src/merge_close_points.cpp @@ -0,0 +1,120 @@ +// 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 + +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::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()); + } + } + } +} +} // namespace autoware::lanelet2_map_utils + +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 (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + autoware::lanelet2_map_utils::merge_points(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/map/autoware_lanelet2_map_utils/src/remove_unreferenced_geometry.cpp b/map/autoware_lanelet2_map_utils/src/remove_unreferenced_geometry.cpp new file mode 100644 index 00000000..9333afa3 --- /dev/null +++ b/map/autoware_lanelet2_map_utils/src/remove_unreferenced_geometry.cpp @@ -0,0 +1,98 @@ +// 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 + +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::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; +} +} // namespace autoware::lanelet2_map_utils + +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 (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + autoware::lanelet2_map_utils::remove_unreferenced_geometry(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/map/autoware_lanelet2_map_utils/src/transform_maps.cpp b/map/autoware_lanelet2_map_utils/src/transform_maps.cpp new file mode 100644 index 00000000..59f1ff3a --- /dev/null +++ b/map/autoware_lanelet2_map_utils/src/transform_maps.cpp @@ -0,0 +1,154 @@ +// 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 + +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; +} + +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; +} +} // namespace autoware::lanelet2_map_utils + +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 (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + if (!autoware::lanelet2_map_utils::load_pcd_map(pcd_map_path, pcd_map_ptr)) { + return EXIT_FAILURE; + } + Eigen::Affine3d affine = + autoware::lanelet2_map_utils::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; + + autoware::lanelet2_map_utils::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; +}