Skip to content

Commit fca09e8

Browse files
mitsudome-rmitsudome-r
committed
feat: put back lanelet2_map_loader_ndoe.hpp in public include
Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>
1 parent 21428db commit fca09e8

6 files changed

+93
-93
lines changed

‎map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.hpp ‎map/autoware_map_loader/include/autoware/map_loader/lanelet2_differential_loader_module.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef LANELET2_MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_
16-
#define LANELET2_MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_
15+
#ifndef AUTOWARE__MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_
16+
#define AUTOWARE__MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_
1717

1818
#include <autoware/component_interface_specs_universe/map.hpp>
1919
#include <autoware/component_interface_utils/rclcpp.hpp>
@@ -83,4 +83,4 @@ class Lanelet2DifferentialLoaderModule
8383
};
8484

8585
} // namespace autoware::map_loader
86-
#endif // LANELET2_MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_
86+
#endif // AUTOWARE__MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_

‎map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.hpp ‎map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp

+12-5
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,12 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef LANELET2_MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
16-
#define LANELET2_MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
17-
18-
#include "lanelet2_differential_loader_module.hpp"
15+
#ifndef AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
16+
#define AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
1917

2018
#include <autoware/component_interface_specs_universe/map.hpp>
2119
#include <autoware/component_interface_utils/rclcpp.hpp>
20+
#include <autoware/map_loader/lanelet2_differential_loader_module.hpp>
2221
#include <autoware/map_loader/lanelet2_map_loader_utils.hpp>
2322
#include <autoware_lanelet2_extension/version.hpp>
2423
#include <rclcpp/rclcpp.hpp>
@@ -43,6 +42,14 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
4342
public:
4443
explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
4544

45+
static lanelet::LaneletMapPtr load_map(
46+
const std::string & lanelet2_filename,
47+
const autoware_map_msgs::msg::MapProjectorInfo & projector_info);
48+
49+
static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg(
50+
const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
51+
const rclcpp::Time & now);
52+
4653
private:
4754
using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo;
4855

@@ -65,4 +72,4 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
6572
};
6673
} // namespace autoware::map_loader
6774

68-
#endif // LANELET2_MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
75+
#endif // AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_

‎map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_utils.hpp

-8
Original file line numberDiff line numberDiff line change
@@ -45,14 +45,6 @@ std::map<std::string, Lanelet2FileMetaData> replace_with_absolute_path(
4545

4646
void merge_lanelet2_maps(lanelet::LaneletMap & merge_target, lanelet::LaneletMap & merge_source);
4747

48-
lanelet::LaneletMapPtr load_map(
49-
const std::string & lanelet2_filename,
50-
const autoware_map_msgs::msg::MapProjectorInfo & projector_info);
51-
52-
autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg(
53-
const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
54-
const rclcpp::Time & now);
55-
5648
} // namespace utils
5749

5850
} // namespace autoware::map_loader

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

+5-5
Original file line numberDiff line numberDiff line change
@@ -12,11 +12,10 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "lanelet2_differential_loader_module.hpp"
16-
1715
#include "lanelet2_local_projector.hpp"
18-
#include "lanelet2_map_loader_node.hpp"
1916

17+
#include <autoware/map_loader/lanelet2_differential_loader_module.hpp>
18+
#include <autoware/map_loader/lanelet2_map_loader_node.hpp>
2019
#include <autoware/map_loader/lanelet2_map_loader_utils.hpp>
2120

