Vehicle-cpp/remote_drive/EgoZJ/common/sensor_socket.h

#pragma once
#ifdef WIN32
#ifndef WIN32_LEAN_AND_MEAN
#define WIN32_LEAN_AND_MEAN
#endif
#include <WS2tcpip.h>
#include <WinSock2.h>
#define socketerrno WSAGetLastError()
#define SOCKET_EAGAIN_EINPROGRESS WSAEINPROGRESS
#define SOCKET_EWOULDBLOCK WSAEWOULDBLOCK
#ifndef _SSIZE_T_DEFINED
typedef int ssize_t;
#define _SSIZE_T_DEFINED
#endif
#ifndef _SOCKET_T_DEFINED
typedef SOCKET socket_t;
#define _SOCKET_T_DEFINED
#endif
#else
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h> 
#include <fcntl.h>
#include <poll.h>

#define socketerrno errno
#define SOCKET_EAGAIN_EINPROGRESS EAGAIN
#define SOCKET_EWOULDBLOCK EWOULDBLOCK
#define INVALID_SOCKET -1
#define SOCKET_ERROR -1
#ifndef _SOCKET_T_DEFINED
typedef int socket_t;
#define _SOCKET_T_DEFINED
#endif
#endif
#include <thread>
#include <mutex>
#include <iostream>
#include <unordered_map>
#include "../common/comm.h"

#ifdef _MAINDLG
class CThreadWindow;
#else
class CMessageQueue;
#endif
class CIOBuffer;
template<typename T>
class SensorSocket
{
public:
#ifdef _MAINDLG
	SensorSocket(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port);
#else
	SensorSocket(CMessageQueue * q,std::string can_ip,int32_t can_port,int32_t host_port);
#endif
	bool Start(const char* ip=nullptr);
	void Run();
	void Stop();
	void Write(CIOBuffer * pBuffer);
	void Read(CIOBuffer* pBuffer);
	T* Get();
#ifndef WIN32
    void SetStartRead(bool b);
	void SetStartWrite(bool b);
#endif
private:
	socket_t _fd;
	std::thread  _thread;
	bool _run;
	std::string _canip;
	int32_t _canport;
	int32_t _hostport;
	std::mutex _lock;
	std::unordered_map<int32_t, cannet_frame> _message;
	std::unique_ptr<T> _sensorNotify;
    sockaddr_in _canaddr;

	int flag;
#ifndef WIN32
	bool _startRead;
	bool _startWrite;
#endif
};
template<typename T>
#ifdef _MAINDLG
SensorSocket<T>::SensorSocket(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port)
#else
SensorSocket<T>::SensorSocket(CMessageQueue* q, std::string can_ip, int32_t can_port, int32_t host_port)
#endif
{
  	_sensorNotify=std::make_unique<T>(q);
	_canip=can_ip;
	_canport=can_port;
	_hostport=host_port;
#ifndef WIN32
 _startWrite=_startRead=true;
#endif
}
template<typename T>

