Vehicle-cpp/remote_drive/EgoZJ/EgoSystem/message_queue.cpp

#include <atomic>
#include <string>
#include "../common/comm.h"
#include "api.h"
#include "../common/iobuffer.h"
#include "../common/sensor_socket.h"
#include "../common/peer_connection.h"
#include "VideoRenderer.h"
#include "../thirdparty/jsoncpp/include/json/json.h"
#include "protocol.pb.h"
#include "radar_sensor.h"
#include "imu_sensor.h"
#include "robot_sensor.h"
#include "Rtk.h"

#include "message_queue.h"
#include <fstream>
#include <iostream>
#include <cstring>

#include "hmacsha256.h"
#include <linux/usbdevice_fs.h>
#include <sys/ioctl.h>
 
CMessageQueue::CMessageQueue():Head(nullptr),Tail(nullptr)
{
   _peerId=-1;
   btimeStopedCar = false;
}
CMessageQueue::~CMessageQueue()
{

}

void renableUSB(const char* file)
{
    printf("Resetting USB device %s\n", file);
    int fd = open(file, O_WRONLY);
    if (fd < 0) {
        char text[256];
        perror(text);
        printf("Error opening output file %s", text);
        return;
    }


    int rc = ioctl(fd, USBDEVFS_RESET, 0);
    if (rc < 0) {
        perror("Error in ioctl");
        return;
    }
    printf("Reset successful\n");

    close(fd);
}