2221
#include <map>
@@ -77,7 +76,7 @@ bool Lanelet2DifferentialLoaderModule::on_service_get_differential_lanelet2_map(
7776
// because the loaded lanelets will be expired when map is destructed
7877
std::vector<lanelet::LaneletMapPtr> maps;
7978
for (const auto & path : lanelet2_paths) {
80-
auto map_tmp = utils::load_map(path, *projector_info_);
79+
auto map_tmp = Lanelet2MapLoaderNode::load_map(path, *projector_info_);
8180
if (!map_tmp) {
8281
RCLCPP_ERROR(logger_, "Failed to load lanelet2_map, %s", path.c_str());
8382
return false;
@@ -99,7 +98,8 @@ bool Lanelet2DifferentialLoaderModule::on_service_get_differential_lanelet2_map(
9998
}
10099

101100
// create the map bin message
102-
const auto map_bin_msg = utils::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
101+
const auto map_bin_msg =
102+
Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
103103

104104
res->lanelet2_cells = map_bin_msg;
105105
res->header.frame_id = "map";

‎map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

+73-4
Original file line numberDiff line numberDiff line change
@@ -31,12 +31,11 @@
3131
*
3232
*/
3333

34-
#include "lanelet2_map_loader_node.hpp"
35-
3634
#include "lanelet2_local_projector.hpp"
3735

3836
#include <ament_index_cpp/get_package_prefix.hpp>
3937
#include <autoware/geography_utils/lanelet2_projector.hpp>
38+
#include <autoware/map_loader/lanelet2_map_loader_node.hpp>
4039
#include <autoware/map_loader/lanelet2_map_loader_utils.hpp>
4140
#include <autoware_lanelet2_extension/io/autoware_osm_parser.hpp>
4241
#include <autoware_lanelet2_extension/projection/mgrs_projector.hpp>
@@ -50,7 +49,9 @@
5049
#include <lanelet2_io/Io.h>
5150
#include <lanelet2_projection/UTM.h>
5251

52+
#include <algorithm>
5353
#include <filesystem>
54+
#include <limits>
5455
#include <map>
5556
#include <memory>
5657
#include <stdexcept>
@@ -134,7 +135,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
134135
// because the loaded lanelets will be expired when map is destructed
135136
std::vector<lanelet::LaneletMapPtr> maps;
136137
for (const auto & path : lanelet2_paths) {
137-
auto map_tmp = utils::load_map(path, *msg);
138+
auto map_tmp = load_map(path, *msg);
138139
if (!map_tmp) {
139140
RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map, %s", path.c_str());
140141
return;
@@ -212,7 +213,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
212213
}
213214

214215
// create map bin msg
215-
const auto map_bin_msg = utils::create_map_bin_msg(map, lanelet2_paths[0], now());
216+
const auto map_bin_msg = create_map_bin_msg(map, lanelet2_paths[0], now());
216217

217218
// create publisher and publish
218219
pub_map_bin_ =
@@ -221,6 +222,74 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
221222
RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published.");
222223
}
223224

225+
lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
226+
const std::string & lanelet2_filename,
227+
const autoware_map_msgs::msg::MapProjectorInfo & projector_info)
228+
{
229+
lanelet::ErrorMessages errors{};
230+
if (projector_info.projector_type != autoware_map_msgs::msg::MapProjectorInfo::LOCAL) {
231+
std::unique_ptr<lanelet::Projector> projector =
232+
autoware::geography_utils::get_lanelet2_projector(projector_info);
233+
lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors);
234+
if (errors.empty()) {
235+
return map;
236+
}
237+
} else {
238+
const autoware::map_loader::LocalProjector projector;
239+
lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
240+
241+
if (!errors.empty()) {
242+
for (const auto & error : errors) {
243+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
244+
}
245+
}
246+
247+
// overwrite local_x, local_y
248+
for (lanelet::Point3d point : map->pointLayer) {
249+
if (point.hasAttribute("local_x")) {
250+
point.x() = point.attribute("local_x").asDouble().value();
251+
}
252+
if (point.hasAttribute("local_y")) {
253+
point.y() = point.attribute("local_y").asDouble().value();
254+
}
255+
}
256+
257+
// realign lanelet borders using updated points
258+
for (lanelet::Lanelet lanelet : map->laneletLayer) {
259+
auto left = lanelet.leftBound();
260+
auto right = lanelet.rightBound();
261+
std::tie(left, right) = lanelet::geometry::align(left, right);
262+
lanelet.setLeftBound(left);
263+
lanelet.setRightBound(right);
264+
}
265+
266+
return map;
267+
}
268+
269+
for (const auto & error : errors) {
270+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
271+
}
272+
return nullptr;
273+
}
274+
275+
autoware_map_msgs::msg::LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
276+
const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now)
277+
{
278+
std::string format_version{};
279+
std::string map_version{};
280+
lanelet::io_handlers::AutowareOsmParser::parseVersions(
281+
lanelet2_filename, &format_version, &map_version);
282+
283+
autoware_map_msgs::msg::LaneletMapBin map_bin_msg;
284+
map_bin_msg.header.stamp = now;
285+
map_bin_msg.header.frame_id = "map";
286+
map_bin_msg.version_map_format = format_version;
287+
map_bin_msg.version_map = map_version;
288+
lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);
289+
290+
return map_bin_msg;
291+
}
292+
224293
/**
225294
* @brief Get list of lanelet2 map file paths from input paths/directories
226295
* @param lanelet2_paths_or_directory Vector of paths that can be either lanelet2 map files or

‎map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_utils.cpp

-68
Original file line numberDiff line numberDiff line change
@@ -86,56 +86,6 @@ std::map<std::string, Lanelet2FileMetaData> replace_with_absolute_path(
8686
return absolute_path_map;
8787
}
8888

89-
lanelet::LaneletMapPtr load_map(
90-
const std::string & lanelet2_filename,
91-
const autoware_map_msgs::msg::MapProjectorInfo & projector_info)
92-
{
93-
lanelet::ErrorMessages errors{};
94-
if (projector_info.projector_type != autoware_map_msgs::msg::MapProjectorInfo::LOCAL) {
95-
std::unique_ptr<lanelet::Projector> projector =
96-
autoware::geography_utils::get_lanelet2_projector(projector_info);
97-
lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors);
98-
if (errors.empty()) {
99-
return map;
100-
}
101-
} else {
102-
const autoware::map_loader::LocalProjector projector;
103-
lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
104-
105-
if (!errors.empty()) {
106-
for (const auto & error : errors) {
107-
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
108-
}
109-
}
110-
111-
// overwrite local_x, local_y
112-
for (lanelet::Point3d point : map->pointLayer) {
113-
if (point.hasAttribute("local_x")) {
114-
point.x() = point.attribute("local_x").asDouble().value();
115-
}
116-
if (point.hasAttribute("local_y")) {
117-
point.y() = point.attribute("local_y").asDouble().value();
118-
}
119-
}
120-
121-
// realign lanelet borders using updated points
122-
for (lanelet::Lanelet lanelet : map->laneletLayer) {
123-
auto left = lanelet.leftBound();
124-
auto right = lanelet.rightBound();
125-
std::tie(left, right) = lanelet::geometry::align(left, right);
126-
lanelet.setLeftBound(left);
127-
lanelet.setRightBound(right);
128-
}
129-
130-
return map;
131-
}
132-
133-
for (const auto & error : errors) {
134-
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
135-
}
136-
return nullptr;
137-
}
138-
13989
void merge_lanelet2_maps(lanelet::LaneletMap & merge_target, lanelet::LaneletMap & merge_source)
14090
{
14191
for (lanelet::Lanelet & lanelet : merge_source.laneletLayer) {
@@ -170,22 +120,4 @@ void merge_lanelet2_maps(lanelet::LaneletMap & merge_target, lanelet::LaneletMap
170120
}
171121
}
172122

173-
autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg(
174-
const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now)
175-
{
176-
std::string format_version{};
177-
std::string map_version{};
178-
lanelet::io_handlers::AutowareOsmParser::parseVersions(
179-
lanelet2_filename, &format_version, &map_version);
180-
181-
autoware_map_msgs::msg::LaneletMapBin map_bin_msg;
182-
map_bin_msg.header.stamp = now;
183-
map_bin_msg.header.frame_id = "map";
184-
map_bin_msg.version_map_format = format_version;
185-
map_bin_msg.version_map = map_version;
186-
lanelet::utils::conversion::toBinMsg(map, &map_bin_msg);
187-
188-
return map_bin_msg;
189-
}
190-
191123
} // namespace autoware::map_loader::utils

0 commit comments

Comments
 (0)