AGV/MPC/navigation.cpp

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

#include "navigation.h"
#include "loaded_mpc.h"
#include "rotate_mpc.h"
#include "../define/TimerRecord.h"

Navigation::~Navigation() {
    exit_ = true;
    if (terminator_)
        delete terminator_;
    if (brotherEmqx_)
        delete brotherEmqx_;
    if (monitor_)
        delete monitor_;

    if (pubthread_ != nullptr) {
        if (pubthread_->joinable())
            pubthread_->join();
        delete pubthread_;
    }
}

/*
 * 高斯函数压缩，x=3
 */
double limit_gause(double x, double min, double max) {
    double r = x >= 0 ? 1.0 : -1.0;
    double delta = max / 1.0;
    return (max - (max - min) * exp(-x * x / (delta * delta))) * r;

}

double limit(double x, double min, double max) {
    double ret = x;
    if (x >= 0) {
        if (x > max) ret = max;
        if (x < min) ret = min;
    } else {
        if (x < -max) ret = -max;
        if (x > -min) ret = -min;
    }
    return ret;

}

double next_speed(double speed, double target_speed, double acc, double dt) {
    if (fabs(target_speed - speed) / dt < acc) {
        return target_speed;
    } else {
        double r = target_speed - speed >= 0. ? 1.0 : -1.0;
        return speed + r * acc * dt;
    }
}

bool Navigation::PoseTimeout() {
//    if (timedPose_.timeout() == false && timedBrotherPose_.timeout() == false) {
    if (timedPose_.timeout() == false) {
        return false;
    } else {
        if (timedPose_.timeout()) {
            printf(" current pose is timeout \n");
        }
//        if (timedBrotherPose_.timeout()) {
//            printf(" brother pose is timeout \n");
//        }
        return true;
    }
}

bool Navigation::Init(const NavParameter::Navigation_parameter &parameter) {
    parameter_ = parameter;
    NavParameter::AgvEmqx_parameter agv_p = parameter.agv_emqx();
    if (monitor_ == nullptr) {
        monitor_ = new Monitor_emqx(agv_p.nodeid());
        if (monitor_->Connect(agv_p.ip(), agv_p.port()) == false) {
            printf(" agv emqx connected failed\n");
            return false;
        }
        monitor_->set_speedcmd_topic(agv_p.pubspeedtopic());
        monitor_->set_clampLifterCmd_topic(agv_p.pubclampliftertopic());
        monitor_->AddCallback(agv_p.subposetopic(), Navigation::RobotPoseCallback, this);
        monitor_->AddCallback(agv_p.subspeedtopic(), Navigation::RobotSpeedCallback, this);
    }

    NavParameter::Emqx_parameter terminal_p = parameter.terminal_emqx();
    if (terminator_ == nullptr) {
        terminator_ = new Terminator_emqx(terminal_p.nodeid());

        if (terminator_->Connect(terminal_p.ip(), terminal_p.port()) == false) {
            printf(" terminator emqx connected failed\n");
            return false;
        }

    }

    if (parameter.has_brother_emqx()) {
        NavParameter::BrotherEmqx brotherEmqx = parameter.brother_emqx();
        if (brotherEmqx_ == nullptr) {
            brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());

            if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
                brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback,
                                          this);
            } else {
                printf(" brother emqx connected failed\n");
                // return false;
            }
//
        }
    }
    inited_ = true;

    if (pubthread_ != nullptr) {
        exit_ = true;
        if (pubthread_->joinable())
            pubthread_->join();
        delete pubthread_;
    }
    exit_ = false;
    pubthread_ = new std::thread(&Navigation::pubStatuThreadFuc, this);

    return true;

}

void Navigation::RobotPoseCallback(const MqttMsg &msg, void *context) {
    Navigation *navigator = (Navigation *) context;
    NavMessage::LidarOdomStatu statu;
    if (msg.toProtoMessage(statu)) {
        navigator->ResetPose(Pose2d(statu.x(), statu.y(), statu.theta()));
    }
}

void Navigation::HandleAgvStatu(const MqttMsg &msg) {
    NavMessage::AgvStatu speed;
    if (IsMasterAGV()) {
        if (msg.toProtoMessage(speed)) {
            ResetStatu(speed.v(), speed.w());
            ResetClamp((ClampStatu) speed.clamp());
//            ResetOtherClamp((ClampStatu) speed.clamp_other());
            ResetLifter((LifterStatus) speed.lifter());
//            ResetOtherLifter((LifterStatus) speed.lifter_other());
            //printf(" clamp:%d   other:%d\n",speed.clamp(),speed.clamp_other());
        }
    } else {
        if (msg.toProtoMessage(speed)) {
            ResetStatu(speed.v(), speed.w());
            ResetClamp((ClampStatu) speed.clamp());
            ResetLifter((LifterStatus) speed.lifter());
        }
    }
    ResetDoor(speed.door());
    std::vector<int32_t> vecStatus;
    for (int i = 0; i < speed.zcb_size(); ++i) {
        vecStatus.push_back(speed.zcb(i));
    }
    ResetCarrier(vecStatus);
}


void Navigation::RobotSpeedCallback(const MqttMsg &msg, void *context) {
    Navigation *navigator = (Navigation *) context;
    navigator->HandleAgvStatu(msg);
}

void Navigation::pubStatuThreadFuc(void *p) {
    Navigation *navogator = (Navigation *) p;

    if (navogator) {
        while (navogator->exit_ == false) {
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            NavMessage::NavStatu statu;
            statu.set_main_agv(false);
            navogator->publish_statu(statu);
        }
    }
}

void Navigation::publish_statu(NavMessage::NavStatu &statu) {
    if (timedPose_.timeout() == false) {
        NavMessage::LidarOdomStatu odom;
        odom.set_x(timedPose_.Get().x());
        odom.set_y(timedPose_.Get().y());
        odom.set_theta(timedPose_.Get().theta());
        if (timedV_.timeout() == false)
            odom.set_v(timedV_.Get());
        if (timedA_.timeout() == false)
            odom.set_vth(timedA_.Get());
        statu.mutable_odom()->CopyFrom(odom);
    }
    statu.set_in_space(isInSpace_);
    if (isInSpace_) statu.set_space_id(space_id_);
    //发布nav状态
    statu.set_statu(actionType_); //
    statu.set_move_mode(move_mode_);
    if (running_) {
        statu.set_key(global_navCmd_.key());
    }
    //发布MPC信息
    //发布mpc 预选点
    if (selected_traj_.timeout() == false) {
        for (int i = 0; i < selected_traj_.Get().size(); ++i) {
            Pose2d pt = selected_traj_.Get()[i];
            NavMessage::Pose2d *pose = statu.mutable_selected_traj()->add_poses();
            pose->set_x(pt.x());
            pose->set_y(pt.y());
            pose->set_theta(pt.theta());
        }

    }
    //发布 mpc 预测点
    if (predict_traj_.timeout() == false) {
        for (int i = 0; i < predict_traj_.Get().size(); ++i) {
            Pose2d pt = predict_traj_.Get()[i];
            NavMessage::Pose2d *pose = statu.mutable_predict_traj()->add_poses();
            pose->set_x(pt.x());
            pose->set_y(pt.y());
            pose->set_theta(pt.theta());
        }
    }
//    std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;

    MqttMsg msg;
    msg.fromProtoMessage(statu);
    if (terminator_)
        terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(), msg);

    //发布位姿  ------robot状态--------------------------
    NavMessage::RobotStatu robot;
    if (CreateRobotStatuMsg(robot)) {
        if (terminator_) {
            MqttMsg msg;
            msg.fromProtoMessage(robot);
            terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
        }
    }
}

bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
    //printf(" %d %d %d \n",timedPose_.timeout(),timedV_.timeout(),timedA_.timeout());
    if (timedPose_.timeout() == false) {

        Pose2d pose = timedPose_.Get();
        robotStatu.set_x(pose.x());
        robotStatu.set_y(pose.y());
        robotStatu.set_theta(pose.theta());
        if ((timedV_.timeout() == false && timedA_.timeout() == false)) {
            NavMessage::AgvStatu plc;
            plc.set_v(timedV_.Get());
            plc.set_w(timedA_.Get());
            plc.set_clamp(timed_clamp_.Get());
            plc.set_lifter(timed_lifter_.Get());
            robotStatu.mutable_agvstatu()->CopyFrom(plc);
        }
        return true;
    }
    return false;
}

void Navigation::ResetStatu(double v, double a) {
    timedV_.reset(v, 0.5);
    timedA_.reset(a, 0.5);
}

void Navigation::ResetClamp(ClampStatu statu) {
    timed_clamp_.reset(statu, 1);
}

void Navigation::ResetLifter(LifterStatus status) {
    timed_lifter_.reset(status, 1);
}


void Navigation::ResetPose(const Pose2d &pose) {
    timedPose_.reset(pose, 1.0);
    if (pose.theta() > 0) {
        if (parameter_.main_agv()) {
            isFront_.reset(true, 1.0);
        } else {
            isFront_.reset(false, 1.0);
        }
    } else {
        if (parameter_.main_agv()) {
            isFront_.reset(false, 1.0);
        } else {
            isFront_.reset(true, 1.0);
        }
    }

}

void Navigation::Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
    cancel_ = true;
    response.set_ret(0);
}

bool Navigation::Stop() {
    if (monitor_) {
        monitor_->stop();
        while (cancel_ == false) {
            if (timedV_.timeout() == false && timedA_.timeout() == false) {
                if (fabs(timedV_.Get()) < 1e-2 && fabs(timedA_.Get()) < 1e-3)
                    return true;
                else
                    monitor_->stop();
                printf("Stoped wait V/A ==0(1e-4,1e-4),V:%f,A:%f\n", fabs(timedV_.Get()), fabs(timedA_.Get()));
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            } else {
                printf("Stop failed V/A timeout\n");
                return false;
            }
        }
    }
    printf("stoped ret false.\n");
    return false;
}