void CMessageQueue::Create()
{
  
    Json::Value root;
	Json::Reader jsonReader;
    std::ifstream ifile("./config.json");
    std::string serverip;
    int32_t _hostPort;
    int32_t _remotePort;
    _curTick=0;
    bStopedCar=false;
    if(jsonReader.parse(ifile,root))
    {
        std::cout<<"enter config json"<<std::endl;
        _serial=root["serial"].asString();
         
       int32_t _Rtkport=root["rtk_port"].asInt();
       int32_t _Rtkhost=root["rtk_host"].asInt();
       std::string _Rtkip=root["rtk_ip"].asString();
       std::cout<<"Rtk ip:"<< _Rtkip <<std::endl;
       //_Rtk = std::make_unique<SensorSocket<CRtkSensor>>(this, _Rtkip, _Rtkport, _Rtkhost);
       //_Rtk->Start();

       int32_t _radarport=root["radar_port"].asInt();
       int32_t _radarhost=root["radar_host"].asInt();
       std::string _radarip=root["radar_ip"].asString();
       std::cout<<"radar ip:"<<_radarip<<std::endl;
       //_radar=std::make_unique<SensorSocket<CRadarSensor>>(this,_radarip,_radarport,_radarhost);
       //_radar->Start();

       int32_t _UdpCanport=root["Udp_Can_port"].asInt();
       int32_t _UdpCanhost=root["Udp_Can_host"].asInt();
       std::string _UdpCanip=root["Udp_Can_ip"].asString();
       std::cout<<"UdpCan ip:"<<_UdpCanip<<std::endl;
       _UdpCan = std::make_unique<SensorSocket<CUdpCanSensor>>(this,_UdpCanip,_UdpCanport,_UdpCanhost);
       //_UdpCan->Start();

       int32_t _UdpStateport=root["Udp_State_port"].asInt();
       int32_t _UdpStatehost=root["Udp_State_host"].asInt();
       std::string _UdpStateip=root["Udp_State_ip"].asString();
       std::cout<<"UdpStateip ip:"<<_UdpStateip<<std::endl;
       _UdpState = std::make_unique<SensorSocket<CUdpStateSensor>>(this,_UdpStateip,_UdpStateport,_UdpStatehost);
       //_UdpState->Start();

       //_robot = std::make_unique<SensorPeakCan<CRobotSensor>>(this);
       //_robot->Start();

       _GSML = root["GMSL"].asInt();

       _UdpMinPort=root["udp_min_port"].asInt();
       _UdpMaxPort=root["udp_max_port"].asInt();
       //20230417
       //const char* serverUrl = "tcp://localhost:1883";  //服务器地址
       //const char* userName = "";  //用户名
       //const char* password = "";  //密码
       std::string serverUrl = root["MqttserverUrl"].asString();
       std::string userName = root["Esn"].asString();
       std::string password = root["EsnPass"].asString();
       std::string clientId = "";  //客户端标识符

       bool _Hm256 = false;

       unsigned char Source[128];

       time_t rawtime;
       struct tm* info;
       char buffer[80];
       time(&rawtime);

       info = localtime(&rawtime);

       uint8_t secret[32] = { 0 };
       //如UTC 时间2018/7/24 17:56:20 则应表示为2018072417。
       /*sprintf((char*)secret, "%d%.2d%.2d%.2d", info->tm_year + 1900, info->tm_mon + 1, info->tm_mday,
           info->tm_hour);*/
       sprintf((char*)secret, "%d%.2d%.2d", info->tm_year + 1900, info->tm_mon + 1, info->tm_mday);

       memset(Source, 0, 128);
       sprintf((char*)Source, "%s_0_0_%s", userName.c_str(), secret);
       clientId.clear();
       clientId.append((const char*)Source);

       if (password.length() && _Hm256)
       {
           uint8_t outdata[128] = { 0 };
           uint8_t md[SHA256_DIGESTLEN] = { 0 };

           int len1 = strlen((char*)secret);
           int len2 = strlen((char*)password.c_str());

           HMAC_SHA256_CTX hmac;
           hmac_sha256_init(&hmac, secret, len1);
           hmac_sha256_update(&hmac, (const uint8_t*)password.c_str(), len2);
           hmac_sha256_final(&hmac, md);

           memcpy(outdata, md, SHA256_DIGESTLEN);


           password.clear();
           password.append((const char*)outdata);
           
       }
       
       //_Mqtt = std::make_unique<SensorMQTT<CMqttSensor>>(this, serverUrl, userName, password, clientId);
       //_Mqtt->Start();

       std::string can_port = root["can_bus_vehicle"].asString();
       _CanBusVehicle = std::make_unique<SensorCanBus<CCanBusSensor>>(this, can_port);
       //_CanBusVehicle->Start();//车辆线控
       
       std::string can_port_radar = root["can_bus_radar"].asString();
       //_CanBusRadar->Start();

       std::string serial_Articulated = root["serial_Articulated"].asString();
       int32_t serial_Articulated_speed = root["serial_Articulated_speed"].asInt();
       //_ArticulatedSensor = std::make_unique<SensorSerial<CSerialSensor>>(this, serial_Articulated,serial_Articulated_speed);
       //_ArticulatedSensor->Start();//铰接转向姿态编码器

       std::string Drivenwheels_modbus_server = root["Drivenwheels_modbus_ip"].asString();
       int32_t Drivenwheels_modbus_port = root["Drivenwheels_modbus_port"].asInt();
       uint32_t Drivenwheels_modbus_slave[16] = {0};

       const Json::Value arrayslave_Drivenwheels=root["Drivenwheels_slave_id"];
        int32_t count_slave = arrayslave_Drivenwheels.size();
        for(int32_t i = 0; i < count_slave ; i++)
        {
            const Json::Value& value_id=arrayslave_Drivenwheels[i];
            Drivenwheels_modbus_slave[i] = value_id["id"].asInt();
        }
       //_ModbusTcp_Drivenwheels = std::make_unique<SensorModbusTcp<CModbusTcpSensor>>(this, Drivenwheels_modbus_server,Drivenwheels_modbus_port,Drivenwheels_modbus_slave,0);
       //_ModbusTcp_Drivenwheels->Start();//从动轮防撞激光传感器


       std::string Outriggers_modbus_server = root["Outriggers_modbus_ip"].asString();
       int32_t Outriggers_modbus_port = root["Outriggers_modbus_port"].asInt();
       uint32_t Outriggers_modbus_slave[16] = {0};

       const Json::Value arrayslave_Outriggers=root["Outriggers_slave_id"];
       count_slave = arrayslave_Outriggers.size();
        for(int32_t i = 0; i < count_slave ; i++)
        {
            const Json::Value& value_id = arrayslave_Outriggers[i];
            Outriggers_modbus_slave[i] = value_id["id"].asInt();
        }
       //_ModbusTcp_Outriggers = std::make_unique<SensorModbusTcp<COutriggersSensor>>(this, Outriggers_modbus_server,Outriggers_modbus_port,Outriggers_modbus_slave,0);
       //_ModbusTcp_Outriggers->Start();//后车架支腿拉线传感器器

       

       

        /*
        RemoNet::vehicle_QT m_vehicle_QT;
        RemoNet::vehicle_data *pdata = m_vehicle_QT.add__vehicle_data();
        pdata->set_dlc(1);
       */
       //std::string _usb_2 = root["usb_2"].asString();
       //std::string _usb_1 = root["usb_3"].asString();
       //renableUSB("/dev/bus/usb/002/002");
       //renableUSB("/dev/bus/usb/001/003");
       //renableUSB(_usb_2.c_str());
       //renableUSB(_usb_1.c_str());
        
        serverip=root["ip"].asString();
        _hostPort=root["TcpHostPort"].asInt();
        _remotePort=root["TcpRemotePort"].asInt();

        _name=root["name"].asString();
        const Json::Value arrayObj=root["camerainfo"];
        int32_t count=arrayObj.size();
        for(int32_t i=0;i<count;i++)
        {
            LocalCameraInfo info;
            const Json::Value& value=arrayObj[i];
            info.index=value["index"].asInt();
            info.label=value["label"].asString();
            _cameraArray.push_back(info);
        }
        for(int i=0;i<_cameraArray.size();i++)
        {
            _peerArray.push_back({nullptr});
        }

       
    }
    else{
        std::string error=jsonReader.getFormattedErrorMessages();
        std::cout<<error<<std::endl;
    }
    _client=std::make_unique<SocketClient>(this);
    _client->Start(serverip.c_str(),_remotePort,_hostPort);
  
    std::this_thread::yield();

}

