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) {
        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_->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 (msg.toProtoMessage(speed)) {
        ResetStatu(speed.v(), speed.w());
        ResetClamp((ClampStatu) speed.clamp());
    }
}

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());
            //printf(" timed_clamp_:%d\n ",timed_clamp_.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::ResetPose(const Pose2d &pose) {
    timedPose_.reset(pose, 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_, Monitor_emqx::ActionType(actionType_), new_v, 0);
            } else {
                double new_v = v + acc * dt;
                new_v = new_v < 0 ? new_v : 0;
                SendMoveCmd(move_mode_, Monitor_emqx::ActionType(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() {
    isInSpace_ = false; //是否在车位或者正在进入车位
    RWheel_position_ = eUnknow;
    move_mode_ = Monitor_emqx::eSingle;
    monitor_ = nullptr;
    terminator_ = nullptr;

    timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5);
}

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_, Monitor_emqx::eRotate, 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;
}

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(100));
        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 & MpcRotateDirection::eForward){
                yaw_diffs.push_back(target_yaw_diff);
            }
            if (directions & MpcRotateDirection::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 & MpcRotateDirection::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 & MpcRotateDirection::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()));
            }

            std::cout << " yaw_diffs[i] = ";
            for (auto i = 0; i < yaw_diffs.size(); ++i) {
                std::cout << yaw_diffs[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) > 2 * M_PI / 180.0) {
            SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 0, out[0]);
            actionType_ = eRotation;
            printf(" MPC_rotate | input angular_v:%f, down:%f, diff:%f autoDirect:%d\n",
                   timedA_.Get(), out[0], target_yaw_diff, directions);
        } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
            printf(" MPC_rotate refer target completed,cuv:%f\n", target_yaw_diff);
            return eMpcSuccess;
        }
        continue;
    }
    return eMpcFailed;
}


bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w,
                              bool anyDirect, bool enable_rotate) {
    if (IsArrived(node)) {
        return true;
    }
    bool already_in_mpc = false;
    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;
        }
        //巡线无法到达目标点
        if (enableTotarget == false) {
            if (already_in_mpc && enable_rotate == false) {
                printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
                return false;
            }
            Pose2d target(node.x(), node.y(), 0);
//        if (RotateReferToTarget(target, limit_w, anyDirect) == false) {
            int direction = MpcRotateDirection::eForward | MpcRotateDirection::eBackward |
                            MpcRotateDirection::eLeft | MpcRotateDirection::eRight;
            if (RotateReferToTarget(node, limit_w, direction) != eMpcSuccess) {
                std::cout << " Rotate refer to target failed,target: " << target <<
                          " direction: " << direction << " line_: " << __LINE__ << std::endl;
                return false;
            }
            continue;
        }
        already_in_mpc = true;
        MpcResult ret = MpcToTarget(node, limit_v, anyDirect);
        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 {
            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 adjust_angular = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
        stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};

        for (int i = 0; i < action.pathnodes_size(); ++i) {
            NavMessage::PathNode node = action.pathnodes(i);
            if (i + 1 < action.pathnodes_size()) {
                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()));
                node.set_l(0.3);
                node.set_w(0.1);

            } else {
                node.set_theta(0);
                node.set_w(0.2);
                node.set_l(0.2);
            }
            if (MoveToTarget(node, mpc_velocity, adjust_angular, anyDirect, true) == false)
                return false;
            else
                isInSpace_ = false;
        }
        return true;
    }
    printf("execute PathNode failed ,action type is not 3/4  :%d\n", action.type());
    return false;
}

