@@ -130,7 +130,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
130
130
}
131
131
132
132
// load lanelet2 map
133
- // We have to keep all loaded maps until publishing the map bin msg
133
+ // Note that we cannot destroy loaded maps even after merging them
134
134
// because the loaded lanelets will be expired when map is destructed
135
135
std::vector<lanelet::LaneletMapPtr> maps;
136
136
for (const auto & path : lanelet2_paths) {
@@ -143,9 +143,9 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
143
143
}
144
144
145
145
// merge all maps
146
- lanelet::LaneletMapPtr map = std::make_shared<lanelet::LaneletMap> ();
147
- for (const auto & map_i : maps) {
148
- utils::merge_lanelet2_maps (*map, *map_i );
146
+ lanelet::LaneletMapPtr map = maps. front ();
147
+ for (size_t i = 1 ; i < maps. size (); ++i ) {
148
+ utils::merge_lanelet2_maps (*map, *maps. at (i) );
149
149
}
150
150
151
151
// setup differential map loader module
@@ -169,12 +169,11 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
169
169
}
170
170
171
171
// set metadata and projection info to differential loader module
172
- differential_loader_module_->setLaneletMapMetadata (
172
+ differential_loader_module_->set_lanelet2_map_medata (
173
173
lanelet2_metadata_dict, x_resolution, y_resolution);
174
- differential_loader_module_->setProjectionInfo (*msg);
174
+ differential_loader_module_->set_projection_info (*msg);
175
175
}
176
176
177
-
178
177
// we use first lanelet2 path to get format_version and map_version
179
178
std::string format_version{" null" }, map_version{" " };
180
179
lanelet::io_handlers::AutowareOsmParser::parseVersions (
@@ -262,8 +261,9 @@ std::map<std::string, Lanelet2FileMetaData> Lanelet2MapLoaderNode::get_lanelet2_
262
261
{
263
262
std::map<std::string, Lanelet2FileMetaData> lanelet2_metadata_dict;
264
263
lanelet2_metadata_dict =
265
- utils::loadLanelet2Metadata (lanelet2_metadata_path, x_resolution, y_resolution);
266
- lanelet2_metadata_dict = utils::replaceWithAbsolutePath (lanelet2_metadata_dict, lanelet2_paths);
264
+ utils::load_lanelet2_metadata (lanelet2_metadata_path, x_resolution, y_resolution);
265
+ lanelet2_metadata_dict =
266
+ utils::replace_with_absolute_path (lanelet2_metadata_dict, lanelet2_paths);
267
267
RCLCPP_INFO_STREAM (get_logger (), " Loaded Lanelet2 metadata: " << lanelet2_metadata_path);
268
268
269
269
return lanelet2_metadata_dict;
0 commit comments