bool SensorSocket<T>::Start(const char * ip)
{
#ifdef WIN32
	WSAData data;
	WSAStartup(MAKEWORD(2, 2), &data);
#endif

    std::cout<<"SensorSocket<T>::Start"<<std::endl;
	
	_sensorNotify->SetSensorSocket(this);
	_fd = socket(AF_INET, SOCK_DGRAM, 0);
	sockaddr_in sin;
	sin.sin_family = AF_INET;
	sin.sin_port = htons(_hostport);
#ifdef WIN32
	sin.sin_addr.s_addr = inet_addr(ip);
#else
	sin.sin_addr.s_addr = htonl(INADDR_ANY);
#endif
	if (::bind(_fd, (sockaddr*)&sin, sizeof(sin)) == -1)
		return false;
	/*
	flag = fcntl(_fd, F_GETFL, 0);
	if (flag < 0)
	{
		printf("fcntl failed.\n");
	}
	flag |= O_NONBLOCK;
	if (fcntl(_fd, F_SETFL, flag) < 0)
	{
		printf("fcntl failed.\n");
	}*/

	_canaddr.sin_family=AF_INET;
	_canaddr.sin_addr.s_addr=inet_addr(_canip.c_str());
	_canaddr.sin_port=htons(_canport);	
	_sensorNotify->Start();
	_thread = std::thread(&SensorSocket::Run, this);
	
	return true;
}
template<typename T>
void SensorSocket<T>::Read(CIOBuffer* pBuffer)
{
	sockaddr_in from;
	socklen_t fromlen=sizeof(sockaddr_in);
	int32_t ret = recvfrom(_fd,(char *)pBuffer->Buffer, CIOBuffer::IO_BUFFER_SIZE,0,(sockaddr*)&from,&fromlen);
	if (ret <= 0)
	{
		 return;
    }
	pBuffer->Length=ret;
}
template<typename T>
void SensorSocket<T>::Run()
{
	_run = true;
	
	struct pollfd fds[1];
	fds[0].fd = _fd;
    fds[0].events = POLLIN;
	

	while (_run)
	{
		if(poll(&fds[0], 1, 1000) > 0)
	   	{
			
			if (fds[0].revents & POLLIN)
			{
				CIOBuffer pBuffer;
				sockaddr_in from;
				socklen_t fromlen=sizeof(sockaddr_in);
				int32_t ret = recvfrom(_fd,(char *)pBuffer.Buffer, CIOBuffer::IO_BUFFER_SIZE,0,(sockaddr*)&from,&fromlen);
				if (ret <= 0||!_run)
				{
					continue;
				}
	   			_sensorNotify->Notify(pBuffer.Buffer,ret);
			}
	   }
	}
    std::cout<<"SensorSocket<T>::Run Finished"<<std::endl;
}
template<typename T>
void SensorSocket<T>::Write(CIOBuffer * pBuffer)
{
	
#ifndef WIN32
	if(_startWrite==false) return;
#endif
	socklen_t len=sizeof(_canaddr);
	 
    int ret=::sendto(_fd,(char *)pBuffer->Buffer,pBuffer->Length,0,(const sockaddr *)&_canaddr,len);
	{
	  //   std::cout<<"ret = "<<ret<<" size ="<<pBuffer->Length<<std::endl;
	}
 
}


template<typename T>
void SensorSocket<T>::Stop()
{
	if(!_run) return;
	_sensorNotify->Stop();
	
	_run = false;
#ifdef WIN32
	closesocket(_fd);
#else
	close(_fd);
#endif
    std::cout<<"SensorSocket<T>::Stop"<<std::endl;
	_thread.join();
}
#ifndef WIN32
template<typename T>
void SensorSocket<T>::SetStartRead(bool b)
{
	_startRead=b;
}
template<typename T>
void SensorSocket<T>::SetStartWrite(bool b)
{
	_startWrite=b;
}
#endif
template<typename T>
T* SensorSocket<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
class SensorTCP
{
public:
#ifdef _MAINDLG
	SensorTCP(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port);
#else
	SensorTCP(CMessageQueue * q,std::string can_ip,int32_t can_port,int32_t host_port);
#endif
	bool Start(const char* ip=nullptr);
	void Run();
	void Stop();
	void Write(CIOBuffer * pBuffer);
	T* Get();
#ifndef WIN32
    void SetStartRead(bool b);
	void SetStartWrite(bool b);
#endif
private:
	socket_t _fd;
	std::thread  _thread;
	bool _run;
	std::string _canip;
	int32_t _canport;
	int32_t _hostport;
	std::mutex _lock;
	std::unordered_map<int32_t, cannet_frame> _message;
	std::unique_ptr<T> _sensorNotify;
    sockaddr_in _canaddr;
	int flag;
#ifndef WIN32
	bool _startRead;
	bool _startWrite;
#endif
};
template<typename T>
#ifdef _MAINDLG
SensorTCP<T>::SensorTCP(CThreadWindow* q, std::string can_ip, int32_t can_port, int32_t host_port)
#else
SensorTCP<T>::SensorTCP(CMessageQueue* q, std::string can_ip, int32_t can_port, int32_t host_port)
#endif
{
  	_sensorNotify=std::make_unique<T>(q);
	_canip=can_ip;
	_canport=can_port;
	_hostport=host_port;

	_run = false;
#ifndef WIN32
 _startWrite=_startRead=true;
#endif
}
template<typename T>

