AGV/MPC/navigation_main.h

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

#ifndef NAVIGATION_NAVIGATION_MAIN_H
#define NAVIGATION_NAVIGATION_MAIN_H

#include "navigation.h"
class NavigationMain : public Navigation {

public:
    NavigationMain();

    ~NavigationMain();

    virtual void ResetPose(const Pose2d &pose);

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

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


    void ResetOtherClamp(ClampStatu statu);

    void ResetOtherLifter(LifterStatus status);

    virtual void HandleAgvStatu(const MqttMsg &msg);

    virtual void SwitchMode(int mode, float wheelBase) {
        printf("change mode to:%d\n", mode);
        wheelBase_ = wheelBase;
        if (move_mode_ == mode)
            return;
        if (mode == eDouble) {
            if (timedBrotherPose_.timeout() == true || timedPose_.timeout() == true) {
                std::cout << "Brother/self 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 pose = timedPose_.Get();
            Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));

            if (diff.x() > 3.4 || diff.x() < 2.3 || diff.y() > 0.2 || diff.theta() > 5 * M_PI / 180.0) {
                std::cout << " distance with two agv is too far diff: " << diff << std::endl;
                return;
            }

            Pose2d self_pose = timedPose_.Get();
            Pose2d brother_pose = timedBrotherPose_.Get();
            Pose2d brother_in_self = Pose2d::relativePose(brother_pose, self_pose);

            if (brother_in_self.x() <= 0)
                mode = mode | eMainInForward;
            else
                mode = mode | eMainInBackward;
            if (brother_in_self.theta() >= -M_PI / 2 && brother_in_self.theta() <= M_PI / 2)
                mode = mode | eChildSameToMain;
            else
                mode = mode | eChildOppositeToMain;

            printf(" Switch MoveMode --> main(%d), wheelBase: %f\n", mode, wheelBase);
        }

        move_mode_ = mode;
    };

protected:
    virtual Navigation::MpcResult
    RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target);

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

    virtual bool clamp_close();

    virtual bool clamp_half_open();

    virtual bool lifter_rise();

    virtual bool lifter_down();

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

protected:

    TimedLockData<int> timed_other_clamp_;
    TimedLockData<int> timed_other_lifter_;
};

#endif //NAVIGATION_NAVIGATION_MAIN_H