/*
void CMessageQueue::WriteCanMessage(std::unordered_map<int32_t, cannet_frame>& node,bool islidar)
{
    if(!bDataChannelCreated) return;
     // std::lock_guard<std::mutex> l(_canLock);
      RemoNet::CCCanMesage Req;
      Req.set_islidar(islidar);
      for(auto& p:node)
      {
         int32_t lidar=p.second.canid;
         auto m=Req.add_message();
         m->set_head(p.second.dlc);
         m->set_canid(lidar);
         m->set_data(p.second.data,8);
        
      }
      
      MessageHead Head;
      CIOBuffer pBuffer;
      Head.Command = RemoNet::CC_CAN;
      Head.Length = Req.ByteSizeLong();
      Head.Serialize(pBuffer.Buffer);
      auto ptr = pBuffer.Buffer + MessageHead::Size();
      Req.SerializeToArray(ptr, Head.Length);
      pBuffer.Length = MessageHead::Size() + Head.Length;
      _peerArray[0]->SendData(&pBuffer);
}

bool CMessageQueue::IsCarId(int32_t value)
{
    return std::find(_carArray.begin(),_carArray.end(),value)!=_carArray.end();
}
*/
/*
#define QUEUE_ZISE	32//队列长度

typedef struct Queue
{
	int qFront;
	int qRear;
	CIOBuffer *BasicArr[QUEUE_ZISE];
}Queue, * pQueue;

void InitQueue(pQueue queue)
{
	queue->qFront = 0;
	queue->qRear  = 0;
}

bool IsEmptyQueue(pQueue queue)
{
	if (queue->qFront == queue->qRear)
		return true;
	else
		return false;
}

bool IsFullQueue(pQueue queue)
{
	if (((queue->qRear + 1) % QUEUE_ZISE) == queue->qFront)
		return true;
	else
		return false;
}

void EnterQueue(pQueue queue, CIOBuffer *vale)
{
	if (IsFullQueue(queue))
	{
		return;
	}

	queue->BasicArr[queue->qRear] = vale;
	queue->qRear = (queue->qRear + 1) % QUEUE_ZISE;
}

CIOBuffer * OutQueue(pQueue queue)
{
	if (IsEmptyQueue(queue))
	{
		return nullptr;
	}

	CIOBuffer * p = queue->BasicArr[queue->qFront];
	queue->qFront = (queue->qFront + 1) % QUEUE_ZISE;
	return  p;
}

Queue queue;
*/
void CMessageQueue::EnQueue(CIOBuffer* pBuffer)
{
    bool bNullBuffer=false;
    //std::unique_lock <std::mutex>  lck(_lock);
    if(Head==nullptr)
    {
        Head=Tail=pBuffer;
        bNullBuffer=true;
    }
    else{
        Tail->NextBuf=pBuffer;
        Tail=Tail->NextBuf;
    } 
    pBuffer->NextBuf=nullptr;
    if(bNullBuffer)
    {
        //_cv.notify_one();
        //20231023
        _cv.notify_all();
    }
}  

void CMessageQueue::Process()
{
    
    CIOBuffer * ptr=nullptr;
    {
        
        std::unique_lock <std::mutex>  lck(_lock);

        while(Head==nullptr&&_cv.wait_for(lck,std::chrono::seconds(1))==std::cv_status::timeout)
        {
            CheckSignal();
            //std::cout<<".";
            //std::cout.flush();
        }
        
        
    }
    while(Head!=nullptr)
    {
        ptr=Head;
        Head=Head->NextBuf;
        if(ptr!=nullptr)
        {
            Message* message=reinterpret_cast<Message *>(ptr->Buffer);
            switch (message->cmd)
            {
            case MessageType::ReqVideo:
            OnNotifyReq((int32_t)message->param_l); 
            break;
            case MessageType::RepVideo:
            OnNotifyRep((int32_t)message->param_l);
            break;
            case MessageType::Connected:
            OnNotifyConnected((bool)message->param_l);
            break;
            case MessageType::Leave:
            OnNotifyLeave();
            break;
            case MessageType::AsyncMessage:
            OnNotifyMessage();
            break;
            case MessageType::StopSensor:
            OnNotifyStopSensor();
            break;      
            case MessageType::Ping:
            OnNotifyPing(message->param_l);
            //_curTick = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
            break; 
            }
            ptr->Release();
        }   
    }
    
}
void CMessageQueue::SetTick(long long tick)
{
    _curTick=tick;
}


void CMessageQueue::OnNotifyConnected(bool bRet)
{
    if(bRet)
    {
        
        _client->WriteAddRobot(_serial,_name,static_cast<int32_t>(EgoType::Car));
        _updatethread.start(_client.get());
        //cs->Analog(0,0,0,0,0);

    }
    else
    {
        if(_peerId!=-1)
        {
            OnVideoLeave(_peerId,EgoType::User);
          /*  for (size_t i = 0; i < _peerArray.size(); i++)
            {
                if(_peerArray[i]!=nullptr)
                {
                    _peerArray[i]->Close();
                    _peerArray[i].reset();
                }
               
            }
            */
         //   for (size_t i = 0; i < _windowArray.size(); i++)
         //   {
                /* code */
         //       if(_windowArray[i]!=nullptr)
         //         _windowArray[i].reset();
         //   }
            _peerId=-1;
//            StopCar();
        }
      
        _updatethread.stop();
    }
}

