lio-cpp/MPC/monitor/monitor.cpp

//
// Created by zx on 22-12-7.
//

#include "monitor.h"
void Stringsplit(const std::string& str, const char split, std::vector<std::string>& res)
{
  if (str == "")        return;
  //在字符串末尾也加入分隔符，方便截取最后一段
  std::string strs = str + split;
  size_t pos = strs.find(split);

  // 若找不到内容则字符串搜索函数返回 npos
  while (pos != strs.npos)
  {
    std::string temp = strs.substr(0, pos);
    res.push_back(temp);
    //去掉已分割的字符串,在剩下的字符串中进行分割
    strs = strs.substr(pos + 1, strs.size());
    pos = strs.find(split);
  }
}


Monitor* Monitor::g_Monitor= nullptr;
Monitor* Monitor::get_instance()
{
  if(g_Monitor== nullptr)
  {
    g_Monitor=new Monitor();
  }
  return g_Monitor;
}

Monitor::Monitor(){
  set_read_callback(recv_callback,this);
}
Monitor::~Monitor()
{

}


void Monitor::set_speed(double v,double a)
{
  v_=v;
  a_=a;
  char buf[255]={0};
  sprintf(buf,"V:%f;A:%f",v,a);
  write_data(buf);

}

void Monitor::set_statucallback(StatuCallback callback)
{
  callback_=callback;
}

void Monitor::recv_callback(char* data,void* p)
{
  Monitor* pMonitor= (Monitor*)p;
  if(pMonitor== nullptr)
    return;

  std::string recv(data);
  std::vector<std::string> splits;
  Stringsplit(recv,';',splits);
  if(splits.size()!=3)
    return ;

  std::string pose_str=splits[0];
  std::string v_str=splits[1];
  std::string a_str=splits[2];

  std::vector<std::string> pose_split;
  Stringsplit(pose_str,':',pose_split);
  if(pose_split.size()==2)
  {
    std::string pose_value=pose_split[1];
    std::vector<std::string> value_strs;
    Stringsplit(pose_value,',',value_strs);
    if(value_strs.size()==3)
    {
      float x=atof(value_strs[0].c_str());
      float y=atof(value_strs[1].c_str());
      float theta=atof(value_strs[2].c_str());
      pMonitor->mtx_.lock();
      pMonitor->pose_=Pose2d(x,y,theta);
      pMonitor->mtx_.unlock();
      //std::cout<<"pose:"<<pMonitor->pose_;
    }
  }

  std::vector<std::string> v_split;
  Stringsplit(v_str,':',v_split);
  if(v_split.size()==2)
  {
    pMonitor->v_=atof(v_split[1].c_str());
    //std::cout<<"; v:"<<pMonitor->v_;
  }

  std::vector<std::string> a_split;
  Stringsplit(a_str,':',a_split);
  if(a_split.size()==2)
  {
    pMonitor->a_=atof(a_split[1].c_str());
    //std::cout<<"; a:"<<pMonitor->a_;
  }
  //std::cout<<std::endl;
  if(pMonitor->callback_!= nullptr)
    pMonitor->callback_(pMonitor->pose_,pMonitor->v_,pMonitor->a_);
}