AGV/MPC/navigation_main.cpp

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

#include "navigation_main.h"


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

}

void NavigationMain::ResetPose(const Pose2d& pose){
  if(move_mode_==Monitor_emqx::eMain) {
    Pose2d transform(-wheelBase_/2.0,0,0);
    Navigation::ResetPose(pose * transform);
  }
  else
    Navigation::ResetPose(pose);
}

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

void NavigationMain::HandleNavCmd(const NavMessage::NavCmd& cmd)
{
  if(cmd.action()==4)
  {
    if(cmd.wheelbase()<2.4 || cmd.wheelbase()>3.5)
    {
      printf("Failed to Switch MoveMode --> main,wheelbase invalid :%f\n",cmd.wheelbase());
      return;
    }
    printf(" Switch MoveMode --> main\n");
    SwitchMode(Monitor_emqx::eMain,cmd.wheelbase());
    return ;
  }
  if(cmd.action()==5)
  {
    printf(" Switch MoveMode --> single\n");
    SwitchMode(Monitor_emqx::eSingle,0);
    return ;
  }
  else
  {
    Navigation::HandleNavCmd(cmd);
  }
}

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

  return Navigation::Start(cmd);

}

void NavigationMain::SendMoveCmd(Monitor_emqx::MoveMode mode,Monitor_emqx::SpeedType type,
                double v,double angular)
{
  if(monitor_)
  {
    monitor_->set_speed(mode,type,v,angular,wheelBase_);
  }

}