bool Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
    if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
        printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
        return false;
    }
    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 rotated = timedPose_.Get();

    //前车先到,当前车进入2点,保持与前车一致的朝向
    if (brother.in_space() && brother.space_id() == space.id()) {
        rotated.mutable_theta() = brother.odom().theta();
    } else { //当前车先到（后车）,倒车入库
        rotated.mutable_theta() = space.theta();
        rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
    }
    double x = space.x() - wheelbase / 2 * cos(rotated.theta());
    double y = space.y() - wheelbase / 2 * sin(rotated.theta());
    target.set_x(x);
    target.set_y(y);
    target.set_theta(rotated.theta());
    target.set_l(0.05);
    target.set_w(0.03);
    target.set_id(space.id());

    //整车协调的时候，保持前后与车位一致即可
    if (move_mode_ == Monitor_emqx::eMain) {
        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(100));
        Pose2d current = timedPose_.Get();
        double yawDiff = (rotated - current).theta();
        if (fabs(yawDiff) > 1 * M_PI / 180.0) {
            double theta = limit_gause(yawDiff, 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_, Monitor_emqx::eRotate, 0, limit_angular);
            actionType_ = eRotation;
            printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
                   timedA_.Get(), angular, limit_angular, yawDiff);

            continue;
        } else {
            if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
                printf(" RotateBeforeEnterSpace refer target completed\n");
                printf("---------------- update new target :%f %f %f \n", target.x(), target.y(), target.theta());
                return true;
            }
        }
    }
    return false;

}

bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
    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.2);
        action.mutable_streetnode()->set_w(0.05);
        action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
        if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true) == false) {
            printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
            return false;
        }
        //移动到库位点， 禁止任意方向// 不允许巡线中停下旋转（当无法到达目标点时，返回false）
        printf("inout ----------------------------------------\n");
        //计算车位朝向
        action.mutable_spacenode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
        //计算当前车是进入车位1点还是2点
        NavMessage::PathNode new_atrget;
        if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_atrget) == false) {
            return false;
        }
        if (MoveToTarget(new_atrget, limit_v, limit_w, true, false) == false) {
            printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
            return false;
        }
    } else if (action.type() == 2) {
        Pose2d space(action.spacenode().x(), action.spacenode().y(), 0);
        Pose2d diff = Pose2d::abs(Pose2d::relativePose(space, timedPose_.Get()));
        if (diff.y() < 0.05) {
            //出库，移动到马路点，允许自由方向，不允许中途旋转
            Pose2d vec(action.streetnode().x() - action.spacenode().x(),
                       action.streetnode().y() - action.spacenode().y(), 0);
            action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
            action.mutable_streetnode()->set_l(0.2);
            action.mutable_streetnode()->set_w(0.1);
            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;
        }

    }
    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 = 2;//0.85;
    double obs_h = 3;//1.45;

    MpcError ret = failed;
    Pose2d brother = timedBrotherPose_.Get();
    Pose2d relative2brother = Pose2d::relativePose(brother, pose);
