AGV/MPC/monitor/monitor_emqx.cpp

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

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

Monitor_emqx::~Monitor_emqx()
{
  if(client_!= nullptr){
    client_->disconnect();
    delete client_;
    client_= nullptr;
  }
}

bool Monitor_emqx::Connect(std::string ip,int port)
{

  if(client_!= nullptr)
  {
    client_->disconnect();
    delete client_;
  }
  client_=new Paho_client(nodeId_);
  bool ret= client_->connect(ip,port);
  if(ret)
  {
    while(!client_->isconnected()) usleep(1000);
    client_->subcribe(subTopic_,1,StatuArrivedCallback,this);
  }
  return ret;

}

void Monitor_emqx::set_statu_arrived_callback(StatuCallback callback,void* context)
{
  StatuArrivedCallback_=callback;
  context_=context;
}
void Monitor_emqx::set_speed(double v,double a)
{
  MqttMsg msg;
  msg.fromSpeed(v,a);
  if(client_)
    client_->publish(pubTopic_,1,msg);
  else
    printf("set speed failed : emqx client disconnected...\n");
}
void Monitor_emqx::rotate(double v)
{
  MqttMsg msg;
  msg.fromSpeed(0,v);
  if(client_)
    client_->publish(pubTopic_,1,msg);
  else
    printf("rotate failed : emqx client disconnected...\n");
}
void Monitor_emqx::stop()
{
  MqttMsg msg;
  msg.fromSpeed(0,0);
  if(client_)
    client_->publish(pubTopic_,1,msg);
  else
    printf("stop failed : emqx client disconnected...\n");
}

Monitor_emqx::Monitor_emqx(std::string nodeId,std::string pubTopic,std::string subTopic)
  :client_(nullptr),nodeId_(nodeId),pubTopic_(pubTopic),subTopic_(subTopic)
{
  StatuArrivedCallback_= nullptr;
  context_= nullptr;
}

void Monitor_emqx::StatuArrivedCallback(std::string topic,int QOS,MqttMsg& msg,void* context)
{
  Monitor_emqx* monitor=(Monitor_emqx*)context;
  double x=0,y=0,theta=0,v=0,vth=0;

  if(msg.toStatu(x,y,theta,v,vth))
  {
    if (monitor->StatuArrivedCallback_)
      monitor->StatuArrivedCallback_(x, y, theta, v, vth,monitor->context_);
  }
}