bool SensorTCP<T>::Start(const char * ip)
{
#ifdef WIN32
	WSAData data;
	WSAStartup(MAKEWORD(2, 2), &data);
#endif

    std::cout<<"SensorSocket<T>::Start"<<_canip<<","<<_canport<<std::endl;
	//_sensorNotify->SetSensorSocket(this);
	_fd = socket(AF_INET, SOCK_STREAM, 0);
	sockaddr_in sin;
	sin.sin_family = AF_INET;
	sin.sin_port = htons(_canport);
#ifdef WIN32
	sin.sin_addr.s_addr = inet_addr(ip);
#else
	sin.sin_addr.s_addr =inet_addr(_canip.c_str());// htonl(INADDR_ANY);
#endif
	while (::connect(_fd, (sockaddr*)&sin, sizeof(sin)) == -1)
	{
		std::cout<< "connect "<<_canip<<" failed"<<std::endl;
		  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

	}	 

	flag = fcntl(_fd, F_GETFL, 0);
	if (flag < 0)
	{
		printf("fcntl failed.\n");
	}
	flag |= O_NONBLOCK;
	if (fcntl(_fd, F_SETFL, flag) < 0)
	{
		printf("fcntl failed.\n");
	}

	if(_run == false)
		_thread = std::thread(&SensorTCP::Run, this);
	_sensorNotify->Start();
	
	return true;
}
template<typename T>
void SensorTCP<T>::Run()
{
	_run = true;
	struct pollfd fds[1];
	fds[0].fd = _fd;
    fds[0].events = POLLIN;
	
//	long long  k = 0;
	//long long tick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
	while (_run)
	{
#ifdef WEBRTC_LINUX
      //  _sensorNotify->PreProcess();
		if(poll(&fds[0], 1, 1000) > 0)
	   {
		    if (fds[0].revents & POLLIN)
			{
#endif
				CIOBuffer pBuffer;
				sockaddr_in from;
				socklen_t fromlen=sizeof(sockaddr_in);
				int32_t ret = recv(_fd,(char *)pBuffer.Buffer, CIOBuffer::IO_BUFFER_SIZE,0);
				if (ret <= 0||!_run)
				{
					close(_fd);
					this->Start();

	
				}
        
	   			//_sensorNotify->Notify(pBuffer.Buffer,ret);
#ifdef WEBRTC_LINUX
			}
	   }
#endif
 
	}
    std::cout<<"SensorSocket<T>::Run Finished"<<std::endl;
}
template<typename T>
void SensorTCP<T>::Write(CIOBuffer * pBuffer)
{
	
#ifndef WIN32
	if(_startWrite==false) return;
#endif
	socklen_t len=sizeof(_canaddr);
	 
    int ret=::send(_fd,(char *)pBuffer->Buffer,pBuffer->Length,0);
	if(ret<=0)
	{
	     std::cout<<"ret = "<<ret<<" size ="<<pBuffer->Length<<std::endl;
	}
 
}
template<typename T>
void SensorTCP<T>::Stop()
{
	if(!_run) return;
	_sensorNotify->Stop();
	
	_run = false;
#ifdef WIN32
	closesocket(_fd);
#else
	close(_fd);
#endif
    std::cout<<"SensorSocket<T>::Stop"<<std::endl;
	_thread.join();
	std::cout<<"SensorSocket<T>::Stop finished"<<std::endl;
}
#ifndef WIN32
template<typename T>
void SensorTCP<T>::SetStartRead(bool b)
{
	_startRead=b;
}
template<typename T>
void SensorTCP<T>::SetStartWrite(bool b)
{
	_startWrite=b;
}
#endif
template<typename T>
T* SensorTCP<T>::Get()
{
	return _sensorNotify.get();
}


#include "../thirdparty/Mqtt/include/MQTTAsync.h"
#include <string.h>

//20230414 MQTT
template<typename T>
class SensorMQTT
{
public:
	SensorMQTT(CMessageQueue* q, std::string Server_Address, std::string Esn, std::string Password, std::string Clientid);

