1919
2020Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule (
2121 rclcpp::Node * node,
22- const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict)
22+ const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict, double & x_res,
23+ double & y_res)
2324: logger_(node->get_logger ()),
2425 clock_(node->get_clock ()),
2526 lanelet2_file_metadata_dict_(lanelet2_file_metadata_dict)
2627{
27- // Publish lanelet2 map meta data
28- {
29- const auto msg = getLaneletMapMetaDataMsg ();
28+ pub_whole_map_bin_ =
29+ node->create_publisher <HADMapBin>(" output/lanelet2_map" 1 }.transient_local ());
3030
31- const auto metadata_adaptor = component_interface_utils::NodeAdaptor (node);
32- metadata_adaptor.init_pub (pub_lanelet_map_meta_data_);
33- pub_lanelet_map_meta_data_->publish (msg);
34- }
31+ const auto metadata_adaptor = component_interface_utils::NodeAdaptor (node);
32+ metadata_adaptor.init_pub (pub_lanelet_map_meta_data_);
3533
3634 const auto adaptor = component_interface_utils::NodeAdaptor (node);
3735 adaptor.init_sub (
3836 sub_map_projector_info_,
39- [this ](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
37+ [this , &x_res, &y_res ](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
4038 projector_info_ = *msg;
39+
40+ // load whole map once when projector info is available
41+ {
42+ const auto map = loadWholeMap ();
43+ pub_whole_map_bin_->publish (map);
44+ }
45+
46+ // publish lanelet2 map metadata when projector info is available
47+ {
48+ const auto msg = getLaneletMapMetaDataMsg (x_res, y_res);
49+ pub_lanelet_map_meta_data_->publish (msg);
50+ }
4151 });
4252
4353 get_differential_lanelet2_maps_service_ = node->create_service <GetDifferentialLanelet2Map>(
@@ -60,6 +70,57 @@ bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
6070 return true ;
6171}
6272
73+ autoware_auto_mapping_msgs::msg::HADMapBin Lanelet2DifferentialLoaderModule::loadWholeMap ()
74+ {
75+ std::vector<std::string> lanelet2_paths;
76+ for (const auto & file : lanelet2_file_metadata_dict_) {
77+ if (!std::filesystem::exists (file.first )) {
78+ continue ;
79+ }
80+ lanelet2_paths.push_back (file.first );
81+ }
82+
83+ if (projector_info_.value ().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
84+ std::unique_ptr<lanelet::Projector> projector =
85+ geography_utils::get_lanelet2_projector (projector_info_.value ());
86+
87+ lanelet::ErrorMessages errors{};
88+ lanelet::io_handlers::MultiOsmParser parser (*projector);
89+ lanelet::LaneletMapPtr map = parser.parse (lanelet2_paths, errors);
90+
91+ if (!errors.empty ()) {
92+ for (const auto & error : errors) {
93+ RCLCPP_ERROR_STREAM (rclcpp::get_logger (" map_loader"
94+ }
95+ }
96+
97+ lanelet::utils::overwriteLaneletsCenterline (map, 5.0 , false );
98+
99+ const auto map_bin_msg =
100+ Lanelet2MapLoaderNode::create_map_bin_msg (map, lanelet2_paths[0 ], rclcpp::Clock ().now ());
101+
102+ return map_bin_msg;
103+ } else {
104+ const lanelet::projection::LocalProjector projector;
105+ lanelet::ErrorMessages errors{};
106+ lanelet::io_handlers::MultiOsmParser parser (projector);
107+ lanelet::LaneletMapPtr map = parser.parse (lanelet2_paths, errors);
108+
109+ if (!errors.empty ()) {
110+ for (const auto & error : errors) {
111+ RCLCPP_ERROR_STREAM (rclcpp::get_logger (" map_loader"
112+ }
113+ }
114+
115+ lanelet::utils::overwriteLaneletsCenterline (map, 5.0 , false );
116+
117+ const auto map_bin_msg =
118+ Lanelet2MapLoaderNode::create_map_bin_msg (map, lanelet2_paths[0 ], rclcpp::Clock ().now ());
119+
120+ return map_bin_msg;
121+ }
122+ }
123+
63124bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load (
64125 GetDifferentialLanelet2Map::Request::SharedPtr & request,
65126 GetDifferentialLanelet2Map::Response::SharedPtr & response)
@@ -126,15 +187,30 @@ bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
126187}
127188
128189autoware_map_msgs::msg::LaneletMapMetaData
129- Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg () const
190+ Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg (
191+ const double & x_res, const double & y_res) const
130192{
193+ std::unique_ptr<lanelet::Projector> projector;
194+ if (projector_info_.value ().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
195+ projector = geography_utils::get_lanelet2_projector (projector_info_.value ());
196+ } else {
197+ projector = std::make_unique<lanelet::projection::LocalProjector>();
198+ }
199+
131200 autoware_map_msgs::msg::LaneletMapMetaData metadata;
132201 for (const auto & file : lanelet2_file_metadata_dict_) {
202+ lanelet::GPSPoint gps_point;
203+ gps_point.lat = file.second .origin_lat ;
204+ gps_point.lon = file.second .origin_lon ;
205+ gps_point.ele = 0 ;
206+ lanelet::BasicPoint3d point = projector->forward (gps_point);
207+
133208 autoware_map_msgs::msg::LaneletMapTileMetaData tile_msg;
134209 tile_msg.tile_id = file.second .id ;
135- tile_msg.mgrs_code = file.second .mgrs_code ;
136- tile_msg.origin_lat = file.second .origin_lat ;
137- tile_msg.origin_lon = file.second .origin_lon ;
210+ tile_msg.min_x = point.x ();
211+ tile_msg.min_y = point.y ();
212+ tile_msg.max_x = point.x () + x_res;
213+ tile_msg.max_y = point.y () + y_res;
138214
139215 metadata.metadata_list .push_back (tile_msg);
140216 }
0 commit comments