void CMessageQueue::OnNotifyReq(int32_t index)
{ 
   // std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
   if(_peerId==-1) return;
     if(index==0)
       {
        mrsWebrtcCreateFactory(true);
        _curTick=0;
        bStopedCar=false;

        /*
       _CanBusVehicle->Start();//车辆线控
       _ArticulatedSensor->Start();//铰接转向姿态编码器
       _ModbusTcp_Drivenwheels->Start();//从动轮防撞激光传感器
       _ModbusTcp_Outriggers->Start();//后车架支腿拉线传感器器
       _radar->Start(); //DYP雷达
       _CanBusRadar->Start();//纳雷雷达
       _Rtk->Start();
       _robot->Start();
       _Mqtt->Start();
        */
       
       
       _CanBusVehicle->Start();//车辆线控
       _UdpState->Start();
       _UdpCan->Start();
       
       }
        
    
   
//	PeerConnectionWrapper* peer = nullptr;
//	VideoRenderer* window = nullptr;
   // _windowArray[temp]=std::make_unique<VideoRenderer>();// .reset(new VideoRenderer());
    _peerArray[index]=std::make_unique<CPeerConnection>(static_cast<ChannelType>(index),_client.get());
   
	
	_client->WriteVideoRep(_peerId, RemoNet::VideoDesc::OK, index);
}
void CMessageQueue::OnNotifyRep(int32_t index)
{
   
    _peerArray[index]=std::make_unique<CPeerConnection>(static_cast<ChannelType>(index), _client.get());
    
	 
	InitPeerConnection(_peerId,index);
	_peerArray[index]->CreateOffer();
	
}
void CMessageQueue::InitPeerConnection(int32_t peer,int32_t index)
{
	 
    //bool NeedData=index==RenderPosition::CAR_FRONT;

	_peerArray[index]->Initialize(peer,index,_UdpMinPort,_UdpMaxPort);
    //_peerArray[index]->AddDataChannel(true, false);
  	
    
   
    _peerArray[index]->AddLocalVideoTrack(static_cast<RenderPosition>(index),_cameraArray[index].index,_GSML);
    
    
     if(index==RenderPosition::BACK)
    {
        void * front=_peerArray[RenderPosition::FRONT]->GetCurrentCtx();
        while(front==nullptr)
        {
            std::cout<<"front==nullptr"<<std::endl;
             std::this_thread::sleep_for(std::chrono::microseconds(50));  
             front=_peerArray[RenderPosition::FRONT]->GetCurrentCtx();
        }
        void * back=_peerArray[RenderPosition::BACK]->GetCurrentCtx();
        while (back==nullptr)
        {
             std::cout<<"back==nullptr"<<std::endl;
            std::this_thread::sleep_for(std::chrono::microseconds(50));  
            back=_peerArray[RenderPosition::BACK]->GetCurrentCtx();
        }
        _peerArray[RenderPosition::FRONT]->SetOtherCtx(back);
        _peerArray[RenderPosition::BACK]->SetOtherCtx(front);
    }
    
    
   /*
    if((index+1)==RenderPosition::ALL)
    {
        void * front=_peerArray[RenderPosition::FRONT]->GetCurrentCtx();
        while(front==nullptr)
        {
            std::cout<<"front==nullptr"<<std::endl;
             std::this_thread::sleep_for(std::chrono::microseconds(50));  
             front=_peerArray[RenderPosition::FRONT]->GetCurrentCtx();
        }
        void * back=_peerArray[RenderPosition::BACK]->GetCurrentCtx();
        while (back==nullptr)
        {
             std::cout<<"back==nullptr"<<std::endl;
            std::this_thread::sleep_for(std::chrono::microseconds(50));  
            back=_peerArray[RenderPosition::BACK]->GetCurrentCtx();
        }
        _peerArray[RenderPosition::FRONT]->SetOtherCtx(back);
        _peerArray[RenderPosition::BACK]->SetOtherCtx(front);

        void * left=_peerArray[RenderPosition::LEFT]->GetCurrentCtx();
        while(left==nullptr)
        {
            std::cout<<"left==nullptr"<<std::endl;
             std::this_thread::sleep_for(std::chrono::microseconds(50));  
             left=_peerArray[RenderPosition::LEFT]->GetCurrentCtx();
        }
        void * right=_peerArray[RenderPosition::RIGHT]->GetCurrentCtx();
        while (right==nullptr)
        {
             std::cout<<"right==nullptr"<<std::endl;
            std::this_thread::sleep_for(std::chrono::microseconds(50));  
            right=_peerArray[RenderPosition::RIGHT]->GetCurrentCtx();
        }
         _peerArray[RenderPosition::LEFT]->SetOtherCtx(right);
        _peerArray[RenderPosition::RIGHT]->SetOtherCtx(left);
    }
    */
    /*
     if((index+1)==RenderPosition::ALL)
    {
        void * front=_peerArray[RenderPosition::FRONT]->GetCurrentCtx();
        void * back=_peerArray[RenderPosition::BACK]->GetCurrentCtx();
        void * left=_peerArray[RenderPosition::LEFT]->GetCurrentCtx();
        void * right=_peerArray[RenderPosition::RIGHT]->GetCurrentCtx();
        void * dash=_peerArray[RenderPosition::DASHBOARD]->GetCurrentCtx();
        _peerArray[RenderPosition::FRONT]->SetOtherCtx(back);
        _peerArray[RenderPosition::BACK]->SetOtherCtx(front);
        _peerArray[RenderPosition::LEFT]->SetOtherCtx(left);
        _peerArray[RenderPosition::RIGHT]->SetOtherCtx(right);
        _peerArray[RenderPosition::DASHBOARD]->SetOtherCtx(dash);
    }
    */
   

    if(index==RenderPosition::FRONT)
	   _peerArray[index]->AddLocalAudioTrack();
}
 
