Skip to content

Commit 5bd5865

Browse files
a-maumauYamatoAndo
a-maumau
and
YamatoAndo
authored
feat(map): port lanelet2_map_preprocessor from autoware.universe (#112)
* import lanelet2_map_preprocessor Signed-off-by: a-maumau <[email protected]> * mod version Signed-off-by: a-maumau <[email protected]> * mod maintainer Signed-off-by: a-maumau <[email protected]> Co-authored-by: Yamato Ando <[email protected]> * fix company name Signed-off-by: a-maumau <[email protected]> Co-authored-by: Yamato Ando <[email protected]> * change tier to TIER Signed-off-by: a-maumau <[email protected]> --------- Signed-off-by: a-maumau <[email protected]> Co-authored-by: Yamato Ando <[email protected]>
1 parent 72e5883 commit 5bd5865

15 files changed

+1009
-0
lines changed

‎build_depends.repos

+4
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,10 @@ repositories:
2424
type: git
2525
url: https://github.com/autowarefoundation/autoware_internal_msgs.git
2626
version: main
27+
core/autoware_lanelet2_extension:
28+
type: git
29+
url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git
30+
version: 0.6.1
2731
# universe
2832
universe/autoware.universe:
2933
type: git

‎map/autoware_lanelet2_map_utils/CMakeLists.txt

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_lanelet2_map_utils)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
find_package(PCL REQUIRED COMPONENTS common io kdtree)
8+
9+
include_directories(
10+
include
11+
SYSTEM
12+
${PCL_INCLUDE_DIRS}
13+
)
14+
15+
link_libraries(
16+
${PCL_LIBRARIES}
17+
)
18+
19+
ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp)
20+
ament_auto_add_executable(transform_maps src/transform_maps.cpp)
21+
ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp)
22+
ament_auto_add_executable(merge_close_points src/merge_close_points.cpp)
23+
ament_auto_add_executable(remove_unreferenced_geometry src/remove_unreferenced_geometry.cpp)
24+
ament_auto_add_executable(fix_lane_change_tags src/fix_lane_change_tags.cpp)
25+
26+
if(BUILD_TESTING)
27+
find_package(ament_lint_auto REQUIRED)
28+
ament_lint_auto_find_test_dependencies()
29+
endif()
30+
31+
ament_auto_package(INSTALL_TO_SHARE
32+
config
33+
launch
34+
)

‎map/autoware_lanelet2_map_utils/README.md

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# autoware_lanelet2_map_utils
2+
3+
This package is for preprocessing the lanelet map.

