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

 
#include <stdio.h>
#include <iostream>
#include "../common/comm.h"
#include "../common/iobuffer.h"
#include "api.h"
#include "protocol.pb.h"
#include "message_queue.h"
#include <signal.h>
#include <cstring>
#include <stdlib.h>

#include <fcntl.h> 
#include <sys/types.h> 
#include <sys/stat.h> 
//#include "modbus/modbus-tcp.h"



/*
#include <unistd.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
*/
//#define command "echo \"nvidia\" | sudo -S /sbin/ip link set can0 type can bitrate 500000"
//#define up "echo \"nvidia\" | sudo -S ifconfig can0 up"
//#define down "echo \"nvidia\" | sudo -S ifconfig can0 down"

/*
extern "C"
{
#include "/usr/local/ffmpeg/include/libavcodec/avcodec.h"

//#include "/usr/local/ffmpeg/include/libavutil/time.h"
//#include "/usr/local/ffmpeg/include/libavutil/opt.h"
//#include "/usr/local/ffmpeg/include/libavutil/imgutils.h"
//#include "/usr/local/x264/include/x264.h"
}
*/

//#include <sensors/sensors.h>
//#include <PCANBasic.h>

//20230413 MQTT
//#include "../thirdparty/Mqtt/include/MQTTAsync.h"
//#include "../thirdparty/jsoncpp/include/json/json.h"
//
//MQTTAsync mqttClient;
////static const char* subTopic = "hello1";  //订阅主题
//
//
//void sendMessage(char* data, int qos ,const char * pubTopic)
//{
//    int rc;
//    MQTTAsync_message pubmsg = MQTTAsync_message_initializer;
//    MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer;
//
//    opts.context = mqttClient;
//
//    pubmsg.payload = data;
//    pubmsg.payloadlen = strlen(data);
//    pubmsg.qos = qos;
//    printf("mqtt send: %s\n", (char*)pubmsg.payload);
// 
//    if ((rc = MQTTAsync_sendMessage(mqttClient, pubTopic, &pubmsg, &opts)) != MQTTASYNC_SUCCESS)
//    {
//        printf("MQTTAsync_sendMessage() fail, error code:%d\n", rc);
//    }
//}
//
//void onConnectCallCBack(void* context, char* cause) //连接成功callback
//{
//    //printf("Connection successful.\n");
//    //int rc, qos;
//    //MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer;
//
//    //qos = 0;
//    //opts.context = mqttClient;
//
//    //if ((rc = MQTTAsync_subscribe(mqttClient, subTopic, qos, &opts)) != MQTTASYNC_SUCCESS) //subTopic
//    //{
//    //    printf("MQTTAsync_subscribe() fail, error code:%d\n", rc);
//    //}
//
//    printf("Connection successful.\n");
//    int rc;
//    const int qos[2] = {0,1};
//    MQTTAsync_responseOptions opts = MQTTAsync_responseOptions_initializer;
//
//    char* const subTopic[2] = { "hello" ,"hello1"};  //订阅主题
//
//    opts.context = mqttClient;
//
//    if ((rc = MQTTAsync_subscribeMany(mqttClient,2,subTopic, qos, &opts)) != MQTTASYNC_SUCCESS) //subTopic
//    {
//        printf("MQTTAsync_subscribe() fail, error code:%d\n", rc);
//    }
//}
//
//void onConnectFailure(void* context, MQTTAsync_failureData* response) //连接失败callback
//{
//    printf("connection fail, error code: %d, error message:%s\n", response ? response->code : 0, response ? response->message : 0);
//}
//
//void Disconnect(void* context, char* cause) //连接断开callback
//{
//    printf("disconnect, cause: %s\n", cause);
//}
//
//int  RecevieMessage(void* context, char* pubTopic, int topicLen, MQTTAsync_message* message)
//{
//    char buf[message->payloadlen + 1];
//
//    memcpy(buf, message->payload, message->payloadlen);
//    printf("mqtt recv: %s\n", buf);
//    MQTTAsync_freeMessage(&message);
//    MQTTAsync_free(pubTopic);
//
//    return 1;
//}


void daemonize(void) {
  signal(SIGTTOU, SIG_IGN);
  signal(SIGTTIN, SIG_IGN);
  signal(SIGTSTP, SIG_IGN);
  if (0 != fork()) exit(0);
  if (-1 == setsid()) exit(0);
  signal(SIGHUP, SIG_IGN);
  if (0 != fork()) exit(0);
  //if (0 != chdir("/")) exit(0);
}
 