void CMessageQueue::OnAdd(bool bRet)
{

}
void CMessageQueue::OnConnected(bool bRet)
{
    
    CIOBuffer * pBuffer=CIOBuffer::Alloc();
    Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
    message->cmd=MessageType::Connected;
    message->param_l=bRet;
    EnQueue(pBuffer);

    //20240104
    //InitQueue(&queue);
    //EnterQueue(&queue, pBuffer);
   
}
 
void CMessageQueue::OnVideoLeave(int32_t peer,EgoType type)
{
    {
         CIOBuffer * pBuffer=CIOBuffer::Alloc();
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::StopSensor;
        EnQueue(pBuffer);

        //20240104
        //EnterQueue(&queue, pBuffer);
    }
    {
        CIOBuffer * pBuffer=CIOBuffer::Alloc();
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::Leave;
     
        EnQueue(pBuffer);

        //20240104
        //EnterQueue(&queue, pBuffer);
    }
}
#ifdef WIN32
void CMessageQueue::OnVideoRep(int32_t index,RemoNet::VideoDesc desc)
{
   if (desc == RemoNet::VideoDesc::OK)
   {
        assert(_peerId!=-1);
        CIOBuffer * pBuffer=CIOBuffer::Alloc();
        Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
        message->cmd=MessageType::ReqVideo;
        message->param_l=index;
        
        EnQueue(pBuffer);
   }

}
#else
void CMessageQueue::OnVideoReq(int32_t video,int32_t peer)
{
   // std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
    _peerId=peer;
    CIOBuffer * pBuffer=CIOBuffer::Alloc();
    Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
    message->cmd=MessageType::ReqVideo;
    message->param_l=video;
    
    EnQueue(pBuffer); 

    //20240104
    //EnterQueue(&queue, pBuffer);
}
#endif
void CMessageQueue::OnNotifyLeave()
{
  
     std::this_thread::sleep_for(std::chrono::milliseconds(100));   
   
     for (size_t i = 0; i < _peerArray.size(); i++)
     {
         if(_peerArray[i]!=nullptr)
         {
             _peerArray[i]->Close();
             _peerArray[i].reset();
         }
          /* code */
     }
     //for (size_t i = 0; i < _windowArray.size(); i++)
    // {
          /* code */
    //     _windowArray[i].reset();
   //  }
      _peerId=-1;
      
      //StopCar();

      
      
}
void CMessageQueue::OnNotifyStopSensor()
{
    _curTick=0;
    /*
    std::cout<<"vehicle can stop"<<std::endl;
    _CanBusVehicle->Stop();//车辆线控
    std::cout<<"rs485  _ArticulatedSensor stop"<<std::endl;
    _ArticulatedSensor->Stop();//铰接转向姿态编码器
    std::cout<<"Drivenwheels modbus tcp stop"<<std::endl;
    _ModbusTcp_Drivenwheels->Stop();//从动轮防撞激光传感器
    std::cout<<"Outriggers modbus tcp stop"<<std::endl;
    _ModbusTcp_Outriggers->Stop();//后车架支腿拉线传感器器
    std::cout<<"radar stop"<<std::endl;
    _radar->Stop(); //DYP雷达
    _CanBusRadar->Stop();//纳雷雷达
    _robot->Stop();
    std::cout << "RTK stop" << std::endl;
    _Rtk->Stop();
    std::cout<<"mqtt stop"<<std::endl;
    _Mqtt->Stop();
    */
    
    std::cout<<"vehicle can stop"<<std::endl;
    _CanBusVehicle->Stop();//车辆线控
    std::cout<<"vehicle UdpState stop"<<std::endl;
    _UdpState->Stop();
    std::cout<<"vehicle UdpCan stop"<<std::endl;
    _UdpCan->Stop();

     RemoNet::StopAck Rep;
    
     CIOBuffer Buffer;
     MessageHead Head;
     Head.Command = RemoNet::CC_StopACK;
     Head.Length = Rep.ByteSizeLong();
     Head.Serialize(Buffer.Buffer);
     auto ptr = Buffer.Buffer + MessageHead::Size();
     Rep.SerializeToArray(ptr, Head.Length);
     Buffer.Length = Head.Length + MessageHead::Size();
     _peerArray[RenderPosition::FRONT]->SendData(Buffer);


}
void CMessageQueue::OnVideoOffer(int32_t index,const char* type, const char* sdp)
{ 
 //   std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
    InitPeerConnection(_peerId,index); 
    
    _peerArray[index]->SetRemoteDescription(type,sdp);
    _peerArray[index]->CreateAnswer();
     
}
void CMessageQueue::OnVideoAnswer(int32_t index, const char* type, const char* sdp)
{ 
  //  std::cout<<__FUNCTION__<<","<<__LINE__<<std::endl;
      
     _peerArray[index]->SetRemoteDescription(type,sdp);
    
}
void CMessageQueue::OnVideoCandidate(int32_t index,const char* candidate,
		int32_t sdp_mline_index,
		const char* sdp_mid)
{ 
      
    _peerArray[index]->AddIceCandidate(candidate,sdp_mline_index,sdp_mid);
     
}
/*
void CMessageQueue::SwitchCamera(bool front)
{
     _peerArray[RenderPosition::FRONT_BACK]->SwitchCapture(front);
}
*/