	bool Start();
	//void Run();
	void Stop();
	void Write(CIOBuffer* pBuffer, const char* pubTopic);
	T* Get();

private:
	MQTTAsync mqttClient;
	//std::thread  _thread;
	std::string _Server_Address;
	std::string _Esn;
	std::string _Password;
	std::string _ClientID;
	//std::mutex _lock;
	//std::unordered_map<int32_t, cannet_frame> _message;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorMQTT<T>::SensorMQTT(CMessageQueue* q, std::string Server_Address, std::string Esn, std::string Password, std::string Clientid)
{
	_sensorNotify = std::make_unique<T>(q);
	_Server_Address = Server_Address;
	_Esn = Esn;
	_Password = Password;
	_ClientID = Clientid;
}

template<typename T>
bool SensorMQTT<T>::Start()
{
	std::cout << "SensorMQTT<T>::Start" << _Server_Address << "," << _Esn << "," << _Password << ","  << _ClientID  <<std::endl;

	int rc = 0;
	MQTTAsync_create(&mqttClient, _Server_Address.c_str(), _ClientID.c_str(), MQTTCLIENT_PERSISTENCE_NONE, NULL);

	_sensorNotify->SetSensorMQTT(mqttClient, _Esn);
	
	MQTTAsync_setCallbacks(mqttClient, NULL, _sensorNotify->Disconnect, _sensorNotify->RecevieMessage, NULL); //�������ӶϿ��ͽ������ݻص�
	
	MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer;  //��ʼ���ṹ��
	conn_opts.cleansession = 1;

	conn_opts.username = _Esn.c_str();
	conn_opts.password = _Password.c_str();
	conn_opts.onFailure = _sensorNotify->onConnectFailure; //����ʧ�ܻص�
	conn_opts.context = mqttClient;
	conn_opts.automaticReconnect = true;    //�����Ͽ��Զ�����
	conn_opts.minRetryInterval = 5; //��С�������ʱ��(��)��ÿ��ʧ���������ʱ�䶼��ӱ�
	conn_opts.maxRetryInterval = 365 * 24 * 60 * 60;    //����������ʱ��(��)
	
	
	MQTTAsync_setConnected(mqttClient, (char *)conn_opts.username , _sensorNotify->onConnectCallCBack);   //�������ӳɹ��ص�,�����ǵ�һ�����ӳɹ����������ɹ�������ô˻ص�
	if ((rc = MQTTAsync_connect(mqttClient, &conn_opts)) != MQTTASYNC_SUCCESS)  //��������
	{
		std::cout << "MQTTAsync_connect() fail, error code: " << rc << std::endl;
	}
	//_thread = std::thread(&SensorMQTT::Run, this);
	//_sensorNotify->Start();
	return true;
}

template<typename T>
void SensorMQTT<T>::Stop()
{
	_sensorNotify->Stop(_Esn.c_str());
	std::cout << "SensorMQTT<T>::Stop" << std::endl;
}

template<typename T>
T* SensorMQTT<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorMQTT<T>::Write(CIOBuffer* pBuffer, const char* pubTopic)
{
	_sensorNotify->sendMessage((char *)pBuffer->Buffer,1,pubTopic);
}

#include <PCANBasic.h>
#include <assert.h>
//20230414 PEAKCAN
template<typename T>
class SensorPeakCan
{
public:
	SensorPeakCan(CMessageQueue* q);

	bool Start();
	void Run();
	void Stop();
	void Write(TPCANMsg* dataMessage);
	T* Get();

private:
	TPCANStatus result;
	TPCANHandle _handle = PCAN_NONEBUS;
	bool _isFD;
	bool _run;

