Skip to content

Commit 4247d1a

Browse files
StepTurtleStepTurtle
committed
Update
Signed-off-by: Barış Zeren <bzeren1819@gmail.com>
1 parent c64ea99 commit 4247d1a

File tree

2 files changed

+42
-101
lines changed

2 files changed

+42
-101
lines changed

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

-6
Original file line numberDiff line numberDiff line change
@@ -54,9 +54,6 @@ class Lanelet2DifferentialLoaderModule
5454

5555
rclcpp::Service<GetDifferentialLanelet2Map>::SharedPtr get_differential_lanelet2_maps_service_;
5656

57-
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
58-
rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_debug_;
59-
6057
rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapMetaData>::SharedPtr
6158
pub_lanelet_map_meta_data_;
6259

@@ -69,9 +66,6 @@ class Lanelet2DifferentialLoaderModule
6966
GetDifferentialLanelet2Map::Response::SharedPtr & response);
7067

7168
autoware_map_msgs::msg::LaneletMapMetaData getLaneletMapMetaDataMsg() const;
72-
73-
private:
74-
// Lanelet Parser Functions
7569
};
7670

7771
#endif // MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_

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

+42-95
Original file line numberDiff line numberDiff line change
@@ -38,11 +38,6 @@ Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule(
3838
std::bind(
3939
&Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map, this,
4040
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());
4641
}
4742

4843
bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
@@ -53,7 +48,6 @@ bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
5348
RCLCPP_ERROR(logger_, "Failed to load differential lanelet2 map");
5449
return false;
5550
}
56-
res->header.stamp = clock_->now();
5751
res->header.frame_id = "map";
5852

5953
return true;
@@ -63,101 +57,58 @@ bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
6357
GetDifferentialLanelet2Map::Request::SharedPtr & request,
6458
GetDifferentialLanelet2Map::Response::SharedPtr & response)
6559
{
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);
60+
// check osm ids
61+
std::vector<std::string> lanelet2_paths;
62+
for (const auto & id : request->osm_file_ids) {
63+
auto it = std::find_if(
64+
lanelet2_file_metadata_dict_.begin(), lanelet2_file_metadata_dict_.end(),
65+
[&id](const auto & file) { return file.second.id == id; });
66+
if (it == lanelet2_file_metadata_dict_.end()) {
67+
continue;
7368
}
74-
if (lanelet2_paths.empty()) {
75-
return false;
69+
if (!std::filesystem::exists(it->first)) {
70+
continue;
7671
}
72+
lanelet2_paths.push_back(it->first);
73+
}
74+
if (lanelet2_paths.empty()) {
75+
return false;
76+
}
77+
78+
// create parser and load map
79+
lanelet::ErrorMessages errors{};
80+
lanelet::projection::MGRSProjector projector{};
81+
lanelet::io_handlers::MultiOsmParser parser(projector);
82+
lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
7783

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-
}
84+
for (lanelet::Point3d point : map->pointLayer) {
85+
if (point.hasAttribute("local_x")) {
86+
point.x() = point.attribute("local_x").asDouble().value();
9087
}
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);
88+
if (point.hasAttribute("local_y")) {
89+
point.y() = point.attribute("local_y").asDouble().value();
9890
}
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
10891
}
10992

93+
// realign lanelet borders using updated points
94+
for (lanelet::Lanelet lanelet : map->laneletLayer) {
95+
auto left = lanelet.leftBound();
96+
auto right = lanelet.rightBound();
97+
std::tie(left, right) = lanelet::geometry::align(left, right);
98+
lanelet.setLeftBound(left);
99+
lanelet.setRightBound(right);
100+
}
101+
102+
// overwrite centerline
103+
lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
104+
// convert map to binary message
110105
{
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-
}
106+
const auto map_bin_msg =
107+
Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
127108

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-
}
109+
pub_map_bin_->publish(map_bin_msg);
110+
response->differential_map = map_bin_msg;
159111
}
160-
161112
return true;
162113
}
163114

@@ -166,10 +117,6 @@ Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg() const
166117
{
167118
autoware_map_msgs::msg::LaneletMapMetaData metadata;
168119
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-
173120
autoware_map_msgs::msg::LaneletMapTileMetaData tile_msg;
174121
tile_msg.mgrs_code = file.second.mgrs_code;
175122
tile_msg.origin_lat = file.second.origin_lat;

0 commit comments

Comments
 (0)