void CMessageQueue::OnMessageFrameNotify(ChannelType type,int16_t cmd,int16_t length,const void * data)
{
     
     //std::cout<<"cmd:" <<std::hex<<cmd<<std::endl;
     if(cmd==RemoNet::CC_Text)
     {
          RemoNet::TestTextReq Req;
       
          Req.ParseFromArray(data,length);
          std::cout<<Req.text()<<std::endl;
          CIOBuffer * pBuffer=CIOBuffer::Alloc();
          Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
          message->cmd=MessageType::AsyncMessage;
          EnQueue(pBuffer);

          //20240104
          //EnterQueue(&queue, pBuffer);
    
     }
     else if(cmd==RemoNet::CC_Switch)
     {
          RemoNet::CCSwitch Req;
          Req.ParseFromArray(data,length);
          bool front=Req.front();
          _peerArray[RenderPosition::FRONT]->SwitchCapture(front);

     }
     else if(cmd==RemoNet::CC_Ping)
     {
         /*
         RemoNet::CCPing Req;
         Req.ParseFromArray(data,length);
         CIOBuffer * pBuffer=CIOBuffer::Alloc();
         Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
         message->cmd=MessageType::Ping;
         message->param_l=Req.tick();
         EnQueue(pBuffer);
        */
         //20240104
         //EnterQueue(&queue, pBuffer);
         
     }
     else if(cmd==RemoNet::CC_SensorStop)
     {

           CIOBuffer * pBuffer=CIOBuffer::Alloc();
           Message* message=reinterpret_cast<Message *>(pBuffer->Buffer);
           message->cmd=MessageType::StopSensor;
           EnQueue(pBuffer);

           //20240104
           //EnterQueue(&queue, pBuffer);
     }
     else if (cmd == RemoNet::CC_CANMSG)
     {
         //_curTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
         /*
         RemoNet::CCCanMsg Req;
         Req.ParseFromArray(data, length);

         cannet_busframe* frames = (cannet_busframe*)alloca(sizeof(cannet_busframe) * Req.frams_size());

         for (int32_t i = 0; i < Req.frams_size(); i++)
         {
             auto& frame = Req.frams(i);
             frames[i].canid = frame.canid();
             frames[i].dlc = frame.dlc();
             memcpy(frames[i].data, frame.data().data(), frame.dlc());

         }
         _CanBusVehicle->Get()->OnMessage(frames, Req.frams_size());
         //_UdpCan->Get()->OnMessage(frames, Req.frams_size());
         */
     }
     
}

void CMessageQueue::SetTickLive(long long tick)
{
    _curTick = tick;
}

void CMessageQueue::SendNoEmergency()
{
    CIOBuffer buffer;
    int8_t source[16] = "wobuting";

    buffer.Length = strlen((const char *)source);
    memcpy(buffer.Buffer, &source, buffer.Length);

    _UdpState->Write(&buffer);
}

void CMessageQueue::SendEmergency()
{
    CIOBuffer buffer;
    int8_t source[16] = "woyaoting";

    buffer.Length = strlen((const char *)source);
    memcpy(buffer.Buffer, &source, buffer.Length);

    _UdpState->Write(&buffer);
}

void CMessageQueue::SetGsmlInfo(int64_t time,int64_t StartRecord,int64_t DeviceId)
{
    //if( _peerArray[ChannelType::CHANNEL_IMU]!=nullptr)
    //    _peerArray[ChannelType::CHANNEL_IMU]->SetGsml(time,StartRecord,DeviceId);

   if( _peerArray[DeviceId]!=nullptr)
    {
        _peerArray[DeviceId]->SetGsml(time,StartRecord,DeviceId);
    }
}

void CMessageQueue::Senddirection(int16_t speed)
{
    CIOBuffer buffer;

    
    int16_t source[16];
    memset(source,0,16);

    if(speed < 0)
        sprintf((char *)source,"%d",speed);
    else
        sprintf((char *)source,"+%d",speed);

    buffer.Length = strlen((const char *)source);
    memcpy(buffer.Buffer, &source, buffer.Length);

     //buffer.Length = 1;
     //buffer.Buffer[0] = speed;
     //printf("%s\r\n",source);

    _UdpState->Write(&buffer);

}