bool Navigation::SlowlyStop() {

    double acc = 3.0;
    double dt = 0.1;
    while (cancel_ == false) {
        if (timedV_.timeout() == false) {

            double v = timedV_.Get();
            if (fabs(v < 1e-2 && fabs(timedA_.Get()) < 1e-3))
                return true;

            if (v > 0) {
                double new_v = v - acc * dt;
                new_v = new_v > 0 ? new_v : 0;
                SendMoveCmd(move_mode_, actionType_, new_v, 0);
            } else {
                double new_v = v + acc * dt;
                new_v = new_v < 0 ? new_v : 0;
                SendMoveCmd(move_mode_, actionType_, new_v, 0);
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        } else {
            printf("Stop failed V/A timeout\n");
            return false;
        }
    }
    printf("stoped ret false.\n");
    return false;
}


Navigation::Navigation() {
    is_master_AGV = false;
    wheelBase_ = 0;
    isInSpace_ = false; //是否在车位或者正在进入车位
    RWheel_position_ = eUnknow;
    move_mode_ = eSingle;
    monitor_ = nullptr;
    terminator_ = nullptr;
    obs_h_ = 4.0;
    obs_w_ = 2.0;
    timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5);

    LoadSpaceNo2CarrierNo();
}

void Navigation::BrotherAgvStatuCallback(const MqttMsg &msg, void *context) {
    Navigation *navigator = (Navigation *) context;

    NavMessage::NavStatu brother_statu;
    if (msg.toProtoMessage(brother_statu) == false) {
        std::cout << " msg transform to AGVStatu failed,msg:" << msg.data() << std::endl;
        return;
    }
    //std::cout<<brother_nav.DebugString()<<std::endl<<std::endl;
    if (brother_statu.has_odom()) {
        NavMessage::LidarOdomStatu odom = brother_statu.odom();
        Pose2d pose(odom.x(), odom.y(), odom.theta());
        navigator->timedBrotherPose_.reset(pose, 1);
        navigator->timedBrotherV_.reset(odom.v(), 1);
        navigator->timedBrotherA_.reset(odom.vth(), 1);
        navigator->timedBrotherNavStatu_.reset(brother_statu, 0.1);
    }
}

//未使用
/*
bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect) {

    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        if (timedPose_.timeout()) {
            printf(" RotateBytarget failed  pose timeout\n");
            return false;
        }
        if (timedA_.timeout()) {
            printf(" RotateBytarget failed  wmg timeout\n");
            return false;
        }
        Pose2d targetInPose = Pose2d::relativePose(target, timedPose_.Get());
        float yawDiff = Pose2d::vector2yaw(targetInPose.x(), targetInPose.y());
        float minYawdiff = yawDiff;

        if (anyDirect) {
            Pose2d p0 = timedPose_.Get();
            float yawdiff[4] = {0};
            yawdiff[0] = yawDiff;

            Pose2d relativePose;
            relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI / 2));
            yawdiff[1] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());  ////使用减法，保证角度在【-pi pi】
            relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, M_PI));
            yawdiff[2] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());
            relativePose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, 3 * M_PI / 2));
            yawdiff[3] = Pose2d::vector2yaw(relativePose.x(), relativePose.y());
            for (int i = 1; i < 4; ++i) {
                if (fabs(yawdiff[i]) < fabs(minYawdiff)) {
                    minYawdiff = yawdiff[i];
                    targetInPose = Pose2d::relativePose(target, p0 - Pose2d(0, 0, i * M_PI / 2.0));
                }
            }
        }
        //std::cout<<" Rotate refer to target:"<<target<<",targetInPose:"
        //<<targetInPose<<",anyDirect:"<<anyDirect<<std::endl;
        //以当前点为原点，想方向经过目标点的二次曲线曲率>0.25,则需要调整 y=a x*x

        float cuv = compute_cuv(targetInPose);
        float dt = 0.1;
        float acc_angular = 15 * M_PI / 180.0;
        if (cuv > 2 * M_PI / 180.0) {
            double theta = limit_gause(minYawdiff, limit_rotate.min, limit_rotate.max);
            double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
            double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);

            SendMoveCmd(move_mode_, eRotation, 0, limit_angular);
            actionType_ = eRotation;
            printf(" Rotate | input angular:%f,next angular:%f,down:%f diff:%f anyDirect:%d\n",
                   timedA_.Get(), angular, limit_angular, minYawdiff, anyDirect);

            continue;
        } else {
            if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
                printf(" Rotate refer target completed,cuv:%f\n", cuv);
                return true;
            }
            continue;
        }
    }
    return false;
}
*/

bool Navigation::AdjustReferToTarget(const Pose2d &target, stLimit limit_rotate, int directions) {
    std::cout << "Adjust to : " << target << std::endl;
    if (inited_ == false) {
        printf(" MPC_rotate has not inited\n");
        return false;
    }
    if (PoseTimeout()) {
        printf(" MPC_rotate Error:Pose is timeout \n");
        return false;
    }

    Pose2d current = timedPose_.Get();
    NavMessage::PathNode yawTarget;
    yawTarget.set_x(current.x() + 100.);
    yawTarget.set_y(current.y());

    std::cout << "Adjust RotateRefer : " << Pose2d(yawTarget.x(), yawTarget.y(), 0) << std::endl;
    RotateReferToTarget(yawTarget, limit_rotate, directions);

    current = timedPose_.Get();
    Pose2d diff = target - current;
    Pose2d newTarget;
    NavMessage::PathNode node;
    if (fabs(diff.x()) > fabs(diff.y())) {
        newTarget = Pose2d(current.x(), target.y(), current.theta());
        node.set_theta(M_PI / 2.0);
    } else {
        newTarget = Pose2d(target.x(), current.y(), current.theta());
        node.set_theta(0);
    }
    node.set_x(newTarget.x());
    node.set_y(newTarget.y());
    node.set_l(0.03);
    node.set_w(0.2);

    stLimit limitV = {parameter_.inoutvlimit().min(), parameter_.inoutvlimit().max()};
    stLimit limitW = {0.03, 0.1};
    std::cout << " Adjust MoveTarget:" << Pose2d(node.x(), node.y(), node.theta()) << std::endl;
    return MoveToTarget(node, limitV, limitW, true, true);
}

Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node, stLimit limit_rotate, int directions) {
    if (inited_ == false) {
        printf(" MPC_rotate has not inited\n");
        return eMpcFailed;
    }
    if (PoseTimeout()) {
        printf(" MPC_rotate Error:Pose is timeout \n");
        return eMpcFailed;
    }

    printf(" exec MPC_rotate autoDirect:%d\n", directions);

    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        if (pause_ == true) {
            //发送暂停消息
            continue;
        }
        if (PoseTimeout()) {
            printf(" MPC_rotate Error:Pose is timeout \n");
            return eMpcFailed;
        }
        if (timedA_.timeout()) {
            printf(" MPC_rotate Error:v/a is timeout \n");
            return eMpcFailed;
        }

        Pose2d current = timedPose_.Get();
        Pose2d target(node.x(), node.y(), node.theta());
        Pose2d targetInPose = Pose2d::relativePose(target, current);
        // 需要旋转的角度
        float target_yaw_diff = Pose2d::vector2yaw(targetInPose.x(), targetInPose.y());

        // 自由方向
        if (directions) {
            std::vector<double> yaw_diffs;
            if (directions & Direction::eForward) {
                yaw_diffs.push_back(target_yaw_diff);
            }
            if (directions & Direction::eBackward) {
                Pose2d relative_pose = Pose2d::relativePose(targetInPose, Pose2d(0, 0, M_PI));
                yaw_diffs.push_back(Pose2d::vector2yaw(relative_pose.x(), relative_pose.y()));
            }
            if (directions & Direction::eLeft) {
                Pose2d relative_pose = Pose2d::relativePose(targetInPose, Pose2d(0, 0, 1.0 / 2 * M_PI));
                yaw_diffs.push_back(Pose2d::vector2yaw(relative_pose.x(), relative_pose.y()));
            }
            if (directions & Direction::eRight) {
                Pose2d relative_pose = Pose2d::relativePose(targetInPose, Pose2d(0, 0, 3.0 / 2 * M_PI));
                yaw_diffs.push_back(Pose2d::vector2yaw(relative_pose.x(), relative_pose.y()));
            }

            for (auto i = 0; i < yaw_diffs.size(); ++i) {
                if (fabs(yaw_diffs[i]) < fabs(target_yaw_diff))
                    target_yaw_diff = yaw_diffs[i];
            }
//            std::cout << std::endl;
//            std::cout << std::endl << " min_diff: " << current_to_target.theta() << std::endl;
        }

        //一次变速
        std::vector<double> out;
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = Rotation_mpc_once(Pose2d(0, 0, target_yaw_diff), limit_rotate, out);
        }, "Rotation_mpc_once");
        if (ret == false) {
            Stop();
            return eMpcFailed;
        }

        //下发速度
        if (fabs(target_yaw_diff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" MPC_rotate | refer target completed\\n,cuv:%f\n", target_yaw_diff);
            Stop();
            return eMpcSuccess;
        } else {
            const int down_count = 3;
            double v[down_count] = {0, 0, 0};
            double w[down_count] = {out[0], out[1], out[2]};

            SendMoveCmd(0, move_mode_, eRotation, v, w);
            actionType_ = eRotation;
            double input_w = timedA_.Get();
            /*
            for (int i = 0; i < down_count; ++i) {
                printf("  MPC_rotate |P[%d], input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
                       i, input_w, w[i], w[i] / M_PI * 180, target_yaw_diff, directions);
            }*/
        }
        continue;
    }
    return eMpcFailed;
}

bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w,
                              bool anyDirect, bool enable_rotate, int clampLifterAction) {
    if (IsArrived(node)) {
        return true;
    }
    bool already_in_mpc = false;
    int retry_count = 0;
    int retry_count_limit = 3;
    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        if (PoseTimeout()) {
            printf(" navigation Error:Pose is timeout \n");
            break;
        }
        Pose2d current = timedPose_.Get();

        if (IsArrived(node)) {
            break;
        }
        //两个方向都无法到达目标点,旋转 朝向目标点
        bool x_enable = PossibleToTarget(node, false);
        bool y_enable = PossibleToTarget(node, true);
        bool enableTotarget = x_enable || y_enable;
        if (anyDirect == false) {
            enableTotarget = x_enable;
        }
        //巡线无法到达目标点
        Pose2d target(node.x(), node.y(), 0);
        Pose2d targetInCurrent = Pose2d::relativePose(target, current);

        if (GetCurrentAction().type() != 1 && GetCurrentAction().type() != 2) {
            if (enableTotarget == false) {
                int directions = Direction::eForward;
                if (anyDirect)
                    directions = Direction::eForward | Direction::eRight | Direction::eLeft | Direction::eBackward;
                bool adjustRet = AdjustReferToTarget(target, limit_w, directions);
                if (adjustRet == false)
                    return false;
            }

        }

        already_in_mpc = true;
        MpcResult ret = MpcToTarget(node, limit_v, anyDirect, clampLifterAction);
        if (ret == eMpcSuccess) {
            printf(" MPC to target:%f %f l:%f,w:%f theta:%f  completed\n",
                   node.x(), node.y(), node.l(), node.w(), node.theta());
            break;
        } else if (ret == eImpossibleToTarget) {
            printf(" MPC to target:%f %f l:%f,w:%f theta:%f  impossible ,retry ...\n",
                   node.x(), node.y(), node.l(), node.w(), node.theta());
        } else {
            retry_count++;
            printf("MpcToTarget failed,ret:%d\n", ret);
            if (retry_count > retry_count_limit)
                return false;
        }
    }
    if (cancel_) {
        return false;
    }
    return true;
}

bool Navigation::execute_nodes(NavMessage::NewAction action) {
    if (action.type() == 3 || action.type() == 4) //最优动作导航 or 导航中保证朝向严格一致
    {
        if (!parameter_.has_nodeangularlimit() || !parameter_.has_nodevelocitylimit() || action.pathnodes_size() == 0) {
            std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
            return false;
        }
        bool anyDirect = (action.type() == 3);

        //速度限制
        stLimit wmg_limit = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
        stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};

        /////////////////////////////////////////
        int clampLifterAction = 0;
        if (move_mode_ == eSingle) {
            clampLifterAction |= eByteClampHalfOpen;
        } else {
            clampLifterAction |= eByteLifterDown;
        }
        ////////////////////////////////////////////////////////////
        for (int i = 0; i < action.pathnodes_size(); ++i) {
            NavMessage::PathNode node = action.pathnodes(i);
            if (i < action.pathnodes_size() - 1) {
                NavMessage::PathNode next = action.pathnodes(i + 1);
                Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
                node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
                node.set_l(0.03);
                node.set_w(0.25);
            } else {
                NavMessage::PathNode befor = action.pathnodes(i - 1);
                Pose2d vec(node.x() - befor.x(), node.y() - befor.y(), 0);
                node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
                node.set_l(0.02);
                node.set_w(0.25);
            }
            int directions = Direction::eForward;
            if (anyDirect)
                directions = Direction::eForward | Direction::eBackward |
                             Direction::eLeft | Direction::eRight;
            /*if(i>=1 && move_mode_==eSingle){
                //马路Y巡航，需要判断等待
                Pose2d targetInAGV = Pose2d::relativePose(Pose2d(node.x(), node.y(), 0), timedPose_.Get());
                double distance=Pose2d::distance(Pose2d(0,0,0),targetInAGV);
                double thresh=0.5*obs_w_*distance;
                while(cancel_==false) {
                    usleep(1000*500);
                    if(timedPose_.timeout() || timedBrotherPose_.timeout()){
                        return false;
                    }
                    Pose2d otherInAGV = Pose2d::relativePose(timedBrotherPose_.Get(), timedPose_.Get());
                    double dot = targetInAGV.x() * otherInAGV.x() + targetInAGV.y() * otherInAGV.y();
                    if (dot > thresh) {  //目标点与other在当前车的同一朝向上，需要等待other点到位
                        printf(" need to wait other,dot:%f,thresh:%f\n",dot,thresh);
                        if(fabs(otherInAGV.x())>obs_w_*3 && fabs(otherInAGV.y())<0.2){
                            std::cout<<"Wait other agv completed otherInCurrent:"<<otherInAGV<<std::endl;
                            break;
                        }else{
                            std::cout<<"Wait other agv ,otherInCurrent:"<<otherInAGV<<std::endl;
                        }
                    }else{
                        printf(" not need wait other,dot:%f,thresh:%f\n",dot,thresh);
                        break;
                    }
                }
            }*/

            if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true, clampLifterAction) == false)
                return false;
            else
                isInSpace_ = false;
        }
        if (cancel_ == true) {
            return false;
        }

        return true;
    }

    printf("execute PathNode failed ,action type is not 3/4  :%d\n", action.type());
    return false;
}

Navigation::MpcResult Navigation::DoubleOutSpaceCorrectTheta() {
    if (move_mode_ != eDouble)
        return eMpcSuccess;
    int clampLifterAction = 0;
    if (move_mode_ == eDouble) {
        clampLifterAction |= eLifterDown;
    }
    Pose2d init = timedPose_.Get();
    double diff1 = fabs(M_PI / 2.0 - init.theta());
    double diff2 = fabs(-M_PI / 2.0 - init.theta());
    double target = diff1 < diff2 ? M_PI / 2.0 : -M_PI / 2.0;
    stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        Pose2d current = timedPose_.Get();
        double yawDiff = (target - current.theta());

        //一次变速
        std::vector<double> out;
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
        }, "Rotation_mpc_once");
        if (ret == false) {
            Stop();
            return eMpcFailed;
        }

        //下发速度
        if (fabs(yawDiff) < 0.2 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" DoubleOutSpaceCorrectTheta refer target completed\\n,cuv:%f\n", yawDiff);
            Stop();
            if (WaitClampLifterStatu(clampLifterAction) == false) {
                return eMpcFailed;
            }
            return eMpcSuccess;
        } else {
            const int down_count = 3;
            double v[down_count] = {0, 0, 0};
            double w[down_count] = {out[0], out[1], out[2]};
            SendMoveCmd(clampLifterAction, move_mode_, eRotation, v, w);
            actionType_ = eRotation;
            printf(" DoubleOutSpaceCorrectTheta | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
        }
        continue;
    }
    return eMpcFailed;
}


Navigation::MpcResult Navigation::RotateBeforeIntoExport() {
    if (move_mode_ != eDouble)
        return eMpcSuccess;
    Pose2d init = timedPose_.Get();
    double diff1 = fabs(M_PI / 2.0 - init.theta());
    double diff2 = fabs(-M_PI / 2.0 - init.theta());
    double target = diff1 > diff2 ? M_PI / 2.0 : -M_PI / 2.0;
    stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        Pose2d current = timedPose_.Get();
        double yawDiff = (target - current.theta());

        //一次变速
        std::vector<double> out;
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
        }, "Rotation_mpc_once");
        if (ret == false) {
            Stop();
            return eMpcFailed;
        }

        //下发速度
        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
            Stop();
            return eMpcSuccess;
        } else {
            const int down_count = 3;
            double v[down_count] = {0, 0, 0};
            double w[down_count] = {out[0], out[1], out[2]};
            SendMoveCmd(0, move_mode_, eRotation, v, w);
            actionType_ = eRotation;
//            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
//                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
        }
        continue;
    }
    return eMpcFailed;
}

Navigation::MpcResult
Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
                                   NavMessage::PathNode &target, int clampLifterAction) {
//    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
    if (timedPose_.timeout()) {
        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
        return eMpcFailed;
    }
    NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();

    stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
    double acc_angular = 20 * M_PI / 180.0;
    double dt = 0.1;
    Pose2d current = timedPose_.Get();
    double x = space.x();
    double y = space.y();
    Pose2d vec(x - current.x(), y - current.y(), 0);
    double vecTheta = Pose2d::vector2yaw(vec.x(), vec.y());
    bool isFront = isFront_.Get();
    printf(" front__:%d  theta:%f\n", isFront, vecTheta);
    if (vecTheta < 0.)
        isFront = !isFront;

    //如果是后车，计算目标点（车位点-轴距）
    if (isFront == false) {
        printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
        if (move_mode_ == eSingle) {
            x -= wheelbase * cos(vecTheta);
            y -= wheelbase * sin(vecTheta);
            printf("确定车位点：eSingle\n");
        }
    } else { //当后车先到,倒车入库
        printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
        //rotated.mutable_theta() = space.theta();
        //rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
    }

    Pose2d spaceInCurrent = Pose2d::relativePose(Pose2d(space.x(), space.y(), 0), current);


    target.set_x(x);
    target.set_y(y);
    target.set_theta(current.theta());
    target.set_l(0.02);
    target.set_w(0.5);
    target.set_id(space.id());
    printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n", x, y, target.theta());

//    //整车协调的时候，保持前后与车位一致即可
//    if (move_mode_ == eDouble) {
//        double yawDiff1 = (rotated - timedPose_.Get()).theta();
//        double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
//        if (fabs(yawDiff1) < fabs(yawDiff2)) {
//            rotated = rotated + Pose2d(0, 0, M_PI);
//        }
//    }

    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(30));
        Pose2d current_now = timedPose_.Get();
        if (spaceInCurrent.x() < 0.)
            current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI);
        double yawDiff = vecTheta - current_now.theta();

        //一次变速
        std::vector<double> out;
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
        }, "Rotation_mpc_once");
        if (ret == false) {
            Stop();
            return eMpcFailed;
        }

        //下发速度
        if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
            Stop();
            return eMpcSuccess;
        } else {
            const int down_count = 3;
            double v[down_count] = {0, 0, 0};
            double w[down_count] = {out[0], out[1], out[2]};
            SendMoveCmd(clampLifterAction, move_mode_, eRotation, v, w);
            actionType_ = eRotation;
//            printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
//                   timedA_.Get(), out[0], out[0] / M_PI * 180, yawDiff);
        }
        continue;

    }
    return eMpcFailed;

}

