AGV/MPC/monitor/monitor_emqx.cpp

//
// Created by zx on 23-3-14.
//

#include "monitor_emqx.h"
#include <math.h>
#include <unistd.h>


Monitor_emqx::Monitor_emqx(std::string nodeId)
    : Terminator_emqx(nodeId)
{
  heat_=0;
}

Monitor_emqx::~Monitor_emqx()
{

}


void Monitor_emqx::set_speedcmd_topic(std::string speedcmd)
{
  speedcmd_topic_=speedcmd;
}

void Monitor_emqx::set_speed(SpeedType type,double v,double a)
{
  double w=fabs(v)>0.001?v:0.0;
  MqttMsg msg;
  NavMessage::Speed speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(type);
  speed.set_v(v);
  speed.set_w(w);
  speed.set_end(1);
  msg.fromProtoMessage(speed);
  Publish(speedcmd_topic_,msg);

}

void Monitor_emqx::stop()
{
  MqttMsg msg;
  NavMessage::Speed speed;
  heat_=(heat_+1)%255;
  speed.set_h(heat_);
  speed.set_t(eStop);
  speed.set_v(0);
  speed.set_w(0);
  speed.set_end(1);
  msg.fromProtoMessage(speed);

  Publish(speedcmd_topic_,msg);
}