void CMessageQueue::StopCar()
{
    std::cout<<"Stop Car"<<std::endl;
    _CanBusVehicle->Get()->Emergency();
}
 
void CMessageQueue::OnNotifyMessage()
{
         RemoNet::TestTextReq Req;
         Req.set_text("ewqewqewqe");
         CIOBuffer Buffer;
          MessageHead Head;
          Head.Command = RemoNet::CC_Text;
          Head.Length = Req.ByteSizeLong();
          Head.Serialize(Buffer.Buffer);
          auto ptr = Buffer.Buffer + MessageHead::Size();
          Req.SerializeToArray(ptr, Head.Length);
          Buffer.Length = Head.Length + MessageHead::Size();
         _peerArray[RenderPosition::FRONT]->SendData(Buffer);
         
}



void CMessageQueue::OnNotifyPing(int64_t value)
{
     RemoNet::CCPing Rep;
     Rep.set_tick(value);
     CIOBuffer Buffer;
     MessageHead Head;
     Head.Command = RemoNet::CC_Ping;
     Head.Length = Rep.ByteSizeLong();
     Head.Serialize(Buffer.Buffer);
     auto ptr = Buffer.Buffer + MessageHead::Size();
     Rep.SerializeToArray(ptr, Head.Length);
     Buffer.Length = Head.Length + MessageHead::Size();

     if(  _peerArray[RenderPosition::FRONT]!=nullptr && _peerArray[RenderPosition::FRONT]->bReadyChannel)
     {
        _peerArray[RenderPosition::FRONT]->SendData(Buffer);
     }
}

/*
void CMessageQueue::StartCar()
{
    _can->SetStartWrite(true);
}
*/
void CMessageQueue::CheckSignal()
{
     
    if(!bStopedCar)
    {
        long long tick=std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch()).count();
        if(_curTick!=0&&tick-_curTick > 3) 
        {
            StopCar();
            bStopedCar=true;
            std::cout<<"_curTick!=0&&tick-_curTick > 3" << std::endl;
            _curStopTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
            btimeStopedCar = true;
            _curTick = 0;
        }
    }
     
}
void CMessageQueue::WriteIMUData(ImuData* data)
{
     
    MessageHead Head;
    CIOBuffer Buffer;
       
    RemoNet::IMuMessage Req;
  
   
    Req.set_rx(data->ry);
    Req.set_ry(data->rx);
   // Req.set_rz(data->rz);
 
    Head.Command=RemoNet::CC_IMU;
    Head.Length=Req.ByteSizeLong();
    Head.Serialize(Buffer.Buffer);
    auto ptr = Buffer.Buffer + MessageHead::Size();
    Req.SerializeToArray(ptr, Head.Length);
    Buffer.Length = Head.Length + MessageHead::Size();
    
    if( _peerArray[ChannelType::CHANNEL_IMU]!=nullptr)
        _peerArray[ChannelType::CHANNEL_IMU]->SendData(Buffer);
    
  
}

void CMessageQueue::SendVehicleStatue()
{
    RemoNet::State req;
    
    req.set_work_pressure(_Feeddata.work_pressure);
    req.set_brake_pressure(_Feeddata.brake_pressure);
    req.set_gearbox_oil_temp(_Feeddata.gearbox_oil_temp);
    req.set_gearbox_oil_pressure(_Feeddata.gearbox_oil_pressure);
    req.set_engine_rpm(_Feeddata.engine_rpm);//发动机转速                    
	req.set_speed(_Feeddata.speed);//不知道？---可能是后加的传感器---驱动轮增量编码器
	req.set_gear(_Feeddata.gear);
	req.set_engine_pressure(_Feeddata.engine_pressure);//发动机压力               
	req.set_cold_water(_Feeddata.cold_water);//冷却液温度-发动机水温          
	req.set_steer_angle(_Feeddata.steer_angle);//转向角度（原来金川是轮子的现在大冶用铰链位置）          铰接转向姿态编码器
	req.set_left_lock(_Feeddata.left_lock);//遥操模式
	req.set_right_lock(_Feeddata.right_lock);//人工模式
    req.set_engine_time(_Feeddata.engine_time);//发动机运行小时
	req.set_wire_sensor_l(_Feeddata.wire_sensor_l);//拉线传感器左侧
	req.set_wire_sensor_r(_Feeddata.wire_sensor_r);//拉线传感器右侧
	req.set_fuel_level(_Feeddata.fuel_level);//燃油油位
	req.set_lock_switch_l(_Feeddata.lock_switch_l);//锁钩到位开关左
	req.set_lock_switch_r(_Feeddata.lock_switch_r); //锁钩到位开关右
	req.set_tip_limit(_Feeddata.tip_limit);//倾翻限位
	req.set_turn_left(_Feeddata.turn_left);//左转
	req.set_turn_right(_Feeddata.turn_right);//右转
	req.set_hight_beam(_Feeddata.hight_beam);//远光灯
	req.set_low_beam(_Feeddata.low_beam);//近光灯 
	req.set_parking(_Feeddata.parking);//驻车
	req.set_brake(_Feeddata.brake);//刹车
	req.set_back_car(_Feeddata.back_car);//倒车
	req.set_front_work_lamp(_Feeddata.front_work_lamp);//前工作灯
	req.set_rear_work_lamp(_Feeddata.rear_work_lamp);//后工作灯
	req.set_cargo_weight(_Feeddata.cargo_weight);//渣包重量
	req.set_system_vol(_Feeddata.system_vol);//系统电压
	req.set_error_buff((char *)_Feeddata.Error_Buff,8);//报警信息
	req.set_f_gear(_Feeddata.f_gear);//1F，2R，3N
	req.set_gear_1(_Feeddata.gear_1);//1，2，3，4
    req.set_outgrigger_l(_Feeddata.outgrigger_l);//左支腿伸到位开关
    req.set_outgrigger_r(_Feeddata.outgrigger_r);//右支腿伸到位开关
    req.set_tip_pressure(_Feeddata.tip_pressure);//倾翻压力开关
    MessageHead Head;
    CIOBuffer pBuffer;
    Head.Command = RemoNet::CC_STATE;
    Head.Length = req.ByteSizeLong();
    Head.Serialize(pBuffer.Buffer);
    auto ptr = pBuffer.Buffer + MessageHead::Size();
    req.SerializeToArray(ptr, Head.Length);
    pBuffer.Length = MessageHead::Size() + Head.Length;

    //if( _peerArray[ChannelType::CHANNEL_CAR] != nullptr  && _peerArray[RenderPosition::FRONT]->bReadyChannel)
    //    WritePacket(ChannelType::CHANNEL_CAR, pBuffer);
    if( _peerArray[ChannelType::CHANNEL_CAR] != nullptr)
        _UdpCan->Write(&pBuffer);
}