	std::thread  _thread;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorPeakCan<T>::SensorPeakCan(CMessageQueue* q)
{
	_sensorNotify = std::make_unique<T>(q);
}

template<typename T>
bool SensorPeakCan<T>::Start()
{
	std::cout << "SensorPeakCan<T>::Start" << std::endl;
	_sensorNotify->SetSensorSocket(this);

	int iBuffer;
	TPCANHandle _HandlesArray[16];
	char strMsg[256];

	_HandlesArray[0] = PCAN_USBBUS1;
	_HandlesArray[1] = PCAN_USBBUS2;
	_HandlesArray[2] = PCAN_USBBUS3;
	_HandlesArray[3] = PCAN_USBBUS4;
	_HandlesArray[4] = PCAN_USBBUS5;
	_HandlesArray[5] = PCAN_USBBUS6;
	_HandlesArray[6] = PCAN_USBBUS7;
	_HandlesArray[7] = PCAN_USBBUS8;
	_HandlesArray[8] = PCAN_USBBUS9;
	_HandlesArray[9] = PCAN_USBBUS10;
	_HandlesArray[10] = PCAN_USBBUS11;
	_HandlesArray[11] = PCAN_USBBUS12;
	_HandlesArray[12] = PCAN_USBBUS13;
	_HandlesArray[13] = PCAN_USBBUS14;
	_HandlesArray[14] = PCAN_USBBUS15;
	_HandlesArray[15] = PCAN_USBBUS16;
	for (int i = 0; i < (sizeof(_HandlesArray) / sizeof(TPCANHandle)); i++)
	{
		result = CAN_GetValue(_HandlesArray[i], PCAN_CHANNEL_CONDITION, &iBuffer, sizeof(iBuffer));
		if (((result) == PCAN_ERROR_OK) && ((iBuffer & PCAN_CHANNEL_AVAILABLE) == PCAN_CHANNEL_AVAILABLE))
		{
			result = CAN_GetValue((TPCANHandle)_HandlesArray[i], PCAN_CHANNEL_FEATURES, (void*)&iBuffer, sizeof(iBuffer));
			_isFD = (result == PCAN_ERROR_OK) && (iBuffer & FEATURE_FD_CAPABLE);
			_handle = _HandlesArray[i];
			break;
		}
	}
	if (_handle != PCAN_NONEBUS)
	{
		result = ::CAN_Initialize(_handle, PCAN_BAUD_250K);

		if (result == PCAN_ERROR_OK)
		{
			result = CAN_SetValue(_handle, PCAN_BUSOFF_AUTORESET | PCAN_PARAMETER_ON, (void*)&iBuffer, sizeof(iBuffer));
		}
		else
			return false;	
		_run = true;

		_sensorNotify->Start();
		_thread = std::thread(&SensorPeakCan::Run, this);
	}
	return true;
}

template<typename T>
void SensorPeakCan<T>::Stop()
{
	if (!_run) return;
	_sensorNotify->Stop();

	_run = false;
	_thread.join();

	CAN_Uninitialize(_handle);
	std::cout << "SensorPeakCan<T>::Stop" << std::endl;
}

template<typename T>
T* SensorPeakCan<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorPeakCan<T>::Write(TPCANMsg *dataMessage)
{
	/*dataMessage.ID = 0x601;
	dataMessage.MSGTYPE = PCAN_MESSAGE_STANDARD;
	dataMessage.LEN = 0x03;

	memset(dataMessage.DATA,0x00,8);
	dataMessage.DATA[0] = 0xB1;
	dataMessage.DATA[1] = 0x10;
	dataMessage.DATA[2] = 0xFF;*/
	int fd;
	if(CAN_GetValue(_handle, PCAN_RECEIVE_EVENT, &fd, sizeof(fd)) == PCAN_ERROR_OK)
	{
		result = CAN_Write(_handle, dataMessage);
		
		if(result == PCAN_ERROR_OK)
			std::cout << std::hex << dataMessage->ID << std::endl;
		else
			std::cout << std::hex << result << std::endl;
	}
	
}

template<typename T>
void SensorPeakCan<T>::Run()
{
	int fd;
	TPCANMsg CANMsg;
	TPCANTimestamp CANTimeStamp;

	TPCANStatus stsResult = CAN_GetValue(_handle, PCAN_RECEIVE_EVENT, &fd, sizeof(fd));

	if (stsResult != PCAN_ERROR_OK)
	    std::cout << "CAN_GetValue Error" << "\n";
	
	 while (_run)
	{
		/*
		std::this_thread::sleep_for(std::chrono::milliseconds(20));
		TPCANStatus ret = PCAN_ERROR_OK;

		do{
			 result = CAN_Read(_handle, &CANMsg, &CANTimeStamp);
			 if (result != PCAN_ERROR_QRCVEMPTY)
			 	_sensorNotify->Notify(&CANMsg, CANMsg.LEN);
			
			memset(&CANMsg, 0, sizeof(CANMsg));
			memset(&CANTimeStamp, 0, sizeof(CANTimeStamp));
		}
		while(result & PCAN_ERROR_QRCVEMPTY);
		*/
		
	    struct timeval timeout = {0,0};
	    timeout.tv_usec = 20000; // 20ms

	    fd_set fds;
	    FD_ZERO(&fds);
	    FD_SET(fd, &fds);

	    int err = select(fd + 1, &fds, NULL, NULL, &timeout);
	    if (err != -1 && FD_ISSET(fd, &fds)) 
	    {
	        result = CAN_Read(_handle, &CANMsg, &CANTimeStamp);
	        if (result != PCAN_ERROR_QRCVEMPTY)
	        {
				_sensorNotify->Notify(&CANMsg, CANMsg.LEN);

				memset(&CANMsg, 0, sizeof(CANMsg));
				memset(&CANTimeStamp, 0, sizeof(CANTimeStamp));
	        }
	    }
		
	}
}

#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>


//20231128 CANBUS
template<typename T>
class SensorCanBus
{
public:
	SensorCanBus(CMessageQueue* q,std::string CanName);