bool Navigation::IsUperSpace(NavMessage::PathNode spaceNode) {
    int space_id = GetSpaceId(spaceNode);
    if (space_id < 0)
        return false;
    if (space_id > 99 && space_id < 1000) {
        return true;
    }
    return false;
}

int Navigation::GetSpaceId(NavMessage::PathNode spaceNode) {
    if (spaceNode.has_id() == false) {
        return -1;
    }
    std::string space_flag = "S_";
    if (spaceNode.id().find(space_flag) >= 0 && spaceNode.id().size() > 2) {
        std::string id = spaceNode.id().substr(space_flag.size(), spaceNode.id().length() - 1);
        int space_id = stoi(id);
        return space_id;
    }
    return -1;
}

bool Navigation::TargetIsEntrance(NavMessage::PathNode node) {
    int space_id = GetSpaceId(node);
    return space_id == 1101 || space_id == 1102;
}

bool Navigation::TargetIsExport(NavMessage::PathNode node) {
    int space_id = GetSpaceId(node);
    return space_id == 1101 || space_id == 1102;
}


bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
    printf("execute_InOut_space\n");
    if (action.type() != 1 && action.type() != 2) {
        printf(" Inout_space failed : msg action type must ==1\n ");
        return false;
    }
    if (!parameter_.has_inoutvlimit() || !action.has_spacenode() ||
        !action.has_streetnode()) {
        std::cout << "execute enter_space failed ,Action miss some infos:" << action.DebugString() << std::endl;
        return false;
    }
    if (PoseTimeout() == true) {
        printf(" inout_space failed type:%d : current pose is timeout\n", action.type());
        return false;
    }

    stLimit limit_v = {parameter_.inoutvlimit().min(), parameter_.inoutvlimit().max()};
    stLimit limit_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
    //入库,起点为streetNode
    if (action.type() == 1) {
        Pose2d vec(action.spacenode().x() - action.streetnode().x(),
                   action.spacenode().y() - action.streetnode().y(),
                   0);
        action.mutable_streetnode()->set_l(0.02);
        action.mutable_streetnode()->set_w(0.2);
        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()) + M_PI / 2.0);
        /*
         * 是否需要添加判断，距离较远才先运动到马路点=============================
         */
        int clampLifterAction = 0;
        printf("IsUperSpace\n");
        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
            clampLifterAction |= eByteLifterUp;
        } else {    //否则下降
            clampLifterAction |= eByteLifterDown;
        }
        if (move_mode_ == eSingle)
            clampLifterAction |= eByteClampHalfOpen;

        if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true, clampLifterAction) == false) {
            printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
            return false;
        }
        //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false）
        printf("inout ----------------------------------------\n");

        //取车流程，送车到出口前旋转180
        if (move_mode_ == eDouble && TargetIsExport(action.spacenode()) && GetSpaceId(action.spacenode()) != 1102) {
            Pose2d current_pose = timedPose_.Get();
            double current_theta = current_pose.theta();
            if (RotateBeforeIntoExport() != eMpcSuccess) {
                printf("Rotate before in entrance failed\n");
                return false;
            }
        }

        //计算车位朝向
        action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
        //计算当前车是进入车位1点还是2点,
        NavMessage::PathNode new_target;
        bool retRotate = RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(),
                                                new_target, clampLifterAction) == eMpcSuccess;

        if (retRotate == false) {
            return false;
        }
        //去出入口，等内门打开
        if (TargetIsExport(action.spacenode()) || TargetIsEntrance(action.spacenode())) {
            printf(" Wait Inside door opened.......\n");
            int port_id = GetPortIDBySpace(action.spacenode());
            if (WaitInsideDoorCompleted(port_id, eDoorOpened) == false) {
                return false;
            }
            printf(" Wait Inside door opened completed!!!\n");
        } else {
            //去车位点
            if (IsUperSpace(action.spacenode())) {
                printf(" Wait Carrier down.......\n");
                if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierDown) == false) {
                    return false;
                }
                printf(" Wait Carrier down completed!!!\n");
            } else {
                if (!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode())) {
                    printf(" Wait Carrier up.......\n");

                    if (WaitCarrierCompleted(GetSpaceId(action.spacenode()), eCarrierUp) == false) {
                        return false;
                    }
                    printf(" Wait Carrier up completed!!!\n");
                } else {
                    printf(" Wait inside door open.......\n");
                    if (WaitInsideDoorCompleted(GetSpaceId(action.spacenode()), eDoorOpened) == false)
                        return false;
                    printf("inside door opened\n");
                }
            }
        }

        if (IsUperSpace(action.spacenode())) {
            if (lifter_rise() == false) {
                return false;
            }
        }

        // 单车进库必须全打开(先半开，行走时全开)
        if (move_mode_ == eSingle) {
            if (clamp_half_open() == false) {
                return false;
            }
        }

        //入库
        Pose2d rotated = timedPose_.Get();
        double x = new_target.x();
        double y = new_target.y();
        Pose2d vecSpace(x - rotated.x(), y - rotated.y(), 0);
        rotated.mutable_theta() = Pose2d::vector2yaw(vecSpace.x(), vecSpace.y());
        bool isFront = isFront_.Get();
        if (rotated.theta() < 0.)
            isFront = !isFront;
        // 后进车，且单车，且先进车已过后进车的目标点
        Pose2d target(new_target.x(), new_target.y(), 0);
        if (isFront == false && move_mode_ == eSingle) {
            while (true) {
                if (cancel_) {
                    return false;
                }
                if (move_mode_ == eDouble) {
                    clampLifterAction |= eLifterDown;
                }
                Pose2d current = timedPose_.Get();
                Pose2d brother_pose = timedBrotherPose_.Get();
                Pose2d vecCurent2T = current - target;
                Pose2d vecBrother2T = brother_pose - target;
                double dot = vecCurent2T.x() * vecBrother2T.x() + vecCurent2T.y() * vecBrother2T.y();
                if (timedBrotherNavStatu_.Get().in_space() == true && dot < 0.) {
                    printf("先进车已经过后进车目标点，后进车停止等待 dot=%f\n", dot);
                    break;
                }
                printf(" 后车 等待 先车先入库。。。\n");
                usleep(1000 * 500);
                continue;
            }
        }
        isInSpace_ = true;
        printf("Enter space beg...\n");
        clampLifterAction = 0;
        if (IsUperSpace(action.spacenode())) {  //进载车板库前提升机构上升
            clampLifterAction |= eByteLifterUp;
        } else {    //否则下降
            clampLifterAction |= eByteLifterDown;
        }
        printf(" clampLifter_statu1 :%d\n", clampLifterAction);
        if (move_mode_ == eSingle)
            clampLifterAction |= eByteClampFullyOpen;  //////// 后续再改为全打开位
        else
            clampLifterAction |= eByteCLampClose;

        printf(" clampLifter_statu2 :%d\n", clampLifterAction);
        if (!TargetIsExport(action.spacenode()) && !TargetIsEntrance(action.spacenode()))
            limit_w = {0.01, 0.05}; //进库纠偏角速度限制
        else {
            // 整车进 出入口速度调整为与巡线一致
            if (move_mode_ == eDouble) {
                limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
                limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
            }
        }
        bool retMove = MoveToTarget(new_target, limit_v, limit_w, true, false, clampLifterAction);

        if (retMove == false) {
            printf(" Enter space:%s failed. type:%d\n", action.spacenode().id().data(), action.type());
            return false;
        }

    } else if (action.type() == 2) {
        //出库,起点为spaceNode
        // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
        Pose2d vec(action.streetnode().x() - action.spacenode().x(),
                   action.streetnode().y() - action.spacenode().y(), 0);
        float theta = Pose2d::vector2yaw(vec.x(), vec.y());
        Pose2d space(action.spacenode().x(), action.spacenode().y(), theta);
        Pose2d current = timedPose_.Get();
        Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(), current.y(), 0), space));
        // 整车从出入口出库速度调整为与巡线一致
        if (move_mode_ == eDouble && (TargetIsExport(action.spacenode()) || TargetIsExport(action.spacenode()))) {
            limit_v = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
            limit_w = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
        }
        if (fabs(diff.x()) < 0.15 || fabs(diff.y()) < 0.10) {
            //出库，移动到马路点，允许自由方向，不允许中途旋转
            action.mutable_streetnode()->set_theta(theta);
            action.mutable_streetnode()->set_l(0.03);
            action.mutable_streetnode()->set_w(0.2);
            std::cout << " Out space target street node:" << space << std::endl;
            if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, false) == false) {
                printf(" out space failed type:%d: MoveTo StreetNode failed\n", action.type());
                return false;
            }
        } else {
            std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
                      << std::endl;
            return false;
        }
        isInSpace_ = false;
        //摆正
        if (GetRemainActionsCount() > 1) {
            if (DoubleOutSpaceCorrectTheta() != eMpcSuccess)
                return false;
        }
    }
    return true;
}

/* current_to_target: current_to_target.theta()为所需旋转的角度
 * */
bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit limit_wmg, std::vector<double> &out,
                                   bool all_direct) {
    if (PoseTimeout() == true) {
        printf(" Rotation_mpc_once Error:Pose is timeout \n");
        return false;
    }
    if (timedV_.timeout() == true || timedA_.timeout() == true) {
        printf(" Rotation_mpc_once Error:V/A is timeout \n");
        return false;
    }

    Pose2d pose = timedPose_.Get();
    double line_velocity = timedV_.Get();    //从plc获取状态
    double angular_velocity = timedA_.Get();

    Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
    statu[0] = pose.x();
    statu[1] = pose.y();
    statu[2] = pose.theta();
    statu[3] = line_velocity;
    statu[4] = angular_velocity;

    NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
    double obs_w = 0.95;//0.85;
    double obs_h = 1.55;//1.45;

    MpcError ret = failed;
    Pose2d brother = timedBrotherPose_.Get();
    Pose2d brother_in_self = Pose2d::relativePose(brother, pose);
//    std::cout<<" brother_in_self: "<<brother_in_self<<std::endl;
    if (move_mode_ == eDouble) {
        brother_in_self = Pose2d(pose.x() + 100, pose.y() + 100, 0);
    }
    RotateMPC MPC(brother_in_self, obs_w, obs_h, limit_wmg.min, limit_wmg.max);
    RotateMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
                                          mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()};
    ret = MPC.solve(current_to_target, statu, parameter, out);
    if (ret == no_solution) {
        printf(" Rotation_mpc solve no solution set v/w = 0\n");
        out.clear();
        out.push_back(0);
        out.push_back(0);
        out.push_back(0);
    } else {
//        double min_angular_velocity = 1.0 / 180 * M_PI;
        double min_angular_velocity = 0.01;
        const int down_count = 3;
        for (int i = 0; i < down_count; ++i) {
            if (fabs(out[i]) < min_angular_velocity) {
                if (out[i] > 0)
                    out[i] = min_angular_velocity;
                else if (out[i] < 0)
                    out[i] = -min_angular_velocity;
                else {}
            }
        }
    }
    return ret == success || ret == no_solution;
}

bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double> &out, bool directY) {
    if (PoseTimeout() == true) {
        printf(" MPC once Error:Pose is timeout \n");
        return false;
    }
    if (timedV_.timeout() == true || timedA_.timeout() == true) {
        printf(" MPC once Error:V/A is timeout \n");
        return false;
    }
    if (traj.size() == 0) {
        printf("traj size ==0\n");
        return false;
    }

    Pose2d pose = timedPose_.Get();
    double velocity = timedV_.Get();    //从plc获取状态
    double angular = timedA_.Get();

//    float diffYaw1=timedPose_.Get().m_diffYaw1;
//    float diffYaw2=timedPose_.Get().m_diffYaw2;

    Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态

    statu[0] = pose.x();
    statu[1] = pose.y();
    statu[2] = pose.theta();
    statu[3] = velocity;
    statu[4] = angular;

    Trajectory optimize_trajectory;
    Trajectory selected_trajectory;
    NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
    double obs_w = obs_w_;// 0.95;//0.85;
    double obs_h = obs_h_;// 1.55;//1.45;

    if (directY == true) {
        //将车身朝向旋转90°，车身朝向在(-pi,pi]
        statu[2] += M_PI / 2;
        if (statu[2] > M_PI)
            statu[2] -= 2 * M_PI;
        mpc_parameter = parameter_.y_mpc_parameter();
        obs_w = obs_h_;// 0.95;//0.85;
        obs_h = obs_w_;//1.55;//1.45;
    }

    Pose2d brother = timedBrotherPose_.Get();
    Pose2d relative = Pose2d::relativePose(brother, pose);
    if (directY)
        relative = Pose2d::relativePose(brother.rotate(brother.x(), brother.y(), M_PI / 2),
                                        pose.rotate(pose.x(), pose.y(), M_PI / 2));//计算另一节在当前小车坐标系下的坐标
    std::cout << "pose:" << pose << ",  brother:" << brother << ", relative:" << relative << std::endl;
    if (move_mode_ == eDouble)
        relative = Pose2d(-1e8, -1e8, 0);

    MpcError ret = failed;
    LoadedMPC MPC(relative, obs_w, obs_h, limit_v.min, limit_v.max);
    // 巡线最大角速度固定0.1745
    LoadedMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
                                          mpc_parameter.acc_velocity(), 0.1745};
    //printf(" r:%f  dt:%f acc:%f  acc_a:%f\n",mpc_parameter.shortest_radius(),
    //       mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
    ret = MPC.solve(traj, traj[traj.size() - 1], statu, parameter,
                    out, selected_trajectory, optimize_trajectory, directY);

    //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
    if (ret == no_solution) {
        printf(" mpc solve no solution set v/w = 0\n");
        out.clear();
        out.push_back(0);
        out.push_back(0);
        out.push_back(0);
        out.push_back(0);
        out.push_back(0);
        out.push_back(0);
    }
    //
//    diff_yaw.push_back(0);
//    diff_yaw.push_back(0);

    predict_traj_.reset(optimize_trajectory, 1);
    selected_traj_.reset(selected_trajectory, 1);
    return ret == success || ret == no_solution;
}

bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
    if (timedPose_.timeout() || timedV_.timeout() || timedA_.timeout()) {
        printf(" pose / v / w    timeout,IsArrived return false\n");
        return false;
    }
    Pose2d current = timedPose_.Get();
    double x = current.x();
    double y = current.y();
    double tx = targetNode.x();
    double ty = targetNode.y();
    double l = fabs(targetNode.l());
    double w = fabs(targetNode.w());
    double theta = -targetNode.theta();
    /*if (newUnfinished_cations_.front().type() == 1 && GetSpaceId(targetNode) > 0) {//入库只判断前后方向上是否到达
        tx = x;
    }*/
    if (l < 1e-10 || w < 1e-10) {
        printf("IsArrived: Error target l or w == 0\n");
        return false;
    }

    double a = (x - tx) * cos(theta) - (y - ty) * sin(theta);
    double b = (x - tx) * sin(theta) + (y - ty) * cos(theta);

    if (pow(a / l, 8) + pow(b / w, 8) <= 1) {
        if (fabs(timedV_.Get()) < 0.06 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" Arrived target: %f  %f l:%f w:%f theta:%f\n", tx, ty, l, w, theta);
            return true;
        }
    }
    return false;

}

void Navigation::SendMoveCmd(int mode, ActionType type,
                             double v, double angular) {
    if (monitor_) {
        monitor_->set_speed(mode, type, v, angular);
        if (type == eRotation)
            RWheel_position_ = eR;
        if (type == eVertical)
            RWheel_position_ = eX;
        if (type == eHorizontal)
            RWheel_position_ = eY;
    }
}

void Navigation::SendMoveCmd(int clampLifterAction, int mode, ActionType type,
                             double v[], double angular[]) {
    if (monitor_) {
        monitor_->set_ToAgvCmd(clampLifterAction, mode, type, v, angular);
        if (type == eRotation)
            RWheel_position_ = eR;
        if (type == eVertical)
            RWheel_position_ = eX;
        if (type == eHorizontal)
            RWheel_position_ = eY;
    }
}

/*void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[],
                             int space_id, double distance,double Y1,double Y2) {
    if (monitor_) {
        monitor_->set_ToAgvCmd(mode, type, v, angular, 0, space_id, distance,Y1,Y2);
        if (type == eRotation)
            RWheel_position_ = eR;
        if (type == eVertical)
            RWheel_position_ = eX;
        if (type == eHorizontal)
            RWheel_position_ = eY;
    }
}*/

void Navigation::SendMoveCmd(int clampLifterAction, int mode, ActionType type, double v[], double angular[],
                             int space_id, double distance, double Y1, double Y2) {
    if (monitor_) {
        monitor_->set_ToAgvCmd(clampLifterAction, mode, type, v, angular, 0, space_id, distance, Y1, Y2);
        if (type == eRotation)
            RWheel_position_ = eR;
        if (type == eVertical)
            RWheel_position_ = eX;
        if (type == eHorizontal)
            RWheel_position_ = eY;
    }
}

//void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], double yaw_diff[] ,int space_id, double distance) {
//    if (monitor_) {
//        monitor_->set_ToAgvCmd(mode, type, v, angular, yaw_diff ,0,space_id, distance);
//        if (type == eRotation)
//            RWheel_position_ = eR;
//        if (type == eVertical)
//            RWheel_position_ = eX;
//        if (type == eHorizontal)
//            RWheel_position_ = eY;
//    }
//}

bool Navigation::clamp_close() {
    if (monitor_) {
        printf("Clamp closing...\n");
        monitor_->clamp_close(move_mode_);
        actionType_ = eClampClose;
        while (cancel_ == false) {
            if (timed_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eClosed)
                return true;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_close(move_mode_);
            actionType_ = eClampClose;
        }
        return false;
    }
    return false;
}

bool Navigation::clamp_half_open() {
    if (monitor_) {
        printf("Clamp openning...\n");
        monitor_->clamp_half_open(move_mode_);
        actionType_ = eClampHalfOpen;
        while (cancel_ == false) {
            if (timed_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eHalfOpened)
                return true;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_half_open(move_mode_);
            actionType_ = eClampHalfOpen;
        }
        return false;
    }
    return false;
}

bool Navigation::singleClamp_fullyOpen_until_inspaceBegin() {
    if (move_mode_ == eDouble) {
        return true;
    }
    while (cancel_ == false) {
        if (timedV_.timeout()) {
            return false;
        }
        if (fabs(timedV_.Get()) < 0.02) {

            usleep(1000 * 300);
            continue;
        }
        break;
    }
    return clamp_fully_open();
};

bool Navigation::clamp_fully_open() {
    if (monitor_) {
        if (timed_clamp_.Get() == eFullyOpened)
            return true;
        printf("Clamp fully openning...\n");
        monitor_->clamp_fully_open(move_mode_);
        actionType_ = eClampFullyOpen;
        while (cancel_ == false) {
            if (timed_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eFullyOpened)
                return true;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_fully_open(move_mode_);
            actionType_ = eClampFullyOpen;
        }
        return false;
    }
    return false;
}


bool Navigation::lifter_rise() {
    if (monitor_) {
        printf("Lifter upping...\n");
        actionType_ = eLifterRise;
        if (timed_lifter_.Get() == eRose)
            return true;

        if (timed_lifter_.Get() != eRose) {
            if (timed_clamp_.Get() != eHalfOpened && timed_clamp_.Get() != eClosed) {
                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
                if (!clamp_half_open()) {
                    printf(" clamp half open failed\n");
                    return false;
                }
            }
        }

        while (cancel_ == false) {
            if (timed_lifter_.timeout()) {
                printf("timed lifter is timeout\n");
                return false;
            }
            if (timed_lifter_.timeout()) {
                return false;
            }
            if (timed_lifter_.Get() == eRose)
                return true;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->lifter_rise(move_mode_);
            actionType_ = eLifterRise;
        }
        return false;
    }
    return false;
}