void CMessageQueue::WritePacket(ChannelType type, CIOBuffer & pBuffer)
{
     if( _peerArray[type]!=nullptr)
        _peerArray[type]->SendData(pBuffer);
}
void CMessageQueue::WriteRadarData(RadarData& data)
{
      MessageHead Head;
      CIOBuffer Buffer;
      RemoNet::CCRadarMessage Req;
      Req.set_radar0(data.r0);
      Req.set_radar1(data.r1);
      Req.set_radar2(data.r2);
      Req.set_radar3(data.r3);
      Req.set_radar4(data.r4);
      Req.set_radar5(data.r5);
      Req.set_radar6(data.r6);
      Req.set_radar7(data.r7);
      Req.set_radar7(data.r8);
      //Head.Command=RemoNet::CC_IMU;
      Head.Command = RemoNet::CC_Radar;
      Head.Length=Req.ByteSizeLong();
      Head.Serialize(Buffer.Buffer);
      auto ptr = Buffer.Buffer + MessageHead::Size();
      Req.SerializeToArray(ptr, Head.Length);
      Buffer.Length = Head.Length + MessageHead::Size();

      if( _peerArray[ChannelType::CHANNEL_RADAR]!=nullptr)
        _UdpCan->Write(&Buffer);
         //_peerArray[ChannelType::CHANNEL_RADAR]->SendData(Buffer);
       
}

void CMessageQueue::WriteRobotStatus(int32_t ,int32_t )
{
    
}
#ifdef LIDAR_SENSOR
void CMessageQueue::WriteLidarPoint(const PointCloudMsg<PointXYZI>& msg,ChannelType side)
{
    RemoNet::LidarPoint pt;
    pt.set_is_left(side==ChannelType::CHANNEL_LEFT_LIDAR);
    pt.set_frame_id(msg.frame_id);
    pt.set_height(msg.height);
    pt.set_width(msg.width);
    pt.set_is_dense(msg.is_dense);
    pt.set_seq(msg.seq);
    pt.set_timestamp(msg.timestamp);
    for(int i=0;i<msg.point_cloud_ptr->size();i++)
    {
        pt.add_data((*msg.point_cloud_ptr)[i].x);
        pt.add_data((*msg.point_cloud_ptr)[i].y);
        pt.add_data((*msg.point_cloud_ptr)[i].z);
        pt.add_data((*msg.point_cloud_ptr)[i].intensity);
    }
    MessageHead Head;
    CIOBuffer Buffer;
    Head.Command=RemoNet::CC_LIDARDATA;
    Head.Length=pt.ByteSizeLong();
    Head.Serialize(Buffer.Buffer);
    auto ptr = Buffer.Buffer + MessageHead::Size();
    pt.SerializeToArray(ptr, Head.Length);
    Buffer.Length = Head.Length + MessageHead::Size();
    
    if( _peerArray[side]!=nullptr)
        _peerArray[side]->SendData(&Buffer);
}
#endif
void CMessageQueue::SwitchCamera(bool front)
{
     
      _peerArray[RenderPosition::FRONT]->SwitchCapture(front);
      _peerArray[RenderPosition::BACK]->SwitchCapture(front);
}

void CMessageQueue::SendZGJStatus(int status)
{ 
    _Rtk->Get()->Send_ZGJ_status(status);
}