Branch data Line data Source code
1 : : // Copyright 2020 The Autoware Foundation 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 "lanelet2_map_provider/lanelet2_map_provider.hpp" 16 : : 17 : : #include <string> 18 : : 19 : : #include "common/types.hpp" 20 : : #include "had_map_utils/had_map_utils.hpp" 21 : : 22 : : #include "GeographicLib/Geocentric.hpp" 23 : : #include "lanelet2_core/primitives/GPSPoint.h" 24 : : #include "lanelet2_io/Io.h" 25 : : #include "lanelet2_projection/UTM.h" 26 : : #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 27 : : 28 : : using autoware::common::types::float64_t; 29 : : 30 : : namespace autoware 31 : : { 32 : : 33 : : namespace lanelet2_map_provider 34 : : { 35 : : 36 [ + - ]: 1 : Lanelet2MapProvider::Lanelet2MapProvider( 37 : : const std::string & map_filename, 38 : : const geometry_msgs::msg::TransformStamped & stf, const float64_t offset_lat, 39 : : const float64_t offset_lon) 40 : : { 41 : : GeographicLib::Geocentric earth( 42 : : GeographicLib::Constants::WGS84_a(), 43 [ + - ]: 1 : GeographicLib::Constants::WGS84_f()); 44 : : 45 : 1 : float64_t origin_lat = 0.0; 46 : 1 : float64_t origin_lon = 0.0; 47 : 1 : float64_t origin_alt = 0.0; 48 : : earth.Reverse( 49 : 1 : stf.transform.translation.x, 50 : 1 : stf.transform.translation.y, 51 [ + - ]: 1 : stf.transform.translation.z, 52 : : origin_lat, origin_lon, origin_alt); 53 [ + - ]: 1 : this->load_map(map_filename, {origin_lat + offset_lat, origin_lon + offset_lon, origin_alt}); 54 : 1 : } 55 : : 56 [ + - ]: 1 : Lanelet2MapProvider::Lanelet2MapProvider( 57 : : const std::string & map_filename, 58 : : const LatLonAlt map_frame_origin, 59 : : const float64_t offset_lat, const float64_t offset_lon) 60 : : { 61 : 1 : LatLonAlt adjusted_origin{map_frame_origin.lat + offset_lat, map_frame_origin.lon + offset_lon, 62 : 1 : map_frame_origin.alt}; 63 [ + - ]: 1 : this->load_map(map_filename, adjusted_origin); 64 : 1 : } 65 : : 66 [ + - ]: 2 : void Lanelet2MapProvider::load_map( 67 : : const std::string & map_filename, const LatLonAlt map_frame_origin) 68 : : { 69 : 2 : lanelet::ErrorMessages errors; 70 [ + - ]: 2 : lanelet::GPSPoint originGps{map_frame_origin.lat, map_frame_origin.lon, map_frame_origin.alt}; 71 : : lanelet::Origin origin{originGps}; 72 : : 73 [ + - ]: 2 : lanelet::projection::UtmProjector projector(origin); 74 [ + - ]: 4 : m_map = lanelet::load(map_filename, projector, &errors); 75 [ + - ]: 4 : autoware::common::had_map_utils::overwriteLaneletsCenterline(m_map, true); 76 : 2 : } 77 : : 78 : : } // namespace lanelet2_map_provider 79 : : 80 : : } // namespace autoware