//    std::cout<<" relative2brother: "<<relative2brother<<std::endl;
    RotateMPC MPC(relative2brother, 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);
    }
    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();

    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 = 0.85;
    double obs_h = 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 = 1.45;
        obs_h = 0.85;
    }

    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_ == Monitor_emqx::ActionMode::eMain)
        relative = Pose2d(-1e8, -1e8, 0);

    MpcError ret = failed;
    LoadedMPC MPC(relative, obs_w, obs_h, limit_v.min, limit_v.max);
    LoadedMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
                                          mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()};
    //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);

    //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);
    }
    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 (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(Monitor_emqx::ActionMode mode, Monitor_emqx::ActionType type,
                             double v, double angular) {
    if (monitor_) {
        monitor_->set_speed(mode, type, v, angular);
        if (mode == ActionType::eRotation)
            RWheel_position_ = eR;
        if (mode == ActionType::eVertical)
            RWheel_position_ = eX;
        if (mode == ActionType::eHorizon)
            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_open() {
    if (monitor_) {
        printf("Clamp openning...\n");
        monitor_->clamp_open(move_mode_);
        actionType_ = eClampOpen;
        while (cancel_ == false) {
            if (timed_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eOpened)
                return true;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_open(move_mode_);
            actionType_ = eClampOpen;
        }
        return false;
    }
    return false;
}


Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) {
    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);
    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;
        }
    }
    printf(" exec along autoDirect:%d\n", autoDirect);
    while (cancel_ == false) {
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        if (pause_ == true) {
            //发送暂停消息
            continue;
        }
        if (PoseTimeout()) {
            printf(" navigation Error:Pose is timeout \n");
            return eMpcFailed;
        }
        if (timedV_.timeout() || timedA_.timeout()) {
            printf(" navigation Error:v/a is timeout | __LINE__:%d \n", __LINE__);
            return eMpcFailed;
        }
        //判断是否到达终点
        if (IsArrived(node)) {
            if (Stop()) {
                printf(" exec along completed !!!\n");
                return eMpcSuccess;
            }
        }

        //一次变速
        std::vector<double> out;
        bool ret;
        TimerRecord::Execute([&, this]() {
            ret = mpc_once(traj, limit_v, out, directY);
        }, "mpc_once");
        if (ret == false) {
            Stop();
            return eMpcFailed;
        }
        /*
         * 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);
        }
        if (Pose2d::abs(target_in_agv) < Pose2d(0.2, 2, M_PI * 2)) {
            if (out[0] >= 0 && out[0] < limit_v.min)
                out[0] = limit_v.min;
            if (out[0] < 0 && out[0] > -limit_v.min)
                out[0] = -limit_v.min;
        }
        //下发速度
        if (directY == false)
            SendMoveCmd(move_mode_, Monitor_emqx::eVertical, out[0], out[1]);
        else
            SendMoveCmd(move_mode_, Monitor_emqx::eHorizontal, out[0], out[1]);
        actionType_ = directY ? eHorizon : eVertical;
        /*
         * 判断车辆是否在终点附近徘徊，无法到达终点
         */
        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();
            return eImpossibleToTarget;
        }
    }

    return eMpcFailed;
}

bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY) {
    if (timedPose_.timeout()) {
        return false;
    }
    Pose2d current = timedPose_.Get();

    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 = 5 * M_PI / 180.0;
    if (move_mode_ == Monitor_emqx::eMain) {
        L = 1.3 + 2.7;

    }
    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) {
            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\n",
           targetNode.x(), targetNode.y(), targetNode.l(), targetNode.w(), targetNode.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");

    while (newUnfinished_cations_.empty() == false && cancel_ == false) {
        std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
        NavMessage::NewAction act = newUnfinished_cations_.front();
        if (act.type() == 1) {  //入库
            space_id_ = act.spacenode().id();
            isInSpace_ = true;
            if (!execute_InOut_space(act)) {
                printf(" In space failed\n");
                break;
            }
        } else if (act.type() == 2) {  //出库
            if (!execute_InOut_space(act)) {
                printf(" out space failed\n");
                break;
            } else {
                isInSpace_ = false;
            }
        } else if (act.type() == 3 || act.type() == 4) { //马路导航

            if (!execute_nodes(act))
                break;
        } else if (act.type() == 5) {
            printf(" 汽车模型导航....\n");

        } else if (act.type() == 6) {//夹持
            if (this->clamp_close() == false) {
                printf("夹持failed ...\n");
                break;
            }
        } else if (act.type() == 7) {
            if (this->clamp_open() == false) {
                printf("打开夹持 failed...\n");
                break;
            }
        } else if (act.type() == 8) { //切换模式
            SwitchMode(Monitor_emqx::ActionMode(act.changedmode()), act.wheelbase());
        } else {
            printf(" action type invalid not handled !!\n");
            break;
        }
        newUnfinished_cations_.pop();
    }
    actionType_ = eReady;
    Stop();
    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;
}

void Navigation::SwitchMode(Monitor_emqx::ActionMode mode, float wheelBase) {
    printf("Child AGV can not Switch Mode\n");
}

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;
}