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 MpcResult {
        eMpcSuccess = 0,
        eMpcFailed,
        eImpossibleToTarget
    };


public:
    Navigation();

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

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

    virtual void ResetPose(const Pose2d &pose);

    void ResetStatu(double v, double a);

    virtual void ResetClamp(ClampStatu statu);

    virtual void ResetLifter(LifterStatus 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 void SwitchMode(int mode, float wheelBase);

    bool PoseTimeout();

    // 测试日志打印
    bool LogWriter(std::string lines){
        {
            std::ofstream ofs;  //创建流对象
            std::string log_file_dir = "../data/12_12/";
            std::string log_file_name = "12_12.txt";
            ofs.open(log_file_dir+log_file_name,std::ios::app);  //打开的地址和方式

            time_t t = time(0);
            char tmp[32]={NULL};
            strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S",localtime(&t));
            std::string time = tmp;
            std::string log_inf_line = lines + ". " + time;
            ofs << log_inf_line << std::endl;  //一行日志的内容
            ofs.close();
        }
        return true;
    }

protected:
    virtual void HandleAgvStatu(const MqttMsg &msg);

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

    virtual void SendMoveCmd(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 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);

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

    /*
     * 旋转摆正，朝向目标点,
     * 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);

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

    bool execute_nodes(NavMessage::NewAction action,int next_actionTypeID=-1);

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

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

    virtual Navigation::MpcResult
    RotateAfterOutSpace();

    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();

    static bool IsUperSpace(NavMessage::PathNode spaceNode);

    bool mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, 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<ClampStatu> timed_clamp_;     //加持杆
    TimedLockData<LifterStatus> timed_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;
    float wheelBase_;   //两节agv的位姿与单节的位姿变换

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

    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_