	bool Start();
	void Run();
	void Stop();
	void Write(can_frame *date);
	T* Get();

	int sockfd;

private:
	
	bool _run;

	struct ifreq ifr;
    struct sockaddr_can can_addr;
	int ret;
	int nbytes;
	struct can_frame Reciveframe;

	std::string _CanName;

	std::thread  _thread;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorCanBus<T>::SensorCanBus(CMessageQueue* q,std::string CanName)
{
	_sensorNotify = std::make_unique<T>(q);
	_CanName = CanName;

	sockfd = -1;
	memset(&ifr,0,sizeof(ifr));
	memset(&can_addr,0,sizeof(can_addr));
	memset(&Reciveframe,0,sizeof(Reciveframe));
	_run = false;
}

template<typename T>
bool SensorCanBus<T>::Start()
{
	std::cout << "SensorCanBus<T>::Start" << std::endl;
	_sensorNotify->SetCanBusSensor(this);

	sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if(sockfd < 0) 
	{
		std::cout << "SensorCanBus Socket Error" << std::endl;
	}
    
	memcpy(ifr.ifr_name,_CanName.c_str(),_CanName.length());
	//std::cout << ifr.ifr_name << std::endl;
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    ret = bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr));
    if (ret < 0) 
    {
		std::cout << "SensorCanBus Bind Error" << std::endl;
        close(sockfd);
		return false;
	}

	int loopback = 0;
    setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback));
    int ro = 1; 
    setsockopt(sockfd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &ro, sizeof(ro));
	
	int flag = fcntl(sockfd, F_GETFL, 0);
    if (flag < 0) 
	{
		std::cout << "fcntl F_GETFL fail" << std::endl;
    }

    if (fcntl(sockfd, F_SETFL, flag | O_NONBLOCK) < 0)
	{
		std::cout << "fcntl F_SETFL fail" << std::endl;
	}
	_run = true;

	_sensorNotify->Start();
	_thread = std::thread(&SensorCanBus::Run, this);
	return true;
}

template<typename T>
void SensorCanBus<T>::Stop()
{
	if (!_run) return;
	_sensorNotify->Stop();

	_run = false;
	_thread.join();

	close(sockfd);
	std::cout << "SensorCanBus<T>::Stop" << std::endl;
}

template<typename T>
T* SensorCanBus<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorCanBus<T>::Write(can_frame *date)
{
	ret = write(sockfd, date, sizeof(can_frame)); 

    if(sizeof(can_frame) != ret) 
	{
		printf("SensorCanBus write error:%d\r\n",ret);
	}
		
}

template<typename T>
void SensorCanBus<T>::Run()
{
	//struct pollfd fds[1];
	//fds[0].fd = sockfd;
    //fds[0].events = POLLIN;
	
	
	while (_run)
	{
		fd_set fds;
		struct timeval timeout = {0,0};
	    timeout.tv_usec = 20000; 

        
	    FD_ZERO(&fds);
	    FD_SET(sockfd, &fds);
		
	    int err = select(sockfd + 1, &fds, NULL, NULL, &timeout);
	    if (err != -1 && FD_ISSET(sockfd, &fds)) 
	   	{
            nbytes = read(sockfd, &Reciveframe, sizeof(Reciveframe));
        
            if(nbytes > 0)
			{
				_sensorNotify->Notify(&Reciveframe);
				//printf("CAN frame:\nID = %x\nDLC = %x\nDATA = %s\n", Reciveframe.can_id,Reciveframe.can_dlc, Reciveframe.data);
			}
	   	}
	}
}


#include <termios.h>
//20231204 RS485
template<typename T>
class SensorSerial 
{
public:
	SensorSerial(CMessageQueue* q,std::string CanName,uint32_t speed);

	bool Start();
	void Run();
	void Stop();
	void Write(uint8_t * pBuffer,uint8_t Length);
	T* Get();