bool checkOnly()
{
    const char filename[]  = "./lockfile";
    int fd = open (filename, O_WRONLY | O_CREAT , 0644);
    int flock = lockf(fd, F_TLOCK, 0 );
    if (fd == -1) {
        return false;
    }
    //给文件加锁
    if (flock == -1) {
        return false;
    }
    //程序退出后，文件自动解锁
    return true;
}
std::string getpath()
{
	  char exec_name [BUFSIZ];
	 readlink ("/proc/self/exe", exec_name, BUFSIZ);
	 int32_t len=strlen(exec_name);
     for(int i=len;i>=0;i--)
     {
         if(exec_name[i]=='/')
           {
               exec_name[i]=0;
               break;
           }
     }
	 return std::string(exec_name);
}


//const char *inFileName  = "/home/zhouchao/2.yuv";
//const char *outFileName  = "/home/zhouchao/3.h264";
/*
int encode(AVCodecContext* codecContent, AVPacket* packet, AVFrame* frame, FILE* outFile)
{
	//编码
	int ret = avcodec_send_frame(codecContent, frame);
	if (ret < 0)
		printf("Error sending a frame for encoding\r\n");
 
	while (ret == 0)
	{
 
		ret = avcodec_receive_packet(codecContent, packet);
 
		if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
			return 0;
		}
		else if (ret < 0)
			printf("Error encoding video frame\r\n");
 
		if (ret == 0)
			fwrite(packet->data, 1, packet->size, outFile);
	}
}
*/


