AGV/MPC/monitor/emqx/mqttmsg.cpp

//
// Created by zx on 23-2-22.
//

#include "mqttmsg.h"
#include <string.h>

#include <google/protobuf/util/json_util.h>
using namespace google::protobuf;
MqttMsg::MqttMsg()
{
  data_= nullptr;
  length_=0;
}
MqttMsg::MqttMsg(char* data,const int length)
{
  data_=(char*)malloc(length);
  memcpy(data_,data,length);
  length_=length;
}
MqttMsg::MqttMsg(const MqttMsg& msg)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);
  data_=(char*)malloc(msg.length_);
  memcpy(data_,msg.data_,msg.length_);
  length_=msg.length_;
}
MqttMsg& MqttMsg::operator=(const MqttMsg& msg)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);
  data_=(char*)malloc(msg.length_);
  memcpy(data_,msg.data_,msg.length_);
  length_=msg.length_;
  return *this;
}

char* MqttMsg::data()const{
  return data_;
}
int MqttMsg::length()const{
  return length_;
}




MqttMsg::~MqttMsg(){
  if(data_!= nullptr)
  {
    free(data_);
    data_ = nullptr;
  }
  length_=0;
}


void MqttMsg::fromStatu(double x, double y, double theta, double v, double vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);

  AGVStatu statu;
  statu.set_x(x);
  statu.set_y(y);
  statu.set_theta(theta);
  statu.set_v(v);
  statu.set_vth(vth);

  google::protobuf::util::JsonPrintOptions option;
  option.always_print_primitive_fields=true;
  std::string json_str;
  google::protobuf::util::MessageToJsonString(statu,&json_str,option);

  length_=json_str.length();
  data_=(char*)malloc(length_);

  memcpy(data_,json_str.c_str(),length_);
}

bool MqttMsg::toStatu(double &x, double &y, double &theta, double &v, double &vth)
{
  if(strlen(data())<length_)
    return false;
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_== nullptr)
    return false;
  AGVStatu statu;
  util::Status ret;
  char* buf=(char*)malloc(length_+1);
  memset(buf,0,length_+1);
  memcpy(buf,data(),length_);
  try
  {
    ret = util::JsonStringToMessage(buf, &statu);
  }
  catch (std::exception &e)
  {
    printf(" exp:%s\n",e.what());
  }
  free(buf);

  if(ret.ok())
  {
    x=statu.x();
    y=statu.y();
    theta=statu.theta();
    v=statu.v();
    vth=statu.vth();

  }

  return ret.ok();
}

void MqttMsg::fromSpeed(double v, double vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);
  ChangeSpeedCmd cmd;
  cmd.set_v(v);
  cmd.set_vth(vth);

  google::protobuf::util::JsonPrintOptions option;
  option.always_print_primitive_fields=true;
  std::string json_str;
  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);

  length_=json_str.size();
  data_=(char*)malloc(length_);
  memcpy(data_,json_str.c_str(),length_);

}

bool MqttMsg::toSpeed(double &v, double &vth)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_== nullptr)
    return false;

  ChangeSpeedCmd cmd;
  util::Status ret;
  char* buf=(char*)malloc(length_+1);
  memset(buf,0,length_+1);
  memcpy(buf,data(),length_);
  try
  {

    //printf("%s\n",buf);
    ret = util::JsonStringToMessage(buf, &cmd);
    printf("change speed v:%f,  vth:%f\n",v,vth);

  }
  catch (std::exception &e)
  {
    printf(" exp:%s\n",e.what());
  }

  free(buf);
  if(ret.ok())
  {
    v=cmd.v();
    vth=cmd.vth();

  }

  return ret.ok();
}

void MqttMsg::fromNavCmd(const NavCmd& cmd)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);

  google::protobuf::util::JsonPrintOptions option;
  option.always_print_primitive_fields=true;
  std::string json_str;
  google::protobuf::util::MessageToJsonString(cmd,&json_str,option);

  length_=json_str.length();
  data_=(char*)malloc(length_);

  memcpy(data_,json_str.c_str(),length_);
}

bool MqttMsg::toNavCmd(NavCmd& cmd)
{
  if(strlen(data())<length_)
    return false;
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_== nullptr)
    return false;

  util::Status ret;
  char* buf=(char*)malloc(length_+1);
  memset(buf,0,length_+1);
  memcpy(buf,data(),length_);
  try
  {
    ret = util::JsonStringToMessage(buf, &cmd);
  }
  catch (std::exception &e)
  {
    printf(" exp:%s\n",e.what());
    return false;
  }
  free(buf);

  return ret.ok();
}

void MqttMsg::fromNavStatu(const NavStatu &statu)
{
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_)
    free(data_);

  google::protobuf::util::JsonPrintOptions option;
  option.always_print_primitive_fields=true;
  std::string json_str;
  google::protobuf::util::MessageToJsonString(statu,&json_str,option);

  length_=json_str.length();
  data_=(char*)malloc(length_);

  memcpy(data_,json_str.c_str(),length_);
}

bool MqttMsg::toNavStatu(NavStatu &statu)
{
  if(strlen(data())<length_)
    return false;
  std::lock_guard<std::mutex> lock(mutex_);
  if(data_== nullptr)
    return false;

  util::Status ret;
  char* buf=(char*)malloc(length_+1);
  memset(buf,0,length_+1);
  memcpy(buf,data(),length_);
  try
  {
    ret = util::JsonStringToMessage(buf, &statu);
  }
  catch (std::exception &e)
  {
    printf(" exp:%s\n",e.what());
    return false;
  }
  free(buf);

  return ret.ok();
}