	int Serialfd;

private:
	
	bool _run;
	int nbytes;
	std::string _CanName;
	uint32_t _speed;

	std::thread  _thread;
	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorSerial<T>::SensorSerial(CMessageQueue* q,std::string CanName,uint32_t speed)
{
	_sensorNotify = std::make_unique<T>(q);
	_run = false;

	_CanName = CanName;
	_speed = speed;
	Serialfd = -1;
}

template<typename T>
bool SensorSerial<T>::Start()
{
	std::cout << "SensorSerial<T>::Start" << std::endl;
	_sensorNotify->SetSerialSensor(this);

	Serialfd = open((const char *)_CanName.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (-1 == Serialfd)
	{
		printf("Can't Open Serial Port\r\n");
		return false;   
	}
        
    if(fcntl(Serialfd, F_SETFL, O_NONBLOCK) < 0)
	{
		printf("Unable set to NONBLOCK \r\n");
		return false;   
	}

	 struct termios Opt;
	 tcgetattr(Serialfd, &Opt);

	int speed_arr[] = { B38400, B19200, B9600, B4800, B2400, B1200, B300, \
                    B38400, B19200, B9600, B4800, B2400, B1200, B300, };
	int name_arr[] = { 38400, 19200, 9600, 4800, 2400, 1200, 300, 38400, \
                   19200, 9600, 4800, 2400, 1200, 300, };

	for (int i=0; i<sizeof(speed_arr)/sizeof(int); i++)
	{
		 if (_speed == name_arr[i])
		 {
            cfsetispeed(&Opt, speed_arr[i]);  
            cfsetospeed(&Opt, speed_arr[i]);   
			break;
		 }
	}

	Opt.c_cflag &= ~CSIZE;
	Opt.c_cflag |= CS8;
	Opt.c_cflag &= ~PARENB;   // Clear parity enable
	Opt.c_cflag &= ~CSTOPB;  

	Opt.c_iflag &= ~(IXON | IXOFF | IXANY);
    Opt.c_oflag &= ~OPOST; 

	Opt.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); 
    Opt.c_cc[VMIN] = 0;  
    Opt.c_cc[VTIME] = 0; 

	tcflush (Serialfd, TCIFLUSH);

	if(tcsetattr(Serialfd, TCSANOW, &Opt))
		printf("Serial succuss!\r\n"); 

	_run = true;

	_sensorNotify->Start();
	_thread = std::thread(&SensorSerial::Run, this);
	return true;
}

template<typename T>
T* SensorSerial<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorSerial<T>::Write(uint8_t * pBuffer,uint8_t Length)
{
	int ret = write(Serialfd,(const char *)pBuffer,Length);
}

template<typename T>
void SensorSerial<T>::Run()
{
	uint8_t pBuffer[128];

	while (_run)
	{
		struct timeval timeout = {0,0};
	    timeout.tv_usec = 20000; 

        fd_set fds;
	    FD_ZERO(&fds);
	    FD_SET(Serialfd, &fds);

	    int err = select(Serialfd + 1, &fds, NULL, NULL, &timeout);
	    if (err != -1 && FD_ISSET(Serialfd, &fds)) 
	    {
			memset(pBuffer,0,128);
            nbytes = read(Serialfd,pBuffer,128);
        
            if(nbytes > 0)
			{
				_sensorNotify->Notify(pBuffer,nbytes);
			}   
        }
	}
}

template<typename T>
void SensorSerial<T>::Stop()
{
	if (!_run) return;
	_sensorNotify->Stop();

	_run = false;
	_thread.join();

	close(Serialfd);
	std::cout << "SensorSerial<T>::Stop" << std::endl;
}


#include "modbus/modbus.h"
//20231205 modbus tcp or rtu
template<typename T>
class SensorModbusTcp 
{
public:
	SensorModbusTcp(CMessageQueue* q,std::string ip,uint32_t port,uint32_t slaveID[16],uint32_t type);//0.tcp.1rtu

	bool Start();
	void Stop();
	void Write(uint8_t * pBuffer,uint8_t Length);
	void read_input_registers(uint16_t addr,uint16_t nb);//0x04
	void read_registers(uint16_t addr,uint16_t nb);//0x03
	T* Get();

	modbus_t *_modbusclient;
	uint32_t mode_type;

private:
	
