Skip to content

Commit 4c5b9b3

Browse files
StepTurtleStepTurtle
committed
Add differential lanelet loading
Signed-off-by: Barış Zeren <bzeren1819@gmail.com>
1 parent a3e4121 commit 4c5b9b3

7 files changed

+446
-0
lines changed

‎map/map_loader/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,8 @@ rclcpp_components_register_node(pointcloud_map_loader_node
2929

3030
ament_auto_add_library(lanelet2_map_loader_node SHARED
3131
src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
32+
src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp
33+
src/lanelet2_map_loader/utils.cpp
3234
)
3335

3436
rclcpp_components_register_node(lanelet2_map_loader_node

‎map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp

+77
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
// Copyright 2022 The Autoware Contributors
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+
#ifndef MAP_LOADER_LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP
16+
#define MAP_LOADER_LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP
17+
18+
#include "lanelet2_extension/io/autoware_multi_osm_parser.hpp"
19+
#include "map_loader/utils.hpp"
20+
21+
#include <lanelet2_extension/io/autoware_osm_parser.hpp>
22+
#include <lanelet2_extension/projection/mgrs_projector.hpp>
23+
#include <lanelet2_extension/utility/message_conversion.hpp>
24+
#include <lanelet2_extension/utility/utilities.hpp>
25+
#include <pugixml.hpp>
26+
#include <rclcpp/rclcpp.hpp>
27+
28+
#include "autoware_map_msgs/srv/get_differential_lanelet2_map.hpp"
29+
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
30+
#include <autoware_map_msgs/msg/lanelet_map_meta_data.hpp>
31+
32+
#include <lanelet2_core/LaneletMap.h>
33+
#include <lanelet2_core/geometry/LineString.h>
34+
#include <lanelet2_io/Io.h>
35+
#include <lanelet2_projection/UTM.h>
36+
37+
#include <filesystem>
38+
39+
class Lanelet2DifferentialLoaderModule
40+
{
41+
using GetDifferentialLanelet2Map = autoware_map_msgs::srv::GetDifferentialLanelet2Map;
42+
using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
43+
44+
public:
45+
explicit Lanelet2DifferentialLoaderModule(
46+
rclcpp::Node * node,
47+
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict);
48+
49+
private:
50+
rclcpp::Logger logger_;
51+
rclcpp::Clock::SharedPtr clock_;
52+
53+
std::map<std::string, Lanelet2FileMetaData> lanelet2_file_metadata_dict_;
54+
55+
rclcpp::Service<GetDifferentialLanelet2Map>::SharedPtr get_differential_lanelet2_maps_service_;
56+
57+
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
58+
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_debug_;
59+
60+
rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapMetaData>::SharedPtr
61+
pub_lanelet_map_meta_data_;
62+
63+
bool onServiceGetDifferentialLanelet2Map(
64+
GetDifferentialLanelet2Map::Request::SharedPtr req,
65+
GetDifferentialLanelet2Map::Response::SharedPtr res);
66+
67+
bool differentialLanelet2Load(
68+
GetDifferentialLanelet2Map::Request::SharedPtr & request,
69+
GetDifferentialLanelet2Map::Response::SharedPtr & response);
70+
71+
autoware_map_msgs::msg::LaneletMapMetaData getLaneletMapMetaDataMsg() const;
72+
73+
private:
74+
// Lanelet Parser Functions
75+
};
76+
77+
#endif // MAP_LOADER_LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP

‎map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

+11
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,9 @@
1515
#ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
1616
#define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
1717

18+
#include "map_loader/lanelet2_differential_loader_module.hpp"
19+
#include "map_loader/utils.hpp"
20+
1821
#include <component_interface_specs/map.hpp>
1922
#include <component_interface_utils/rclcpp.hpp>
2023
#include <rclcpp/rclcpp.hpp>
@@ -47,8 +50,16 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
4750

4851
void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg);
4952

53+
std::unique_ptr<Lanelet2DifferentialLoaderModule> differential_loader_module_;
54+
5055
component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
5156
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
57+
58+
std::vector<std::string> getLanelet2Paths(
59+
const std::vector<std::string> & lanelet2_paths_or_directory) const;
60+
std::map<std::string, Lanelet2FileMetaData> getLanelet2Metadata(
61+
const std::string & lanelet2_metadata_path,
62+
const std::vector<std::string> & lanelet2_paths) const;
5263
};
5364