‎map/autoware_lanelet2_map_utils/config/fix_z_value_by_pcd.param.yaml

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
/**:
2+
ros__parameters:
3+
llt_map_path: $(var llt_map_path)
4+
pcd_map_path: $(var pcd_map_path)
5+
llt_output_path: $(var llt_output_path)

‎map/autoware_lanelet2_map_utils/config/transform_maps.param.yaml

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
/**:
2+
ros__parameters:
3+
llt_map_path: $(var llt_map_path)
4+
pcd_map_path: $(var pcd_map_path)
5+
llt_output_path: $(var llt_output_path)
6+
pcd_output_path: $(var pcd_output_path)
7+
x: 0.0
8+
y: 0.0
9+
z: 0.0
10+
roll: 0.0
11+
pitch: 0.0
12+
yaw: 0.0

‎map/autoware_lanelet2_map_utils/launch/fix_z_value_by_pcd.launch.xml

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<node pkg="autoware_lanelet2_map_utils" exec="fix_z_value_by_pcd" name="fix_z_value_by_pcd" output="screen">
4+
<param from="$(find-pkg-share autoware_lanelet2_map_utils)/config/fix_z_value_by_pcd.param.yaml" allow_substs="true"/>
5+
</node>
6+
</launch>

‎map/autoware_lanelet2_map_utils/launch/transform_maps.launch.xml

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<node pkg="autoware_lanelet2_map_utils" exec="transform_maps" name="transform_maps" output="screen">
4+
<param from="$(find-pkg-share autoware_lanelet2_map_utils)/config/transform_maps.param.yaml" allow_substs="true"/>
5+
</node>
6+
</launch>

‎map/autoware_lanelet2_map_utils/package.xml

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_lanelet2_map_utils</name>
5+
<version>0.1.0</version>
6+
<description>The autoware_lanelet2_map_utils package</description>
7+
<maintainer email="[email protected]">Yamato Ando</maintainer>
8+
<maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
9+
<maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
10+
<maintainer email="[email protected]">Taiki Yamada</maintainer>
11+
<maintainer email="[email protected]">Shintaro Sakoda</maintainer>
12+
<maintainer email="[email protected]">Ryu Yamamoto</maintainer>
13+
<license>Apache License 2.0</license>
14+
<author email="[email protected]">Ryohsuke Mitsudome</author>
15+
16+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
17+
<buildtool_depend>autoware_cmake</buildtool_depend>
18+
19+
<depend>autoware_lanelet2_extension</depend>
20+
<depend>libpcl-all-dev</depend>
21+
<depend>rclcpp</depend>
22+
23+
<test_depend>ament_lint_auto</test_depend>
24+
<test_depend>autoware_lint_common</test_depend>
25+
26+
<export>
27+
<build_type>ament_cmake</build_type>
28+
</export>
29+
</package>

‎map/autoware_lanelet2_map_utils/schema/transform_maps.schema.json

+75
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Transforming Maps",
4+
"type": "object",
5+
"definitions": {
6+
"transform_maps": {
7+
"type": "object",
8+
"properties": {
9+
"llt_map_path": {
10+
"type": "string",
11+
"description": "Path pointing to the input lanelet2 file",
12+
"default": ""
13+
},
14+
"pcd_map_path": {
15+
"type": "string",
16+
"description": "Path pointing to the input point cloud file",
17+
"default": ""
18+
},
19+
"llt_output_path": {
20+
"type": "string",
21+
"description": "Path pointing to the output lanelet2 file",
22+
"default": ""
23+
},
24+
"pcd_output_path": {
25+
"type": "string",
26+
"description": "Path pointing to the output point cloud file",
27+
"default": ""
28+
},
29+
"x": {
30+
"type": "number",
31+
"default": 0.0,
32+
"description": "x factor of Translation vector for transforming maps [m]"
33+
},
34+
"y": {
35+
"type": "number",
36+
"default": 0.0,
37+
"description": "y factor of Translation vector for transforming maps [m]"
38+
},
39+
"z": {
40+
"type": "number",
41+
"default": 0.0,
42+
"description": "z factor of Translation vector for transforming maps [m]"
43+
},
44+
"roll": {
45+
"type": "number",
46+
"default": 0.0,
47+
"description": "roll factor of Rotation vector for transforming maps [rad]"
48+
},
49+
"pitch": {
50+
"type": "number",
51+
"default": 0.0,
52+
"description": "pitch factor of Rotation vector for transforming maps [rad]"
53+
},
54+
"yaw": {
55+
"type": "number",
56+
"default": 0.0,
57+
"description": "yaw factor of Rotation vector for transforming maps [rad]"
58+
}
59+
},
60+
"required": ["x", "y", "z", "roll", "pitch", "yaw"]
61+
}
62+
},
63+
"properties": {
64+
"/**": {
65+
"type": "object",
66+
"properties": {
67+
"ros__parameters": {
68+
"$ref": "#/definitions/transform_maps"
69+
}
70+
},
71+
"required": ["ros__parameters"]
72+
}
73+
},
74+
"required": ["/**"]
75+
}

‎map/autoware_lanelet2_map_utils/src/fix_lane_change_tags.cpp

+104
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
// Copyright 2020 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
16+
#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
17+
#include <autoware_lanelet2_extension/utility/message_conversion.hpp>
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <lanelet2_core/LaneletMap.h>
21+
#include <lanelet2_core/geometry/Lanelet.h>
22+
#include <lanelet2_core/primitives/LaneletSequence.h>
23+
#include <lanelet2_io/Io.h>
24+
#include <lanelet2_routing/RoutingGraph.h>
25+
26+
#include <iostream>
27+
#include <unordered_set>
28+
#include <vector>
29+
30+
namespace autoware::lanelet2_map_utils
31+
{
32+
bool load_lanelet_map(
33+
const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr,
34+
lanelet::Projector & projector)
35+
{
36+
lanelet::LaneletMapPtr lanelet_map;
37+
lanelet::ErrorMessages errors;
38+
lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors);
39+
40+
for (const auto & error : errors) {
41+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error);
42+
}
43+
if (!errors.empty()) {
44+
return false;
45+
}
46+
std::cout << "Loaded Lanelet2 map" << std::endl;
47+
return true;
48+
}
49+
50+
lanelet::Lanelets convert_to_vector(const lanelet::LaneletMapPtr & lanelet_map_ptr)
51+
{
52+
lanelet::Lanelets lanelets;
53+
std::copy(
54+
lanelet_map_ptr->laneletLayer.begin(), lanelet_map_ptr->laneletLayer.end(),
55+
std::back_inserter(lanelets));
56+
return lanelets;
57+
}
58+
void fix_tags(lanelet::LaneletMapPtr & lanelet_map_ptr)
59+
{
60+
auto lanelets = convert_to_vector(lanelet_map_ptr);
61+
lanelet::traffic_rules::TrafficRulesPtr traffic_rules =
62+
lanelet::traffic_rules::TrafficRulesFactory::create(
63+
lanelet::Locations::Germany, lanelet::Participants::Vehicle);
64+
lanelet::routing::RoutingGraphUPtr routing_graph =
65+
lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *traffic_rules);
66+
67+
for (auto & llt : lanelets) {
68+
if (!routing_graph->conflicting(llt).empty()) {
69+
continue;
70+
}
71+
llt.attributes().erase("turn_direction");
72+
if (!!routing_graph->adjacentRight(llt)) {
73+
llt.rightBound().attributes()["lane_change"] = "yes";
74+
}
75+
if (!!routing_graph->adjacentLeft(llt)) {
76+
llt.leftBound().attributes()["lane_change"] = "yes";
77+
}
78+
}
79+
}
80+
} // namespace autoware::lanelet2_map_utils
81+
82+
int main(int argc, char * argv[])
83+
{
84+
rclcpp::init(argc, argv);
85+
86+
auto node = rclcpp::Node::make_shared("fix_lane_change_tags");
87+
88+
const auto llt_map_path = node->declare_parameter<std::string>("llt_map_path");
89+
const auto output_path = node->declare_parameter<std::string>("output_path");
90+
91+
lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap);
92+
lanelet::projection::MGRSProjector projector;
93+
94+
if (!autoware::lanelet2_map_utils::load_lanelet_map(llt_map_path, llt_map_ptr, projector)) {
95+
return EXIT_FAILURE;
96+
}
97+
98+
autoware::lanelet2_map_utils::fix_tags(llt_map_ptr);
99+
lanelet::write(output_path, *llt_map_ptr, projector);
100+
101+
rclcpp::shutdown();
102+
103+
return 0;
104+
}

0 commit comments

Comments
 (0)