自动驾驶---打造自动驾驶系统之导航模块开发（三）

   git clone https://github.com/ramunasd/osmctools.git

    
   cd osmctools
    
   autoreconf --install
    
   ./configure
    
   make install

    sudo apt install osmctools

    osmctools

    osmconvert map.osm --out-statistics

 osmconvert map.osm -o=map.pbf

    
  
    
 osmconvert map.pbf -o=map.osm

 std::string osm_data_file = "";

    
 osm_data_file = "./map.osm";
    
 std::vector<std::byte> osm_data;
    
  
    
 if( osm_data.empty() && !osm_data_file.empty() ) {
    
    AINFO << "Reading data from the file: " <<  osm_data_file << std::endl;
    
  
    
    auto data = ReadFile(osm_data_file);
    
    if( !data )
    
    AINFO << "Failed to read." << std::endl;
    
    else
    
    osm_data = std::move(*data);
    
 }
    
 if(osm_data.empty()){
    
    AINFO << "Reading data error!!!" << std::endl;
    
      return -1;
    
 }
    
  
    
 // Build routing Model.
    
 RoutingModel model{osm_data};

 void Model::LoadXmlData(const std::vector<std::byte> &xml) {

    
     using namespace pugi;
    
     
    
     xml_document doc;
    
     if( !doc.load_buffer(xml.data(), xml.size()) )
    
     AERROR << "Failed to parse the xml file!";
    
     
    
     if( auto bounds = doc.select_nodes("/osm/bounds"); !bounds.empty() ) {
    
     auto node = bounds.first().node();
    
     m_MinLat = atof(node.attribute("minlat").as_string());
    
     m_MaxLat = atof(node.attribute("maxlat").as_string());
    
     m_MinLon = atof(node.attribute("minlon").as_string());
    
     m_MaxLon = atof(node.attribute("maxlon").as_string());
    
     } else {
    
     AERROR << "Map's bounds are not defined!";
    
     }
    
  
    
     std::unordered_map<std::string, int> node_id_to_num;
    
     for( const auto &node: doc.select_nodes("/osm/node") ) {
    
     node_id_to_num[node.node().attribute("id").as_string()] = (int)m_Nodes.size();
    
     m_Nodes.emplace_back();        
    
     m_Nodes.back().y = atof(node.node().attribute("lat").as_string());
    
     m_Nodes.back().x = atof(node.node().attribute("lon").as_string());
    
     }
    
 }

 void RoutingModel::CreateNodeToRoadHashmap() {

    
     for (const Model::Road &road : Roads()) {
    
     if (road.type != Model::Road::Type::Footway) {
    
         for (int node_idx : Ways()[road.way].nodes) {
    
             if (node_to_road.find(node_idx) == node_to_road.end()) {
    
                 node_to_road[node_idx] = std::vector<const Model::Road *> ();
    
             }
    
             node_to_road[node_idx].push_back(&road);
    
         }
    
     }
    
     }
    
 }

 void Route::AStarSearch() {

    
     /** * Implementation of the A* Search algorithm.
    
      */
    
     RoutingModel::Node *current_node = nullptr;
    
  
    
     start_node->visited = true;
    
     open_list.push_back(start_node);
    
  
    
     while(open_list.size() > 0) {
    
     current_node = NextNode();
    
  
    
     float distance_current_node_and_end_node = current_node->distance(*end_node);
    
     if(std::fabs(distance_current_node_and_end_node) < 0.001) {
    
         // The final path is stored in the m_Model.path attribute
    
         m_Model.path = ConstructFinalPath(current_node);
    
         return;
    
     } else {
    
         // Otherwise, we have to repeat the loop adding the neighbors of that node until we find
    
         // the end node.
    
         AddNeighbors(current_node);
    
     }
    
     }
    
 }

 // for coordinate transform

    
 const auto pi = 3.14159265358979323846264338327950288;
    
 const auto deg_to_rad = 2. * pi / 360.;
    
 const auto earth_radius = 6378137.;
    
 const auto latToym = [&](double lat) { return log(tan(lat * deg_to_rad / 2 +  pi/4)) / 2 * earth_radius; };
    
 const auto lonToxm = [&](double lon) { return lon * deg_to_rad / 2 * earth_radius; };
    
 const auto ymTolat = [&](double ym) { return (2 * std::atan(std::exp(ym / earth_radius)) - pi / 2) * 180.0 / pi; };
    
 const auto xmTolon = [&](double xm) { return (2 * xm / (earth_radius * deg_to_rad)); };