5465
#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_

‎map/map_loader/include/map_loader/utils.hpp

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
// Copyright 2022 The Autoware Contributors
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+
#ifndef MAP_LOADER_UTILS_HPP
16+
#define MAP_LOADER_UTILS_HPP
17+
18+
#include <yaml-cpp/yaml.h>
19+
20+
#include <map>
21+
#include <string>
22+
#include <vector>
23+
24+
struct Lanelet2FileMetaData
25+
{
26+
int id;
27+
double origin_lat;
28+
double origin_lon;
29+
std::string mgrs_code;
30+
};
31+
32+
std::map<std::string, Lanelet2FileMetaData> loadLanelet2Metadata(
33+
const std::string & lanelet2_metadata_path);
34+
std::map<std::string, Lanelet2FileMetaData> replaceWithAbsolutePath(
35+
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_metadata_path,
36+
const std::vector<std::string> & lanelet2_paths);
37+
38+
#endif // MAP_LOADER_UTILS_HPP

‎map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

+188
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,188 @@
1+
// Copyright 2022 The Autoware Contributors
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 "map_loader/lanelet2_differential_loader_module.hpp"
16+
17+
#include "map_loader/lanelet2_map_loader_node.hpp"
18+
19+
Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule(
20+
rclcpp::Node * node,
21+
const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict)
22+
: logger_(node->get_logger()),
23+
clock_(node->get_clock()),
24+
lanelet2_file_metadata_dict_(lanelet2_file_metadata_dict)
25+
{
26+
// Publish lanelet2 map meta data
27+
{
28+
pub_lanelet_map_meta_data_ = node->create_publisher<autoware_map_msgs::msg::LaneletMapMetaData>(
29+
"/map/lanelet2_map_meta_data", rclcpp::QoS{1}.transient_local());
30+
31+
const auto msg = getLaneletMapMetaDataMsg();
32+
33+
pub_lanelet_map_meta_data_->publish(msg);
34+
}
35+
36+
get_differential_lanelet2_maps_service_ = node->create_service<GetDifferentialLanelet2Map>(
37+
"/map/get_differential_lanelet2_map",
38+
std::bind(
39+
&Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map, this,
40+
std::placeholders::_1, std::placeholders::_2));
41+
42+
pub_map_bin_ = node->create_publisher<HADMapBin>(
43+
"/map/differential_lanelet2_map_bin", rclcpp::QoS{1}.transient_local());
44+
pub_map_bin_debug_ =
45+
node->create_publisher<HADMapBin>("/map/vector_map", rclcpp::QoS{1}.transient_local());
46+
}
47+
48+
bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
49+
GetDifferentialLanelet2Map::Request::SharedPtr req,
50+
GetDifferentialLanelet2Map::Response::SharedPtr res)
51+
{
52+
if (!differentialLanelet2Load(req, res)) {
53+
RCLCPP_ERROR(logger_, "Failed to load differential lanelet2 map");
54+
return false;
55+
}
56+
res->header.stamp = clock_->now();
57+
res->header.frame_id = "map";
58+
59+
return true;
60+
}
61+
62+
bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
63+
GetDifferentialLanelet2Map::Request::SharedPtr & request,
64+
GetDifferentialLanelet2Map::Response::SharedPtr & response)
65+
{
66+
{
67+
std::vector<std::string> lanelet2_paths;
68+
for (const auto & path : lanelet2_file_metadata_dict_) {
69+
if (!std::filesystem::exists(path.first)) {
70+
continue;
71+
}
72+
lanelet2_paths.push_back(path.first);
73+
}
74+
if (lanelet2_paths.empty()) {
75+
return false;
76+
}
77+
78+
lanelet::ErrorMessages errors{};
79+
lanelet::projection::MGRSProjector projector{};
80+
lanelet::io_handlers::MultiOsmParser parser(projector, {});
81+
lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
82+
83+
for (lanelet::Point3d point : map->pointLayer) {
84+
if (point.hasAttribute("local_x")) {
85+
point.x() = point.attribute("local_x").asDouble().value();
86+
}
87+
if (point.hasAttribute("local_y")) {
88+
point.y() = point.attribute("local_y").asDouble().value();
89+
}
90+
}
91+
// realign lanelet borders using updated points
92+
for (lanelet::Lanelet lanelet : map->laneletLayer) {
93+
auto left = lanelet.leftBound();
94+
auto right = lanelet.rightBound();
95+
std::tie(left, right) = lanelet::geometry::align(left, right);
96+
lanelet.setLeftBound(left);
97+
lanelet.setRightBound(right);
98+
}
99+
// overwrite centerline
100+
lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
101+
// convert map to binary message
102+
{
103+
const auto map_bin_msg =
104+
Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
105+
106+
pub_map_bin_debug_->publish(map_bin_msg);
107+
} // convert map to binary message end
108+
}
109+
110+
{
111+
std::vector<std::string> lanelet2_paths;
112+
for (const auto & id : request->osm_file_ids) {
113+
auto it = std::find_if(
114+
lanelet2_file_metadata_dict_.begin(), lanelet2_file_metadata_dict_.end(),
115+
[&id](const auto & file) { return file.second.id == id; });
116+
if (it == lanelet2_file_metadata_dict_.end()) {
117+
continue;
118+
}
119+
if (!std::filesystem::exists(it->first)) {
120+
continue;
121+
}
122+
lanelet2_paths.push_back(it->first);
123+
}
124+
if (lanelet2_paths.empty()) {
125+
return false;
126+
}
127+
128+
lanelet::ErrorMessages errors{};
129+
lanelet::projection::MGRSProjector projector{};
130+
lanelet::io_handlers::MultiOsmParser parser(projector);
131+
lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
132+
133+
for (lanelet::Point3d point : map->pointLayer) {
134+
if (point.hasAttribute("local_x")) {
135+
point.x() = point.attribute("local_x").asDouble().value();
136+
}
137+
if (point.hasAttribute("local_y")) {
138+
point.y() = point.attribute("local_y").asDouble().value();
139+
}
140+
}
141+
// realign lanelet borders using updated points
142+
for (lanelet::Lanelet lanelet : map->laneletLayer) {
143+
auto left = lanelet.leftBound();
144+
auto right = lanelet.rightBound();
145+
std::tie(left, right) = lanelet::geometry::align(left, right);
146+
lanelet.setLeftBound(left);
147+
lanelet.setRightBound(right);
148+
}
149+
// overwrite centerline
150+
lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
151+
// convert map to binary message
152+
{
153+
const auto map_bin_msg =
154+
Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
155+
156+
pub_map_bin_->publish(map_bin_msg);
157+
response->differential_map = map_bin_msg;
158+
}
159+
}
160+
161+
return true;
162+
}
163+
164+
autoware_map_msgs::msg::LaneletMapMetaData
165+
Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg() const
166+
{
167+
autoware_map_msgs::msg::LaneletMapMetaData metadata;
168+
for (const auto & file : lanelet2_file_metadata_dict_) {
169+
std::cout << "----------------" << std::endl;
170+
std::cout << file.first << std::endl;
171+
std::cout << file.second.id << std::endl;
172+
173+
autoware_map_msgs::msg::LaneletMapTileMetaData tile_msg;
174+
tile_msg.mgrs_code = file.second.mgrs_code;
175+
tile_msg.origin_lat = file.second.origin_lat;
176+
tile_msg.origin_lon = file.second.origin_lon;
177+
178+
autoware_map_msgs::msg::LaneletMapTileMetaDataWithID tile_msg_with_id;
179+
tile_msg_with_id.metadata = tile_msg;
180+
tile_msg_with_id.tile_id = file.second.id;
181+
182+
metadata.metadata_list.push_back(tile_msg_with_id);
183+
}
184+
metadata.header.frame_id = "map";
185+
metadata.header.stamp = clock_->now();
186+
187+
return metadata;
188+
}

0 commit comments

Comments
 (0)