	bool _run;
	int nbytes;
	std::string _ServerIp;
	uint32_t _ServerPort;
	uint32_t _SlaveID[16];

	std::unique_ptr<T> _sensorNotify;

};

template<typename T>
SensorModbusTcp<T>::SensorModbusTcp(CMessageQueue* q,std::string ip,uint32_t port,uint32_t slaveID[16],uint32_t type)
{
	_sensorNotify = std::make_unique<T>(q);
	_run = false;

	_ServerIp = ip;
	_ServerPort = port;
	//_SlaveID = slaveID;
	memset(_SlaveID,0,16);
	memcpy(_SlaveID,slaveID,16);

	_modbusclient = nullptr;
	mode_type = type;
}

template<typename T>
T* SensorModbusTcp<T>::Get()
{
	return _sensorNotify.get();
}

template<typename T>
void SensorModbusTcp<T>::Write(uint8_t * pBuffer,uint8_t Length)
{
	
}

template<typename T>
void SensorModbusTcp<T>::read_input_registers(uint16_t addr,uint16_t nb)//0x04
{
    uint16_t Receive[128];
	memset(Receive,0,128);

	modbus_set_slave(_modbusclient, _SlaveID[0]);
	int ret_id1 = modbus_read_input_registers(_modbusclient,addr,nb,Receive);

	modbus_set_slave(_modbusclient, _SlaveID[1]);
	int ret_id2 = modbus_read_input_registers(_modbusclient,addr,nb,Receive + ret_id1);

	/*
	int ret = ret_id1 + ret_id2;
	uint8_t HexData[256];
	memset(HexData,0,256);
	for(int i = 0 ; i < ret ; i+=2)
	{
		HexData[i + 1] = (Receive[i] & 0x00ff);
		HexData[i] = ((Receive[i] & 0xff00) >> 8);
	}
	*/
	
	_sensorNotify->Notify(Receive);
}

template<typename T>
void SensorModbusTcp<T>::read_registers(uint16_t addr,uint16_t nb)//0x03
{
    uint16_t Receive[128],Buff[128];
	memset(Receive,0,128);
	memset(Buff,0,128);

	modbus_set_slave(_modbusclient, _SlaveID[0]);
	modbus_read_registers(_modbusclient,addr,nb,Receive);
	Buff[0] = Receive[1];

	modbus_set_slave(_modbusclient, _SlaveID[1]);
	modbus_read_registers(_modbusclient,addr,nb,Receive);
	Buff[1] = Receive[1];

	/*
	int ret = ret_id1 + ret_id2;
	uint8_t HexData[256];
	memset(HexData,0,256);
	for(int i = 0 ; i < ret ; i+=2)
	{
		HexData[i + 1] = (Receive[i] & 0x00ff);
		HexData[i] = ((Receive[i] & 0xff00) >> 8);
	}
	*/
	
	
	_sensorNotify->Notify(Buff);
}

template<typename T>
bool SensorModbusTcp<T>::Start()
{
	std::cout << "SensorModbusTcp<T>::Start" << std::endl;
	_sensorNotify->SetModbusTcpSensor(this);
	
	if(!mode_type)
		_modbusclient = modbus_new_tcp(_ServerIp.c_str(), _ServerPort);
	else
		_modbusclient = modbus_new_rtu(_ServerIp.c_str(), _ServerPort,'N',8,1);//modbus_new_rtu("/dev/ttySP1", 115200, 'N', 8, 1);

	if(_modbusclient)
	{
		modbus_set_debug(_modbusclient,1);

		if (!modbus_connect(_modbusclient))
		{
			std::cout << "connect Slave ok" << std::endl;
			//modbus_set_response_timeout(mb, 1, 200000);
			_run = true;
			_sensorNotify->Start();

			return true;
		} 	
		else
		{
			modbus_free(_modbusclient);
			_modbusclient = nullptr;
			return false;
		}
	}
	else
		return false;
}

template<typename T>
void SensorModbusTcp<T>::Stop()
{
	if (!_run) return;
	_sensorNotify->Stop();

	_run = false;

	if(_modbusclient)
	{
		modbus_close(_modbusclient);
    	modbus_free(_modbusclient);
	}
	_modbusclient = nullptr;

	std::cout << "SensorModbusTcp<T>::Stop" << std::endl;
}