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 <glog/logging.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>
#include <fstream>
#include <time.h>

class Navigation {
public:
    typedef struct {
        double min;
        double max;
    } stLimit;
    //舵轮位置
    enum RWheelPosition {
        eUnknow = 0,
        eX,   //超前
        eY,     //横移
        eR      //旋转位
    };
    enum ClampStatu {
        eInvalid = 0,
        eClosed = 1,
        eHalfOpened = 2,    //半开位（默认）
        eFullyOpened = 3 //全开位
    };

    enum CarrierStatu{
        eCarrierInvalid=0,
        eCarrierUp=1,
        eCarrierDown=2
    };

    typedef std::map<int,CarrierStatu> CarrierStatuRegionMap;

    enum DoorStatu{
        eDoorInvalid=0,
        eDoorOpened=1,
        eDoorClosed=2
    };

    typedef std::pair<DoorStatu,DoorStatu> DoorStatusInOnePort;//<out, ins>
    typedef std::map<int,DoorStatusInOnePort> DoorStatusMap;
    enum MpcResult {
        eMpcSuccess = 0,
        eMpcFailed,
        eImpossibleToTarget
    };

public:
    Navigation();

    virtual ~Navigation();
    //连接AGV emqx服务

    bool Init(const NavParameter::Navigation_parameter &parameter);

    bool LoadSpaceNo2CarrierNo();

    bool SpaceNo2CarrierNo(int space_no, int& region_id, int& carrier_no);

    CarrierStatu GetCarrierStatusBySpaceID(int space_id);

    bool Stringsplit(const std::string &str, const std::string splits, std::vector<std::string> &res);

    static void anasisDoorStatu(int32_t i_door_status, DoorStatusMap& m_door_status);

    virtual bool IsMasterAGV();

    virtual void ResetPose(const Pose2d &pose);

    void ResetStatu(double v, double a);

    virtual void ResetClamp(ClampStatu statu);

    virtual void ResetLifter(LifterStatus status);

    virtual void ResetDoor(int32_t status);

    virtual void ResetCarrier(std::vector<int32_t> status);

    TimedLockData<Pose2d> &RealTimePose() {
        return timedPose_;
    }

    TimedLockData<double> &RealTimeV() {
        return timedV_;
    }

    TimedLockData<double> &RealTimeA() {
        return timedA_;
    }

    virtual void Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);

    /* 手动旋转 */
    virtual void ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::NavResponse &response);

    void Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response);

    bool Stop();

    bool SlowlyStop();

    virtual bool SwitchMode(int mode, float wheelBase);

    bool PoseTimeout();
    bool WaitClampLifterStatu(int clampLifterAction);
    bool WaitInsideDoorCompleted(int doorID, DoorStatu status);
    bool WaitCarrierCompleted(int spaceID,CarrierStatu statu);

    bool TargetIsEntrance(NavMessage::PathNode node);
    bool TargetIsExport(NavMessage::PathNode node);

    int GetPortIDBySpace(NavMessage::PathNode node);

    //获取当前流程是出/入库
    int GetSequenceType();

    // 获取当前未完成动作数量
    virtual int GetRemainActionsCount();
    virtual NavMessage::NewAction GetCurrentAction();
    virtual NavMessage::NewAction GetLastAction();

protected:
    static void anasisCarierStatu(int& regionID, CarrierStatuRegionMap& status, int32_t bytes);
    virtual void HandleAgvStatu(const MqttMsg &msg);

    virtual void SendMoveCmd(int mode, ActionType type, double v, double angular);

    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[]);

    /*virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[],
                             int space_id, double distance,double Y1=0.0,double Y2=0.0);*/

    virtual void SendMoveCmd(int clampLifterAction,int mode, ActionType type, double v[], double angular[],
                             int space_id, double distance,double Y1=0.0,double Y2=0.0);