bool Navigation::lifter_down() {
    if (monitor_) {
        printf("Lifter downing...\n");
        if (timed_lifter_.Get() == eDowned)
            return true;
        actionType_ = eLifterDown;
        if (timed_lifter_.Get() != eDowned) {
            if (timed_clamp_.Get() != eHalfOpened && timed_clamp_.Get() != eClosed) {
                printf(" clamp statu !=eHalfOpened or  eClosed, clamp_half_open...\n ");
                if (!clamp_half_open()) {
                    printf(" clamp half open failed\n");
                    return false;
                }
            }
        }
        while (cancel_ == false) {
            if (timed_lifter_.timeout()) {
                printf("timed lifter is timeout\n");
                return false;
            }
            if (timed_lifter_.timeout()) {
                return false;
            }
            if (timed_lifter_.Get() == eDowned)
                return true;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->lifter_down(move_mode_);
            actionType_ = eLifterDown;
        }
        return false;
    }
    return false;
}

bool Navigation::WaitInsideDoorCompleted(int doorID, DoorStatu status) {
    while (cancel_ == false) {
        if (timed_Door_.timeout()) {
            return false;
        }
        if (timed_Door_.Get()[doorID].second == status) {
            return true;
        }
        usleep(1000 * 100);
        printf("waiting door_id(ins):%d,  to:%d...\n", doorID, status);
    }
    if (cancel_ == true) {
        return false;
    }
    return false;
}

bool Navigation::WaitCarrierCompleted(int spaceID, CarrierStatu statu) {
    while (cancel_ == false) {
        int region_id = 0, carrier_no = 0;
        SpaceNo2CarrierNo(spaceID, region_id, carrier_no);
        if (region_id < 0) {
            return true;
        }
        if (timed_Carrier_[region_id].timeout()) {
            printf("spaceID:%d, timed_Carrier_[%d].timeout  carrier_no id:%d\n", spaceID, region_id, carrier_no);
            return false;
        }

        CarrierStatu cur_ = GetCarrierStatusBySpaceID(spaceID);
        if (cur_ == statu) {
            return true;
        }
        usleep(1000 * 100);
        printf("waiting space_id:%d, carrier_id[%d,%d] to:%d, cur:%d...\n", spaceID, region_id, carrier_no, statu,
               cur_);
    }
    if (cancel_ == true) {
        return false;
    }
    return false;
}

bool Navigation::WaitClampLifterStatu(int clampLifterAction) {
    int currentStatu = 0;
    if (move_mode_ == eSingle) {
        while (cancel_ == false) {
            if (timed_clamp_.timeout() || timed_lifter_.timeout()) {
                return false;
            }
            if (timed_clamp_.Get() == eClosed) {
                currentStatu |= eByteCLampClose;
            }
            if (timed_clamp_.Get() == eHalfOpened) {
                currentStatu |= eByteClampHalfOpen;
            }
            if (timed_clamp_.Get() == eFullyOpened) {
                currentStatu |= eByteClampFullyOpen;
            }
            if (timed_lifter_.Get() == eRose) {
                currentStatu |= eByteLifterUp;
            }
            if (timed_lifter_.Get() == eDowned) {
                currentStatu |= eByteLifterDown;
            }
            printf(" current statu:%d   action status:%d  last:%d\n",
                   currentStatu, clampLifterAction, currentStatu & clampLifterAction);
            if ((currentStatu & clampLifterAction) == clampLifterAction)
                return true;

            double down_v[3] = {0, 0, 0};//下发线速度0
            double down_w[3] = {0, 0, 0};//下发角速度0
            SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w);
            usleep(1000 * 100);
        }

    } else if (move_mode_ == eDouble) {

        while (cancel_ == false) {
            if (timed_clamp_.timeout() || timed_lifter_.timeout() ||
                timed_other_clamp_.timeout() || timed_other_lifter_.timeout()) {
                return false;
            }
            if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get() == eClosed) {
                currentStatu |= eByteCLampClose;
            }
            if (timed_clamp_.Get() == eHalfOpened && timed_other_clamp_.Get() == eHalfOpened) {
                currentStatu |= eByteClampHalfOpen;
            }
            if (timed_clamp_.Get() == eFullyOpened && timed_other_clamp_.Get() == eFullyOpened) {
                currentStatu |= eByteClampFullyOpen;
            }
            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
                currentStatu |= eByteLifterUp;
            }
            if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned) {
                currentStatu |= eByteLifterDown;
            }
            printf(" current statu:%d   action status:%d  last:%d\n",
                   currentStatu, clampLifterAction, currentStatu & clampLifterAction);
            if ((currentStatu & clampLifterAction) == clampLifterAction)
                return true;

            double down_v[3] = {0, 0, 0};//下发线速度0
            double down_w[3] = {0, 0, 0};//下发角速度0
            SendMoveCmd(clampLifterAction, move_mode_, actionType_, down_v, down_w);
            usleep(1000 * 100);
        }

    }
    if (cancel_ == true) {
        return false;
    }
    return false;
}

Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v,
                                              bool autoDirect, int clampLifterAction) {
    if (IsArrived(node)) {
        printf(" current already in target completed !!!\n");
        return eMpcSuccess;
    }
    if (inited_ == false) {
        printf(" navigation has not inited\n");
        return eMpcFailed;
    }
    if (PoseTimeout()) {
        printf(" navigation Error:Pose is timeout \n");
        return eMpcFailed;
    }
    Pose2d current = timedPose_.Get();
    Pose2d target(node.x(), node.y(), 0);
    //生成轨迹
    Trajectory traj = Trajectory::create_line_trajectory(current, target, 0.1);
    if (traj.size() == 0)
        return eMpcSuccess;
    //判断 巡线方向
    bool directY = false;
    if (autoDirect) {
        Pose2d target_relative = Pose2d::relativePose(target, timedPose_.Get());
        if (fabs(target_relative.y()) > fabs(target_relative.x())) {  //目标轨迹点在当前点Y轴上
            std::cout << " MPC Y axis  target_relative:" << target_relative << std::endl;
            directY = true;
        } else {
            std::cout << " MPC X axis  target_relative:" << target_relative << std::endl;
        }
        if (!PossibleToTarget(node, directY)) {
            directY = !directY;
        }
    }

    printf(" exec along autoDirect:%d\n", autoDirect);

    //文件log
    std::ofstream ofs;  //创建流对象
    std::string log_file_dir = "../data/";
    time_t t = time(0);
    char tmp[32] = {NULL};
    strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S.txt", localtime(&t));
    std::string log_file_name = tmp;
    ofs.open(log_file_dir + log_file_name, std::ios::app);  //打开的地址和方式
    auto beg_time = std::chrono::steady_clock::now();

    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
        if (pause_ == true) {
            //发送暂停消息
            continue;
        }
        if (PoseTimeout()) {
            printf(" navigation Error:Pose is timeout \n");
            ofs.close();//关闭文件
            return eMpcFailed;
        }
        if (timedV_.timeout() || timedA_.timeout()) {
            printf(" navigation Error:v/a is timeout | __LINE__:%d \n", __LINE__);
            ofs.close();//关闭文件
            return eMpcFailed;
        }

//        //出入库实时判断门控或者载车板
//        float door_no_1_y = -5;
//        NavMessage::NewAction current_action = GetCurrentAction();
//        if (current_action.type() == 1 || current_action.type() == 2) {
//            //去出入口，判断内门是否打开
//            if (TargetIsExport(current_action.spacenode()) || TargetIsEntrance(current_action.spacenode())) {
//                int port_id = GetPortIDBySpace(current_action.spacenode());
//                if (timed_Door_.timeout() || timed_Door_.Get()[port_id].second != eDoorOpened) {
//                    printf(" Inside door is not opened.......| line: %d\n", __LINE__);
//                    return eMpcFailed;
//                }
//            } else {
//                //去车位点
//                if (IsUperSpace(current_action.spacenode())) {
//                    if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierDown) {
//                        printf(" Carrier is not downed.......| line: %d\n", __LINE__);
//                        return eMpcFailed;
//                    }
//                } else {
//                    int region_id = 0, carrier_no = 0;
//                    SpaceNo2CarrierNo(GetSpaceId(current_action.spacenode()), region_id, carrier_no);
//                    if (region_id < 0) {// 子母车位，且不在载车板下方的车位
//
//                    } else {
//                        if (GetCarrierStatusBySpaceID(GetSpaceId(current_action.spacenode())) != eCarrierUp) {
//                            printf(" Carrier is not up.......| line: %d\n", __LINE__);
//                            return eMpcFailed;
//                        }
//                    }
//                }
//            }
//        }

        //判断是否到达终点
        if (IsArrived(node)) {
            if (Stop()) {
                printf(" exec along completed !!!\n");
                ofs.close();//关闭文件
                bool waited = WaitClampLifterStatu(clampLifterAction);
                if (waited) return eMpcSuccess;
                else return eMpcFailed;
            }
        }

        float v_before_compute = timedV_.Get();
        float a_before_compute = timedA_.Get();
        //一次变速
        std::vector<double> out;
//        std::vector<double> diff_yaw; //两车分别与整车的相对角度
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = mpc_once(traj, limit_v, out, directY);
        }, "mpc_once");
        if (ret == false) {
            SlowlyStop();
            ofs.close();//关闭文件
            return eMpcFailed;
        }
        const int down_count = 3;//下发前多少步
        double down_v[down_count] = {out[0], out[2], out[4]};//下发线速度
        double down_w[down_count] = {out[1], out[3], out[5]};//下发角速度
        /*
         * AGV 在终点附近低速巡航时，设置最小速度0.05
         */
        Pose2d target_in_agv = Pose2d::relativePose(target, timedPose_.Get());
        if (directY) {
            target_in_agv = target_in_agv.rotate(M_PI / 2.0);
        }

        for (int i = 0; i < down_count; ++i) {
            if (down_v[i] >= 0 && down_v[i] < limit_v.min)
                down_v[i] = limit_v.min;
            if (down_v[i] < 0 && down_v[i] > -limit_v.min)
                down_v[i] = -limit_v.min;
        }

        //放大角速度
