AGV/MPC/navigation.h

//
// Created by zx on 22-12-2.
//

#ifndef LIO_LIVOX_CPP_MPC_NAVIGATION_H_
#define LIO_LIVOX_CPP_MPC_NAVIGATION_H_
#include "../define/timedlockdata.hpp"
#include "pose2d.h"
#include "trajectory.h"
#include "monitor/monitor_emqx.h"
#include "monitor/terminator_emqx.h"
#include "parameter.pb.h"
#include <queue>
#include <thread>
class Navigation
{
public:
    enum ActionType
    {
        eRotate=0,
        eLine,
        eMPC
    };
 public:
    Navigation();
    ~Navigation();
    //连接AGV emqx服务

    bool Init(const Navigation_parameter& parameter);

    void ResetPose(const Pose2d& pose);
    void ResetStatu(double v,double a);
    TimedLockData<Pose2d>& RealTimePose(){
      return timedPose_;
    }
    TimedLockData<double>& RealTimeV(){
      return timedV_;
    }
    TimedLockData<double>& RealTimeA(){
      return timedA_;
    }
    bool Start(std::queue<Trajectory> global_trajectorys);
    void Cancel();
    void Pause();

 protected:

    static void RobotStatuCallback(double x,double y,double theta,double v,double vth,void* context);
    static void NavCmdCallback(const NavCmd& cmd,void* context);

    static bool IsArrived(const Pose2d& cur,double speed,const Pose2d& target,const Pose2d& diff);
    bool execute_edge(const Trajectory& child,const Pose2d& head_diff,const Pose2d& target_diff);
    bool moveToPoint(const Pose2d& pose,const Pose2d& distance);
    bool mpc_once(const Trajectory& traj,std::vector<double>& out);
    void navigatting();

    /*
     * 发布导航模块状态，当前动作类型、角速度、线速度
     */
    void publish_statu(ActionType type,float velocity,float angular);

 protected:
    std::mutex  mtx_;
    bool inited_=false;
    Monitor_emqx* monitor_= nullptr;
    Terminator_emqx* terminator_= nullptr;

    TimedLockData<Pose2d> timedPose_;           //当前位姿
    TimedLockData<double> timedV_;              //底盘数据
    TimedLockData<double> timedA_;
    std::queue<Trajectory> global_trajectorys_; //多段轨迹
    std::thread* thread_= nullptr;
    bool running_=false;
    bool pause_=false;
    bool cancel_=false;

    TimedLockData<Trajectory> selected_traj_;
    TimedLockData<Trajectory> predict_traj_;
};


#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_