//    virtual void SendMoveCmd(int mode, ActionType type, double v[], double angular[], double diff_yaw[], int space_id, double distance);

    static void RobotPoseCallback(const MqttMsg &msg, void *context);

    static void RobotSpeedCallback(const MqttMsg &msg, void *context);

    static void BrotherAgvStatuCallback(const MqttMsg &msg, void *context);


    bool IsArrived(NavMessage::PathNode targetNode);

    bool PossibleToTarget(NavMessage::PathNode targetNode, bool directY);

    /*
     * 移动到目标点，不能到达则旋转
     */
    bool MoveToTarget(NavMessage::PathNode node, stLimit limit_v, stLimit limit_w,
                      bool autoDirect, bool enable_rotate,int clampLifterAction=0);

    /*
     * autoDirect:自动选择 纵向还是横向巡线
     */
    MpcResult MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect,int clampLifterAction=0);

    /*
     * 旋转摆正，朝向目标点,
     * anyDirect: 任意一个朝向对准目标点
     */
    //virtual bool RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect);

    virtual MpcResult RotateReferToTarget(NavMessage::PathNode target, stLimit limit_rotate, int directions);

    virtual bool AdjustReferToTarget(const Pose2d& target, stLimit limit_v,int directions);

    /*
     * 按照路径点导航
     */

    bool execute_nodes(NavMessage::NewAction action);

    /*
     * 进出库
     */
    bool execute_InOut_space(NavMessage::NewAction action);

    /*
     * 根据自身和配对车辆状态，调整入库姿态，先到的进入车位1点，后车先到的倒车入库，后到的车与先到的车保持朝向一致
     * space:输入车位点,
     * target:输出目标点
     */
    virtual Navigation::MpcResult
    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
                           NavMessage::PathNode &target,int clampLifterAction=0);

    virtual Navigation::MpcResult RotateBeforeIntoExport();

    virtual Navigation::MpcResult RotateAfterOutExport();

    /// 双车出库摆正
    virtual Navigation::MpcResult DoubleOutSpaceCorrectTheta();

    virtual bool clamp_close();

    virtual bool clamp_half_open();

    virtual bool clamp_fully_open();

    virtual bool lifter_rise();

    virtual bool lifter_down();

    virtual bool singleClamp_fullyOpen_until_inspaceBegin();

    bool IsUperSpace(NavMessage::PathNode spaceNode);

    virtual int GetSpaceId(NavMessage::PathNode spaceNode);

    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, int& mpc_result, bool directY = false);

    /* 原地旋转mpc
     *
     * */
    bool
    Rotation_mpc_once(Pose2d current_to_target, stLimit limit_wmg, std::vector<double> &out, bool all_direct = true);

    virtual bool CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu);

    /*
     * 计算以当前点到目标点的曲率
     */
    float compute_cuv(const Pose2d &target);

    /*
     * 发布导航模块状态
     */
    static void pubStatuThreadFuc(void *p);

    virtual void publish_statu(NavMessage::NavStatu &statu);



protected:
    std::mutex mtx_;
    bool exit_ = false;
    ActionType actionType_ = eReady;
    std::thread *pubthread_ = nullptr;
    bool inited_ = false;
    Monitor_emqx *monitor_ = nullptr;
    Terminator_emqx *terminator_ = nullptr;  //本机emqx连接
    Terminator_emqx *brotherEmqx_ = nullptr;  //本机emqx连接

    TimedLockData<double> timeYawDiff1_;
    TimedLockData<double> timeYawDiff2_;
    TimedLockData<Pose2d> timedPose_;           //当前位姿
    TimedLockData<double> timedV_;              //底盘数据
    TimedLockData<double> timedA_;

    TimedLockData<DoorStatusMap> timed_Door_;     //门(1外内，2外内)
    std::map<int,TimedLockData<CarrierStatuRegionMap>> timed_Carrier_;     //载车板
    TimedLockData<ClampStatu> timed_clamp_;     //夹持杆
    TimedLockData<LifterStatus> timed_lifter_;    //提升机构
    TimedLockData<ClampStatu> timed_other_clamp_;
    TimedLockData<LifterStatus> timed_other_lifter_;

    TimedLockData<Pose2d> timedBrotherPose_;           //当前位姿
    TimedLockData<double> timedBrotherV_;              //底盘数据
    TimedLockData<double> timedBrotherA_;
    TimedLockData<NavMessage::NavStatu> timedBrotherNavStatu_;


    NavMessage::NavCmd global_navCmd_; //导航任务,包含一系列动作
    std::queue<NavMessage::NewAction> newUnfinished_cations_; //未完成的动作
    bool running_ = false;
    bool pause_ = false;
    bool cancel_ = false;

    int move_mode_ = eSingle;
    bool is_master_AGV = false; // 是否是主车
    float wheelBase_;   //两节agv的位姿与单节的位姿变换

    bool isInSpace_; //是否在车位或者正在进入车位
    std::string space_id_;
    TimedLockData<Trajectory> selected_traj_;
    TimedLockData<Trajectory> predict_traj_;

    SpaceNo2Region_Carry spaceNo_2_region_carrier_excel_;

    NavParameter::Navigation_parameter parameter_;
    RWheelPosition RWheel_position_;

    double obs_w_;
    double obs_h_;
    TimedLockData<bool> isFront_;


};

double limit_gause(double x, double min, double max);

double limit(double x, double min, double max);

double next_speed(double speed, double target_speed, double acc, double dt = 0.1);


#endif //LIO_LIVOX_CPP_MPC_NAVIGATION_H_