AGV/MPC/navigation_main.cpp

//
// Created by zx on 23-5-8.
//

#include "navigation_main.h"
#include "TimerRecord.h"


NavigationMain::NavigationMain() {
    move_mode_ = eSingle;
    wheelBase_ = 0;
}

NavigationMain::~NavigationMain() {

}

void NavigationMain::ResetPose(const Pose2d &pose) {
    if (move_mode_ == eDouble) {
        if (timedBrotherPose_.timeout() == true) {
            std::cout << "Brother pose is timeout can not set MainAGV pose" << std::endl;
            return;
        }
        //Pose2d transform(-wheelBase_/2.0,0,0);
        //Navigation::ResetPose(pose * transform);
        Pose2d brother = timedBrotherPose_.Get();
        Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));

        if (diff.x() > 3.6 || diff.x() < 2.2 || diff.y() > 0.3 || diff.theta() > 5 * M_PI / 180.0) {
            std::cout << " distance with two agv is too far diff: " << diff << std::endl;
            return;
        }
        Pose2d abs_diff = pose - brother;
        //计算两车朝向的方向
        float theta = Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
        Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);

        Navigation::ResetPose(agv);
        timedPose_().m_diffYaw1=pose.theta()-theta;
        timedPose_().m_diffYaw2=brother.theta()-theta;
        printf("m_diffYaw1:%f, m_diffYaw2:%f\n",timedPose_().m_diffYaw1, timedPose_().m_diffYaw2);

    } else
        Navigation::ResetPose(pose);
}

void NavigationMain::publish_statu(NavMessage::NavStatu &statu) {
    statu.set_main_agv(true);
    Navigation::publish_statu(statu);
}


void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
    /*if(move_mode_!=Monitor_emqx::eDouble)
    {
      printf(" navigation mode must set main,parameter:Pose2d\n");
      return false;
    }*/

    Navigation::Start(cmd, response);

}

void NavigationMain::SendMoveCmd(int mode, ActionType type,
                                 double v[], double angular[]) {
    if (monitor_) {
        monitor_->set_ToAgvCmd(mode, type, v, angular, wheelBase_);
    }
}

bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
    if (Navigation::CreateRobotStatuMsg(robotStatu)) {
        robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
        robotStatu.mutable_agvstatu()->set_lifter_other(timed_other_lifter_.Get());
        return true;
    }
    //std::cout<<agvStatu.DebugString()<<std::endl;
    return false;
}

void NavigationMain::ResetOtherClamp(ClampStatu statu) {
    timed_other_clamp_.reset(statu, 1);
}

void NavigationMain::ResetOtherLifter(LifterStatus status) {
    timed_other_lifter_.reset(status, 1);
}

void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
    NavMessage::AgvStatu speed;
    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());
    }
}

bool NavigationMain::clamp_close() {
    if (move_mode_ == eSingle)
        return Navigation::clamp_close();
    printf("双车夹持\n");
    if (monitor_) {
        monitor_->clamp_close(move_mode_);
        while (cancel_ == false) {
            if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get() == eClosed) {
                printf("双车夹持completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_close(move_mode_);
        }
        return false;
    }
    return false;
}

bool NavigationMain::clamp_open() {
    if (move_mode_ == eSingle)
        return Navigation::clamp_open();

    if (monitor_) {
        printf("双车松夹持\n");
        monitor_->clamp_open(move_mode_);
        while (cancel_ == false) {
            if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
                printf("timed clamp is timeout\n");
                return false;
            }
            if (timed_clamp_.Get() == eOpened && timed_other_clamp_.Get() == eOpened) {
                printf("双车松夹持completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->clamp_open(move_mode_);
        }
        return false;
    }
    return false;
}

bool NavigationMain::lifter_rise() {
    if (move_mode_ == eSingle) {
        return Navigation::lifter_rise();
    }
    if (monitor_) {
        printf("双车提升机构提升\n");
        monitor_->lifter_rise(move_mode_);
        while (cancel_ == false) {
            if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                printf("timed lifter is timeout\n");
                return false;
            }
            if (timed_lifter_.Get() == eRose && timed_other_lifter_.Get() == eRose) {
                printf("双车提升机构提升completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->lifter_rise(move_mode_);
        }
        return false;
    }
    return false;
}

bool NavigationMain::lifter_down() {
    if (move_mode_ == eSingle) {
        return Navigation::lifter_down();
    }
    if (monitor_) {
        printf("双车提升机构下降\n");
        monitor_->lifter_down(move_mode_);
        while (cancel_ == false) {
            if (timed_lifter_.timeout() || timed_other_lifter_.timeout()) {
                printf("timed lifter is timeout\n");
                return false;
            }
            if (timed_lifter_.Get() == eDowned && timed_other_lifter_.Get() == eDowned) {
                printf("双车提升机构下降completed!!!\n");
                return true;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            monitor_->lifter_down(move_mode_);
        }
        return false;
    }
    return false;
}

Navigation::MpcResult
NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
    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 = 25 * M_PI / 180.0;
    double dt = 0.1;
    Pose2d rotated = Pose2d(space.x(), space.y(), space.theta());

    double x = space.x();
    double y = space.y();
    //后车先到,前车进入2点,保持与后车一致的朝向
    if (brother.in_space() && brother.space_id() == space.id()) {
        printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__);
        rotated.mutable_theta() = brother.odom().theta();
        if (move_mode_ == eSingle) {
            x += wheelbase * cos(rotated.theta());
            y += wheelbase * sin(rotated.theta());
        }
    } else { //当前车先到,正向
        printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
        rotated.mutable_theta() = space.theta();
    }

    if (move_mode_ == eDouble){
        x -= wheelbase/2 * cos(rotated.theta());
        y -= wheelbase/2 * sin(rotated.theta());
        printf("RotateBeforeEnterSpace | 整车模式, __LINE__ = %d\n", __LINE__);
    }

    target.set_x(x);
    target.set_y(y);
    target.set_theta(rotated.theta());
    target.set_l(0.02);
    target.set_w(0.05);
    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(100));
        Pose2d current = timedPose_.Get();
        double yawDiff = (rotated - 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.3 * 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(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;
}