
#include "RosPlanService.h"
namespace jf{

    RosPlanService::RosPlanService() : Thread("PlanServiceProc"){
        node.setCallbackQueue(&my_queue);
        server = node.advertiseService("routePlan",&RosPlanService::pathPlan,this);
    }


    /******线程管理相关****/
    bool RosPlanService::Start() {
        return Thread::start();
    }

    bool RosPlanService::Stop() {
        return Thread::stop(1000, true);
    }

    U32 RosPlanService::run() {
        while (false == signalled()) {

//            ros::Rate rate(25);
//            while (ros::ok) {
//                HUInfoHandle();
//                my_queue.callAvailable(ros::WallDuration());
//                rate.sleep();
//            }

        }
        stop_event().reset();
    }

    /********业务相关********/
    bool RosPlanService::pathPlan(hu_msgs::routePlan::Request &req,
                                  hu_msgs::routePlan::Response &res){

        hu_msgs::HuPoint huPoint;
        huPoint.lon = 100;
        huPoint.lat = 200;
        huPoint.alt = 300;
        res.planPath.push_back(huPoint);
    }



}