#include <ros/ros.h>

#include <thread>

#include "HdLib.h"
#include "MapInfo/RosMapInfo.h"
#include "Match/RosMatch.hpp"
#include "PlanService/RosPlanService.h"

static const std::string prj_dir = "/home/nvidia/catkin_ws/src/mapbox/localization/map/submodules/jfxmap/";
static const std::string cfg_dir = "./maps/jicaai/mapconfig.json";

int main(int argc, char **argv) {
    ros::init(argc, argv, "map");

    ros::NodeHandle nodeHandle("~");
    static std::string strPrjPath_param;
    static std::string strCfgPath_param;
    static double reportDistance_param;

    nodeHandle.param("/jfxmap/project_path", strPrjPath_param, prj_dir);
    nodeHandle.param("/jfxmap/config_path", strCfgPath_param, cfg_dir);
    jf::HdLib::GetInstance()->Init(strPrjPath_param, strCfgPath_param);  //初始化地图

    jf::RosMapInfo rosMapInfo;          // HU信息发布
    jf::RosPlanService rosPlanService;  // 路径规划信息发布
    jf::RosMatch rosMatch;              // 地图匹配
    rosMapInfo.Start();

    ros::spin();

    while (ros::ok()) {
        usleep(100000);
    }

    return 0;
}