//        out[1] *= 10;

        //入库，行进方向上最后一米不纠偏
        if (newUnfinished_cations_.front().type() == 1 && GetSpaceId(node) > 0) {//入库
            if (fabs(target_in_agv.x()) < 0.8) {
                printf("target_in_agv: %f", target_in_agv.x());
                for (double &i: down_w) {
                    i = 0.0;
                }
            }
        } else if (newUnfinished_cations_.front().type() == 2) {//出库,行进方向上开始一米不纠偏
            Pose2d agv_in_space =
                    Pose2d::relativePose(
                            Pose2d(newUnfinished_cations_.front().spacenode().x(),
                                   newUnfinished_cations_.front().spacenode().y(), 0),
                            timedPose_.Get());
            if (fabs(agv_in_space.x()) < 1.4) {
                printf("agv_in_space: %f", agv_in_space.x());
                for (double &i: down_w) {
                    i = 0.0;
                }
            }
        }

        //下发速度
        //printf("  nav  input :%f   out:%f\n",timedV_.Get(),out[0]);
        //添加车位号,距目标点距离信息
        double distance = 0;
        int space_id = -1;
        if (GetCurrentAction().type() == 1 || GetCurrentAction().type() == 2) {
            space_id = GetSpaceId(GetCurrentAction().spacenode());
        }
        if (space_id > 0) {
            distance = Pose2d::distance(target_in_agv, Pose2d(0, 0, 0));
        }
        double Y1 = 0.0, Y2 = 0.0;
        if (move_mode_ == eDouble) {
            double dy = -timeYawDiff1_.Get();
            Y1 = 0.2 * (1.0 / (1 + exp(-16. * dy)) - 0.5) * 1.0;
            Y2 = timeYawDiff2_.Get();
            if (directY == false) {
                Y1 = 0;
            }
        }
        if (directY == false)
            SendMoveCmd(clampLifterAction, move_mode_, eVertical, down_v, down_w, space_id, distance, Y1, Y2);
        else
            SendMoveCmd(clampLifterAction, move_mode_, eHorizontal, down_v, down_w, space_id, distance, Y1, Y2);
        actionType_ = directY ? eHorizontal : eVertical;

        //日志打印
        std::string log_inf_line = std::to_string(move_mode_);
        log_inf_line += " ";

        log_inf_line += std::to_string(parameter_.x_mpc_parameter().shortest_radius()) + " " +
                        std::to_string(parameter_.x_mpc_parameter().acc_angular());
        log_inf_line += " ";

        Pose2d cur_pose = timedPose_.Get();//自身x,y,theta
        log_inf_line += std::to_string(cur_pose.x()) + " " + std::to_string(cur_pose.y()) + " " +
                        std::to_string(cur_pose.theta());
        log_inf_line += " ";

        Pose2d cur_Bro_pose = timedBrotherPose_.Get();//另一节x,y,theta
        log_inf_line += std::to_string(cur_Bro_pose.x()) + " " + std::to_string(cur_Bro_pose.y()) + " " +
                        std::to_string(cur_Bro_pose.theta());
        log_inf_line += " ";

        log_inf_line += std::to_string(v_before_compute) + " " + std::to_string(a_before_compute);//计算时反馈速度
        log_inf_line += " ";
        log_inf_line += std::to_string(timedV_.Get()) + " " + std::to_string(timedA_.Get());//下发时反馈速度
        log_inf_line += " ";
        log_inf_line += std::to_string(out[0]) + " " + std::to_string(out[1]);//下发速度
        log_inf_line += " ";
        log_inf_line += std::to_string(out[2]) + " " + std::to_string(out[3]);//下发速度
        log_inf_line += " ";
        log_inf_line += std::to_string(out[4]) + " " + std::to_string(out[5]);//下发速度
        log_inf_line += " ";
        log_inf_line += std::to_string(Y1) + " " + std::to_string(Y2);//下发速度
        log_inf_line += " ";

        auto cur_time = std::chrono::steady_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(cur_time - beg_time);
        double time =
                double(duration.count()) * std::chrono::microseconds::period::num /
                std::chrono::microseconds::period::den;
        log_inf_line += std::to_string(time);

        ofs << log_inf_line << std::endl;  //一行日志的内容
        /*
         * 判断车辆是否在终点附近徘徊，无法到达终点
         */
        if (PossibleToTarget(node, directY) == false) {
            printf(" --------------MPC  imposible to target -------------------------------------------\n");
            if (fabs(timedV_.Get()) > 0.1 || fabs(timedA_.Get()) > 10 * M_PI / 180.0)
                SlowlyStop();
            else
                Stop();
            ofs.close();//关闭文件
            return eImpossibleToTarget;
        }
    }

    ofs.close();//关闭文件
    return eMpcFailed;
}

bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY) {
    if (timedPose_.timeout()) {
        return false;
    }
    Pose2d current = timedPose_.Get();
//    if (GetSpaceId(targetNode) > 0) {
//        printf("车位点强制可到达\n");
//        //车位点强制可到达
//        return true;
//    }
    double tx = targetNode.x();
    double ty = targetNode.y();
    double l = fabs(targetNode.l());
    double w = fabs(targetNode.w());
    double theta = -targetNode.theta();

//    double W = 2.45;
//    double L = 1.3;
//    double minYaw = 3 * M_PI / 180.0;
//    if (move_mode_ == eDouble) {
//        L = 1.3 + 2.7;
//
//    }
    double W = 2.5;
    double L = 1.565;
    double minYaw = 5 * M_PI / 180.0;
    if (move_mode_ == eDouble) {
        L = 1.565 + 2.78;
    }
    if (directY) {
        current = current.rotate(current.x(), current.y(), M_PI / 2);
        double t = W;
        W = L;
        L = t;
    }
    double minR = W / 2 + L / tan(minYaw);
    //printf("l:%f,w:%f\n",l,w);
    std::vector<Pose2d> poses = Pose2d::generate_rectangle_vertexs(Pose2d(tx, ty, 0), l * 2, w * 2);

    bool nagY = false;
    bool posiY = false;
    for (int i = 0; i < poses.size(); ++i) {
        Pose2d rpos = poses[i].rotate(tx, ty, theta);
        Pose2d relative = Pose2d::relativePose(rpos, current);
        double ax = fabs(relative.x());
        double ay = fabs(relative.y());
        if (relative.y() > 0) posiY = true;
        if (relative.y() < 0) nagY = true;
        if (ax * ax + pow(ay - minR, 2) > minR * minR) {
            /*printf(" possible to target:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n",
                   targetNode.x(),targetNode.y(),targetNode.l(),targetNode.w(),targetNode.theta(),
                   minR,ax,ay);*/
            return true;
        } else if (nagY && posiY) {
//            std::cout << "nagY && posiY" << std::endl;
            return true;
        }
//        printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f  minR:%f ax:%f,ay:%f\n", directY,
//               relative.x(), relative.y(), targetNode.l(), targetNode.w(), targetNode.theta(),
//               minR, ax, ay);
    }


    printf(" impossible to target:%f %f l:%f,w:%f theta:%f, ",
           targetNode.x(), targetNode.y(), targetNode.l(), targetNode.w(), targetNode.theta());
    printf(" current:%f, %f, %f\n",
           current.x(), current.y(), current.theta());

    return false;
}

void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
    if (inited_ == false) {
        response.set_ret(-1);
        response.set_info("navigation has not inited");
        printf(" navigation has not inited\n");
        return;
    }
    if (newUnfinished_cations_.empty() == false)  //正在运行中，上一次指令未完成
    {
        response.set_ret(-2);
        response.set_info("navigation is running pls cancel before");
        printf(" navigation is running pls cancel before\n");
        return;
    }
    //add 先检查当前点与起点的距离
    global_navCmd_ = cmd;

    for (int i = 0; i < cmd.newactions_size(); ++i)
        newUnfinished_cations_.push(cmd.newactions(i));

    pause_ = false;
    cancel_ = false;
    running_ = true;
    actionType_ = eReady;
    printf("Navigation beg...\n");
    bool ret = false;
    while (newUnfinished_cations_.empty() == false && cancel_ == false) {
        std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
        NavMessage::NewAction act = newUnfinished_cations_.front();
        NavMessage::NewAction last_act = newUnfinished_cations_.back();
        if (act.type() == 1) {  //入库
            printf("入库\n");
            space_id_ = act.spacenode().id();
            obs_w_ = 1.1;
            obs_h_ = 1.87;
            ret = execute_InOut_space(act);
            if (!ret) {
                printf(" In space failed\n");
                isInSpace_ = false;
                break;
            }
        } else if (act.type() == 2) {  //出库
            printf("出库\n");
            obs_w_ = 1.25;
            obs_h_ = 1.87;
            ret = execute_InOut_space(act);
            if (!ret) {
                printf(" out space failed\n");
                break;
            } else {
                isInSpace_ = false;
            }
        } else if (act.type() == 3 || act.type() == 4) { //马路导航
            printf("马路导航\n");
            obs_w_ = 1.1;
            obs_h_ = 1.87;
            ret = execute_nodes(act);
            if (!ret) {
                printf(" street nav failed\n");
                break;
            }
        } else if (act.type() == 5) {
            printf(" 汽车模型导航....\n");

        } else if (act.type() == 6) {//夹持
            ret = clamp_close();
            if (ret == false) {
                printf("夹持failed ...\n");
                break;
            }
        } else if (act.type() == 7) {
            if (this->clamp_fully_open() == false) {
                printf("打开夹持 failed...\n");
                break;
            }
        } else if (act.type() == 8) { //切换模式
            SwitchMode(act.changedmode(), act.wheelbase());
            ret = true;
        } else if (act.type() == 9) { //提升机构提升
            ret = lifter_rise();
            if (ret == false) {
                printf("提升failed ...\n");
                break;
            }
        } else if (act.type() == 10) { //提升机构下降
            ret = lifter_down();
            if (ret == false) {
                printf("下降failed ...\n");
                break;
            }
        } else {
            printf(" action type invalid not handled !!\n");
            break;
        }
        newUnfinished_cations_.pop();
    }
    actionType_ = eReady;
    Stop();
    if (ret == false) {
        response.set_ret(-4);
        response.set_info("navigation ret false");
        printf(" navigation ret false\n");
        while (newUnfinished_cations_.empty() == false)
            newUnfinished_cations_.pop();
    } else if (cancel_ == true) {
        response.set_ret(-3);
        response.set_info("navigation canceled");
        printf(" navigation canceled\n");
        while (newUnfinished_cations_.empty() == false)
            newUnfinished_cations_.pop();
    } else {
        if (newUnfinished_cations_.empty()) {
            response.set_ret(0);
            response.set_info("navigation completed!!!");
            printf("navigation completed!!!\n");
        } else {
            response.set_ret(-4);
            response.set_info("navigation Failed!!!!");
            printf(" navigation Failed\n");
            while (newUnfinished_cations_.empty() == false)
                newUnfinished_cations_.pop();
        }
    }
    running_ = false;
}