int main(int argc,char *argv[])
{

  //  daemonize();
    auto path=getpath();
    std::cout<<path<<std::endl;
    if(-1==chdir(path.c_str())) return 0;
    
    /*
    if(checkOnly()==false) 
    {
        std::cout<<"进程已经在运行"<<std::endl;
        return 0;
    }*/
   
    std::cout<<"进程启动..."<<std::endl;

    //system("echo \"nvidia\" | sudo -S modprobe can");
    //system("echo \"nvidia\" | sudo -S modprobe can_raw");
    //system("echo \"nvidia\" | sudo -S modprobe mttcan");
    
    //20230425
   /*TPCANStatus result;
   char strMsg[256];

   result = CAN_Initialize(PCAN_USBBUS1, PCAN_BAUD_500K);

   if (result != PCAN_ERROR_OK)
   {
       CAN_GetErrorText(result, 0, strMsg);
       std::cout << strMsg << std::endl;
   }
   else
       std::cout << "PCAN-USB (Ch-1) was initialized" << std::endl;*/


   //TPCANMsg dataMessage;
   //dataMessage.ID = 0x601;
   //dataMessage.MSGTYPE = PCAN_MESSAGE_STANDARD;
   //dataMessage.LEN = 0x03;

   //memset(dataMessage.DATA,0x00,8);
   ////memcpy(dataMessage.DATA, date, dataMessage.LEN);
   //dataMessage.DATA[0] = 0xB1;
   //dataMessage.DATA[1] = 0x10;
   //dataMessage.DATA[2] = 0xFF;
   //result = CAN_Write(PCAN_USBBUS1, &dataMessage);

   //int fd;
   //TPCANMsg CANMsg;
   //TPCANTimestamp CANTimeStamp;

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

   //if (stsResult != PCAN_ERROR_OK)
   //{
   //    std::cout << "CAN_GetValue Error" << "\n";
   //}

   // while (true)
   //{
   //    struct timeval timeout = {};
   //    timeout.tv_usec = 50000; // 50ms

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

   //    //Checks for messages when an event is received
   //    int err = select(fd + 1, &fds, NULL, NULL, &timeout);
   //    if (err != -1 && FD_ISSET(fd, &fds)) 
   //    {
   //        result = CAN_Read(PCAN_USBBUS1, &CANMsg, &CANTimeStamp);
   //        if (result != PCAN_ERROR_QRCVEMPTY)
   //        {
   //            //char Dst[128] = { 0 };
   //            //memcpy(Dst, CANMsg.DATA, CANMsg.LEN);
   //            //std::cout  << Dst << "\n";
   //            std::cout << "111" << "\n";
   //        }
   //    }
   //}
   //CAN_Uninitialize(PCAN_USBBUS1);



    //20230413 HMAC-SHA256签名加密
 //   uint8_t outdata[128] = {0};
	//uint8_t md[SHA256_DIGESTLEN] = {0};
	//uint8_t secret[] = {"hallowordjdadwfxfalmfjwgrjlqsjdwg"};//密钥
	//uint8_t indata[] = {"halloword"};//数据
	//int len1 = strlen((char *)secret);
	//int len2 = strlen((char *)indata);
	//
	//HMAC_SHA256_CTX hmac;
	//hmac_sha256_init(&hmac, secret, len1);
	//hmac_sha256_update(&hmac, indata, len2);
	//hmac_sha256_final(&hmac, md);

	//memcpy(outdata, md, SHA256_DIGESTLEN);


	//for(int i = 0; i < SHA256_DIGESTLEN; i++)
	//{
	//	printf("%02x",outdata[i]);
	//}
	//printf("\n");
    
    
    /*
    //20230413 json
    Json::Value root;
   Json::Value Source;
   Json::FastWriter writer;

   Source["deviceEsn"] = "";
   Source["cloudEsn"] = "";
   Source["sendTime"] = "time";
   Source["ack"] = false;
   Source["seqNum"] = "";

   Json::Value GraspStatus;
   GraspStatus["graspEsn"] = Json::Value("graspEsn");
   GraspStatus["status"] = Json::Value("status");
   GraspStatus["timeStamp"] = Json::Value("timeStamp");

   Source["graspStatusList"].append(GraspStatus);
   //root.append(Source);

   Json::StyledWriter sw;
   std::cout << sw.write(Source) << std::endl << std::endl;
   //std::string data = writer.write(root);
   //std::cout << data << std::endl;   
    */
    /*struct timeval tv;
    gettimeofday(&tv, NULL);

    printf("millisecond: %ld\n", tv.tv_sec * 1000 + tv.tv_usec / 1000);*/
   

   
   //20230413 mqtt
   //int rc;
   //const char* serverUrl = "tcp://localhost:1883";  //服务器地址
   //const char* clientId = "publish_client";  //客户端标识符
   //const char* userName = "";  //用户名
   //const char* password = "";  //密码
   //MQTTAsync_create(&mqttClient, serverUrl, clientId, MQTTCLIENT_PERSISTENCE_NONE, NULL);

   //MQTTAsync_setCallbacks(mqttClient, NULL, Disconnect, RecevieMessage, NULL); //设置连接断开和接受数据回调
   //MQTTAsync_connectOptions conn_opts = MQTTAsync_connectOptions_initializer;  //初始化结构体
   //conn_opts.cleansession = 1;
   //conn_opts.username = userName;
   //conn_opts.password = password;
  
   //conn_opts.onFailure = onConnectFailure; //连接失败回调
   //conn_opts.context = mqttClient;
   //conn_opts.automaticReconnect = true;    //开启断开自动重连
   //conn_opts.minRetryInterval = 5; //最小重连间隔时间(秒)，每次失败重连间隔时间都会加倍
   //conn_opts.maxRetryInterval = 365 * 24 * 60 * 60;    //最大重连间隔时间(秒)
   //MQTTAsync_setConnected(mqttClient, NULL, onConnectCallCBack);   //设置连接成功回调,不管是第一次连接成功还是重连成功都会调用此回调
   //if ((rc = MQTTAsync_connect(mqttClient, &conn_opts)) != MQTTASYNC_SUCCESS)  //尝试连接
   //{
   //    printf("MQTTAsync_connect() fail, error code:%d\n", rc);
   //}
    /*
   double temp=0.f;
      
   auto err=sensors_init(NULL);
   if(err==0)
   {
        const sensors_chip_name *chip;
        int chip_nr=0;
        int i=0;
    
        const sensors_feature * feature=nullptr;
        while ((chip = sensors_get_detected_chips(NULL, &chip_nr))) 
        {
            if ((feature = sensors_get_features(chip, &i))) 
            {
                if (feature->type==SENSORS_FEATURE_TEMP)
                {
                    auto sf = sensors_get_subfeature(chip, feature,SENSORS_SUBFEATURE_TEMP_INPUT);
                    if(sf->type == SENSORS_SUBFEATURE_TEMP_INPUT)
                    {
                        sensors_get_value(chip, sf->number,&temp);
                        //temp = temp * (9.0F / 5.0F) + 32.0F;
                        std::cout << "System_Temp : " << temp << std::endl;
                    }
                }       
            }

        }
    }m
    sensors_cleanup();
*/
    //20231106-YUV转h.264
    
    /*
	AVCodec* codec = NULL;
	//AVCodecContext* codecContent = NULL;
	//AVPacket* packet = NULL;
	//AVFrame* frame = NULL;

    FILE* inFile = nullptr;
	FILE* outFile = nullptr;


    //查找指定编码器

    codec = avcodec_find_encoder(AV_CODEC_ID_H264);
	if (codec == NULL)
		printf("could not find h264 encoder!\r\n");
    else
        printf("could find h264 encoder!\r\n");
       */ 
    /*
    //申请编码器上下文
	codecContent = avcodec_alloc_context3(codec);
	if (codecContent == nullptr)
		printf("could not alloc h264 content!\r\n");

    //必设参数
	codecContent->width = 1280;
	codecContent->height = 720;
	codecContent->time_base = AVRational{ 1, 25 };
    codecContent->max_b_frames = 1;
	codecContent->pix_fmt = AV_PIX_FMT_YUV420P;
	codecContent->gop_size = 10; //关键帧间隔，默认250
	codecContent->framerate = AVRational{ 25, 1 };
 
	//初始化编码器上下文
	ret = avcodec_open2(codecContent, codec, NULL);
	if (ret < 0) 
		printf("Could not open codec: \r\n");

    packet = av_packet_alloc();
	if (packet == nullptr)
		printf("alloc packet error\r\n");
 
	frame = av_frame_alloc();
	if (packet == nullptr)
		printf("alloc frame error\r\n");

    //必设参数
	frame->width = codecContent->width;
	frame->height = codecContent->height;
	frame->format = codecContent->pix_fmt;

    //申请视频数据存储空间
	ret = av_frame_get_buffer(frame, 0);
	if (ret)
		printf("alloc frame buffer error!\r\n");

    inFile = fopen(inFileName, "rb");
	if (inFile == nullptr)
		printf("error to open inFile: %s\r\n", inFileName);

    outFile = fopen(outFileName, "wb+");
	if (inFile == nullptr)
		printf("error to open outFile: %s\n", outFileName);

    int framecount = 0;
	frame->pts = 0;
 
	int start_time = av_gettime() / 1000; //毫秒级
    while (!feof(inFile))
	{
		ret = av_frame_is_writable(frame);
		if (ret < 0)
			ret = av_frame_make_writable(frame);
 
 
		fread(frame->data[0], 1, frame->width * frame->height, inFile); //y
		fread(frame->data[1], 1, frame->width * frame->height / 4, inFile); //u
		fread(frame->data[2], 1, frame->width * frame->height / 4, inFile);  //v
 
		printf("encode frame num: %d\n", ++framecount);
 
 
		frame->pts += 1000 / (codecContent->time_base.den / codecContent->time_base.num);
		encode(codecContent, packet, frame, outFile);
 
	}
    
    encode(codecContent, packet, nullptr, outFile);
	printf("encode time cost: %d ms\n ", av_gettime() / 1000 - start_time);
 
	av_packet_free(&packet);
	av_frame_free(&frame);
	avcodec_free_context(&codecContent);
	fclose(inFile);
	fclose(outFile);
    */
    
    //20231127 can
    //system(down);
    //system(command);
    //system(up);
    /*
    struct ifreq ifr = {0};
    struct sockaddr_can can_addr = {0};
    struct can_frame Sendframe = {0},Reciveframe = {0};
    int sockfd = -1;
    int ret,nbytes = 0,len = 0;

    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if(0 > sockfd) 
        printf("socket error\r\n");

    strcpy(ifr.ifr_name, "can0");
    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 (0 > ret) 
    {
        printf("bind error\r\n");
        close(sockfd);
    }

    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));

    Sendframe.data[0] = 0xB1;
    Sendframe.data[1] = 0x10;
    Sendframe.data[2] = 0xFF;
    Sendframe.can_dlc = 3;
    Sendframe.can_id = 0x601;

    ret = write(sockfd, &Sendframe, sizeof(Sendframe)); 

    if(sizeof(Sendframe) != ret) 
         printf("write error\r\n");

    int flag = fcntl(sockfd, F_GETFL, 0);
    if (flag < 0) {
        printf("fcntl F_GETFL fail\r\n");
    }

    if (fcntl(sockfd, F_SETFL, flag | O_NONBLOCK) < 0)
        printf("fcntl F_SETFL fail\r\n");

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

        fd_set fds;
	    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)
                    printf("CAN frame:\nID = %x\nDLC = %x\nDATA = %s\n", Reciveframe.can_id,Reciveframe.can_dlc, Reciveframe.data);
        }
        else
           std::cout << "111" << std::endl;
    }

    close(sockfd);
    */
    /*
    int fd;
    fd = open("/home/nvidia/Log.txt",O_CREAT | O_RDWR,0777);
    if(fd < 0)
        std::cout << "open /home/nvidia/Log.txt error" << std::endl;
    else
        std::cout << "open /home/nvidia/Log.txt suss" << std::endl;

    
    write(fd,"444",3);
    close(fd);
    */

   //20231127 modbus tcp
   /*
    modbus_t *mb;
    uint8_t dest[1024]; //setup memory for data
    uint16_t * dest16 = (uint16_t *) dest;

    mb = modbus_new_tcp("127.0.0.1", 502); 
    int ret = modbus_set_slave(mb, 1);

    ret = modbus_set_debug(mb,1);

    if(!ret)
        std::cout << "Set Slave ID ok" << std::endl;

    if (modbus_connect(mb) == -1) 
        modbus_free(mb);
    else
    {
        std::cout << "connect Slave ok" << std::endl;
        ret = modbus_set_response_timeout(mb, 1, 200000);

        modbus_read_input_registers(mb,0x10A,1,dest16);
        modbus_close(mb);
        modbus_free(mb);
    }
    */  

    CMessageQueue Q;
    Q.Create();
	while(true)
    {
		Q.Process();
	}
    //gtk_main();
     
    return 0;
}