bool Navigation::SwitchMode(int mode, float wheelBase) {
    wheelBase_ = wheelBase;
    printf("Child AGV can not Switch Mode\n");
    return true;
}

float Navigation::compute_cuv(const Pose2d &target) {
    float x = fabs(target.x());
    float y = fabs(target.y());
    float cuv = atan(y / (x + 1e-8));
    //printf(" ---------------------- cuv：%f\n",cuv);
    return cuv;
}

void Navigation::ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::NavResponse &response) {
    return;
//    pause_ = false;
//    cancel_ = false;
//    running_ = true;
//    actionType_ = eReady;
//    printf("ManualOperation: %d | start...\n", cmd.operation_type());
//
//    float symbol = cmd.velocity() < 0 ? -1 : 1; // 判断为正方向or负方向
//    double velocity;
//    std::this_thread::sleep_for(std::chrono::milliseconds(100));
//    switch (cmd.operation_type()) {
//        case 1: // 旋转
//            velocity = 2.0 * M_PI / 180.0 * symbol;
//            SendMoveCmd(move_mode_, eRotation, 0, velocity);
//            actionType_ = eRotation;
//            printf(" ManualOperation: %d | input angular_v: %f, down: %f\n",
//                   cmd.operation_type(), timedA_.Get(), velocity);
//            break;
//        case 2: // X平移
//            velocity = 1.0 * symbol;
//            SendMoveCmd(move_mode_, eVertical, velocity, 0);
//            actionType_ = eVertical;
//            printf(" ManualOperation: %d | input line_v   : %f, down: %f\n",
//                   cmd.operation_type(), timedV_.Get(), velocity);
//            break;
//        case 3: // Y平移
//            velocity = 1.0 * symbol;
//            SendMoveCmd(move_mode_, eHorizontal, velocity, 0);
//            actionType_ = eHorizontal;
//            printf(" ManualOperation: %d | input line_v: %f, down: %f\n",
//                   cmd.operation_type(), timedV_.Get(), velocity);
//            break;
//        default:
//            break;
//    }
//
//    actionType_ = eReady;
//    response.set_ret(-3);
//    response.set_info("ManualOperation: %d, canceled!!!", cmd.operation_type());
//    printf("ManualOperation: %d | canceled!!!\n", cmd.operation_type());
//    running_ = false;
}

bool Navigation::IsMasterAGV() {
    return is_master_AGV;
}

void Navigation::ResetDoor(int32_t status) {
//    printf("---------------receive status:%d\n",status);
    DoorStatusMap doorStatusMap;
    anasisDoorStatu(status, doorStatusMap);

//    for (auto it = doorStatusMap.begin() ; it != doorStatusMap.end() ; ++it) {
//        printf("doorID:%d, out:%d, ins:%d\n",it->first, it->second.first, it->second.second);
//    }

    timed_Door_.reset(doorStatusMap, 3);
}

void Navigation::anasisDoorStatu(int32_t i_door_status, DoorStatusMap &m_door_status) {
//    int bytesize = sizeof(int32_t)*8;
    m_door_status.clear();
    for (int i = 1; i >= 0; --i) {
        int statu = i_door_status >> (i * 16);
        int door_id = (statu & 0xFF00) >> 8;
        int outside_door_status = statu & 0x00FF;
        int inside_door_status = outside_door_status >> 2;

        if ((outside_door_status & eDoorOpened) == eDoorOpened) {
            outside_door_status = eDoorOpened;
        } else if ((outside_door_status & eDoorClosed) == eDoorClosed) {
            outside_door_status = eDoorClosed;
        } else {
            outside_door_status = eDoorInvalid;
        }

        if ((inside_door_status & eDoorOpened) == eDoorOpened) {
            inside_door_status = eDoorOpened;
        } else if ((inside_door_status & eDoorClosed) == eDoorClosed) {
            inside_door_status = eDoorClosed;
        } else {
            inside_door_status = eDoorInvalid;
        }

        m_door_status[door_id] = DoorStatusInOnePort((DoorStatu) outside_door_status, (DoorStatu) inside_door_status);
    }
}

#define SWAP_BYTES 0

void Navigation::anasisCarierStatu(int &regionID, CarrierStatuRegionMap &status, int32_t bytes) {
    int32_t reverseBytes = bytes;
    if (SWAP_BYTES) {
        reverseBytes = (bytes >> 24) | ((bytes & 0xFF0000) >> 8) | ((bytes & 0xFF00) << 8) | ((bytes & 0xFF) << 24);
    }
    int bytesize = sizeof(int32_t) * 8;
    regionID = (bytes >> ((bytesize - 1) * 8));
    for (int i = 0; i < (bytesize - 8) / 2; ++i) {
        int statu = (reverseBytes >> (i * 2) & 0x03);

        if ((statu & eCarrierUp) == eCarrierUp) {
            status[i + 1] = eCarrierUp;
        } else if ((statu & eCarrierDown) == eCarrierDown) {
            status[i + 1] = eCarrierDown;
        } else {
            status[i + 1] = eCarrierInvalid;
        }
    }
}

void Navigation::ResetCarrier(std::vector<int32_t> status) {
    for (int32_t statu: status) {
        int regionID;
        CarrierStatuRegionMap region;
        anasisCarierStatu(regionID, region, statu);

        timed_Carrier_[regionID].reset(region, 5);
    }

    /*printf("---------------------------------------\n");
    for(std::map<int,TimedLockData<CarrierStatuRegionMap>>::iterator it=timed_Carrier_.begin();
        it!=timed_Carrier_.end();it++){
        int regionID=it->first;
        if(it->second.timeout())
            continue;
        printf(">>>>>>> Region ID :%d\n",regionID);
        std::map<int,CarrierStatu> region=it->second.Get();
        for(std::map<int,CarrierStatu>::iterator it1=region.begin();
            it1!=region.end();++it1){
            int id=it1->first;
            CarrierStatu sta=it1->second;
            printf("\t \tcarrier id :%d  statu:%d\n",id,sta);
        }
    }*/

}

bool Navigation::Stringsplit(const std::string &str, const std::string splits, std::vector<std::string> &res) {
    res.clear();
    if (str.empty()) {
        return false;
    }
    std::string strs = str + splits;
    size_t pos = strs.find(splits);
    int step = splits.size();

    while (pos != std::string::npos) {
        std::string temp = strs.substr(0, pos);
        if (temp.size() > 0)
            res.push_back(temp);
        strs = strs.substr(pos + step, strs.size());
        pos = strs.find(splits);
    }
//    for (int i = 0; i < res.size(); ++i) {
//        printf("%d: %s[%zu]\n",i, res[i].c_str(),res[i].size());
//    }
    return true;
}

bool Navigation::LoadSpaceNo2CarrierNo() {
    std::string file_name = "../config/SpaceNO2CarrarierNO.txt";
    std::ifstream in(file_name);
    std::string line;
    std::vector<std::string> data_in_one_line;
    if (in) {
        int space_id, region_id, carrier_no;
        while (std::getline(in, line)) {
            if (!Stringsplit(line, " ", data_in_one_line) || line.find("//") != std::string::npos)
                continue;
            if (data_in_one_line.size() == 3) {
                space_id = std::stoi(data_in_one_line[0]);
                region_id = std::stoi(data_in_one_line[1]);
                carrier_no = std::stoi(data_in_one_line[2]);
                spaceNo_2_region_carrier_excel_[space_id] = Region_Carry(region_id, carrier_no);
            }
        }
        //打印
        for (auto &i: spaceNo_2_region_carrier_excel_) {
            printf("space_id: %d, region_id: %d, carrier_no: %d\n", i.first, i.second.first, i.second.second);
        }
        return true;
    } else {
        printf("no SpaceNO2CarrarierNO.txt in config!\n");
    }
    return false;
}

bool Navigation::SpaceNo2CarrierNo(int space_no, int &region_id, int &carrier_no) {
    Region_Carry region_carry = spaceNo_2_region_carrier_excel_[space_no];
    region_id = region_carry.first;
    carrier_no = region_carry.second;
    return true;
}

Navigation::CarrierStatu Navigation::GetCarrierStatusBySpaceID(int space_id) {
    int region_id;
    int carrier_no;
    if (SpaceNo2CarrierNo(space_id, region_id, carrier_no) == false) {
        printf("no matched carrier!\n");
        return eCarrierInvalid;
    }
    printf("space_id:%d, region_id:%d, carrier_no:%d\n", space_id, region_id, carrier_no);
    if (timed_Carrier_[region_id].timeout()) {
        printf("carrier status is timeout!\n");
        return eCarrierInvalid;
    } else {
        return timed_Carrier_[region_id].Get()[carrier_no];
    }
    return eCarrierInvalid;
}

int Navigation::GetPortIDBySpace(NavMessage::PathNode node) {
    int port_id = GetSpaceId(node);
    if (port_id == 1101) {
        return 1;
    } else if (port_id == 1102) {
        return 2;
    }

    return -1;
}

int Navigation::GetRemainActionsCount() {
    return newUnfinished_cations_.size();
}

NavMessage::NewAction Navigation::GetCurrentAction() {
    return newUnfinished_cations_.front();
}