Vehicle-cpp/remote_drive/EgoZJ/webrtcinterop/gsml_capturer.cpp

#include "pch.h"
#include "../common/comm.h"
#include "./include/api.h"

#include "lock.h"
#include <iostream>
#include <linux/videodev2.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <string>
#include "capture_op.h"
#include "gsml_capturer.h"

rtc::scoped_refptr<webrtc::VideoTrackSourceInterface> OpenGSMLCapture(CaptureOp* op,int32_t GSML) 
{
	 auto  video_capture=std::make_unique<GSMLCapturer>(op);
	 video_capture->Start(GSML); 
     rtc::scoped_refptr<GSMLTrackSource> video_source = GSMLTrackSource::Create(std::move(video_capture));
     return video_source;
}

GSMLCapturer::GSMLCapturer(CaptureOp* lhs):_op(lhs) 
{
	 
}
void GSMLCapturer::Start(int32_t GSML)
{
    count = 0;
    CameraRecord = GSML;
	_thread = std::thread(std::bind(&GSMLCapturer::Run, this));
}
bool GSMLCapturer::open_cam()
{

    // int32_t count=_op->GetType()==RenderPosition::FRONT_BACK?2:1;
     _op->_ctx0=(context_t *)malloc(sizeof(context_t));
    //for(int i=0;i<count;i++)
	{
        context_t * p=_op->_ctx0;
        
        _op->_ctx0->m_Gsml_Info.m_device = 10;
        _op->_ctx0->m_Gsml_Info.m_record = 0;
        _op->_ctx0->m_Gsml_Info.m_time = 0;

	    p->cam_fd=-1;
	 
         p->cam_pixfmt = V4L2_PIX_FMT_YUYV;
         //p->cam_w = 1280;
         //p->cam_h = 720;
         p->cam_w = 1920;
         p->cam_h = 1080;
         p->g_buff = NULL;
         p->capture_dmabuf = false;  // opencv display v4l2 can't be true
         p->fps = 30;

         p->enable_verbose = false;

        std::string devname="/dev/video" + std::to_string(_op->GetIndex());
       // std::cout<<"设备:"<<devname<<std::endl;
        //ctx.cam_devname=devname+std::to_string();
        p->cam_fd = open(devname.c_str(), O_RDWR | O_NONBLOCK);
        if( p->cam_fd==-1) 
        {
             ERROR_RETURN("Failed to open camera device %s: %s (%d)",
                devname.c_str(), strerror(errno), errno);
        }
        /*
        struct v4l2_fmtdesc fmtdesc;

        fmtdesc.index = 0;
        fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        while (0 == ioctl(p->cam_fd, VIDIOC_ENUM_FMT, &fmtdesc)) {
            printf("fmt: %s <0x%x>\n", fmtdesc.description, fmtdesc.pixelformat);
            fmtdesc.index++;

        }*/
        /*
        struct v4l2_selection crop;	
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.target = V4L2_SEL_TGT_CROP_BOUNDS;
        ioctl(p->cam_fd, VIDIOC_G_SELECTION, &crop);
        
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.target = V4L2_SEL_TGT_CROP;
        crop.r.left = 0;
        crop.r.top = 0;
        crop.r.width = p->cam_w;  
        crop.r.height = p->cam_h;
        ioctl(p->cam_fd, VIDIOC_S_SELECTION, &crop);

        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.target = V4L2_SEL_TGT_COMPOSE;
        crop.r.left = 0;
        crop.r.top = 0;
        crop.r.width = 1280; 
        crop.r.height = 720;*/

        
        
        struct v4l2_format fmt;
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width =  p->cam_w;
        fmt.fmt.pix.height =  p->cam_h;
        fmt.fmt.pix.pixelformat =  p->cam_pixfmt;
        fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
        if (ioctl( p->cam_fd, VIDIOC_S_FMT, &fmt) < 0)
            ERROR_RETURN("Failed to set camera output format: %s (%d)",
                    strerror(errno), errno);
        /* Get the real format in case the desired is not supported */
        memset(&fmt, 0, sizeof fmt);
        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (ioctl( p->cam_fd, VIDIOC_G_FMT, &fmt) < 0)
            ERROR_RETURN("Failed to get camera output format: %s (%d)",
                    strerror(errno), errno);
        if (fmt.fmt.pix.width !=  p->cam_w ||
                fmt.fmt.pix.height !=  p->cam_h ||
                fmt.fmt.pix.pixelformat !=  p->cam_pixfmt)
        {
            WARN("The desired format is not supported");
             p->cam_w = fmt.fmt.pix.width;
             p->cam_h = fmt.fmt.pix.height;
             p->cam_pixfmt =fmt.fmt.pix.pixelformat;
        }

        struct v4l2_streamparm streamparm;
        memset (&streamparm, 0x00, sizeof (struct v4l2_streamparm));
        streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        /*
        streamparm.parm.capture.timeperframe.numerator = 1;
        streamparm.parm.capture.timeperframe.denominator = 3;
        streamparm.parm.output.timeperframe.numerator = 1;
        streamparm.parm.output.timeperframe.denominator = 3;
        */

        ioctl ( p->cam_fd, VIDIOC_G_PARM, &streamparm);

        printf(">>: Frame rate: %u/%u\n",streamparm.parm.capture.timeperframe.numerator,streamparm.parm.capture.timeperframe.denominator);

        INFO("Camera ouput format: (%d x %d)  stride: %d, imagesize: %d, frate: %u / %u",
                fmt.fmt.pix.width,
                fmt.fmt.pix.height,
                fmt.fmt.pix.bytesperline,
                fmt.fmt.pix.sizeimage,
                streamparm.parm.capture.timeperframe.denominator,
                streamparm.parm.capture.timeperframe.numerator);
    }
        
    return true;
}

void GSMLCapturer::Run()
{
    if(!open_cam()) return;
	prepare_buffer();
	start_streams();
	_run=true;
	struct pollfd fds[1];
    

	struct v4l2_buffer v4l2_buf;
	while(_run)
	{	
		int cam_fd=-1;
        context_t * p=nullptr;
		//if((_op->GetType()!=RenderPosition::FRONT&&_op->GetType()!=RenderPosition::BACK)||_op->IsForward())
        if(_op->IsForward())
		{
			cam_fd=_op->_ctx0->cam_fd;
			p=_op->_ctx0; 
		}
        else{
                cam_fd=_op->_ctx1->cam_fd;
		     	p=_op->_ctx1; 
            
        }
         
        //assert(p!=nullptr);
		/*
        else
		{
            if(_op->IsForward())
			 {
                cam_fd=_ctx[0].cam_fd;
                p=&_ctx[0]; 
             }   
            else 
            {
                cam_fd=_ctx[1].cam_fd;
                p=&_ctx[1];
            }
		}
        */
		
		
        /*
        fd_set fds;
	    struct timeval timeout = {0,0};
        timeout.tv_sec = 1;

        FD_ZERO(&fds);
	    FD_SET(cam_fd, &fds);
		
	    int err = select(cam_fd + 1, &fds, NULL, NULL, &timeout);
	    if (err != -1 && FD_ISSET(cam_fd, &fds)) */
        fds[0].fd = cam_fd;
    	fds[0].events = POLLIN;

        if(poll(&fds[0], 1, 3000) > 0)
		{
                if (fds[0].revents & POLLIN)
                {
                    /* Dequeue a camera buff */
                    memset(&v4l2_buf, 0, sizeof(v4l2_buf));
                
                    v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
                    if (p->capture_dmabuf)
                        v4l2_buf.memory = V4L2_MEMORY_DMABUF;
                    else
                        v4l2_buf.memory = V4L2_MEMORY_MMAP;
                    
                    if (ioctl(cam_fd, VIDIOC_DQBUF, &v4l2_buf) < 0)
                        printf("Failed to dequeue camera buff: %s (%d)\r\n",strerror(errno), errno);
                    
                    //_ctx.frame++;
                   

                    //rtc::scoped_refptr<webrtc::I420Buffer> buffer=webrtc::I420Buffer::Create(1280,720);
                    rtc::scoped_refptr<webrtc::I420Buffer> buffer=webrtc::I420Buffer::Create(p->cam_w,p->cam_h);
                    /*
                    const int conversionResult = libyuv::ConvertToI420((uint8_t*)p->g_buff[v4l2_buf.index].start, 0,
                    buffer->MutableDataY(), buffer->StrideY(),
                    buffer->MutableDataU(), buffer->StrideU(),
                    buffer->MutableDataV(), buffer->StrideV(),
                    0, 0,
                    p->cam_w,p->cam_h,
                    1280, 720,
                    libyuv::kRotate0, libyuv::FOURCC_YUYV);
                    */

                    
                    if(!CameraRecord)
                    {
                        rtc::scoped_refptr<webrtc::I420Buffer> buffer=webrtc::I420Buffer::Create(p->cam_w,p->cam_h);
                    
                    
                        const int conversionResult = libyuv::ConvertToI420((uint8_t*)p->g_buff[v4l2_buf.index].start, 0,
                            buffer->MutableDataY(), buffer->StrideY(),
                            buffer->MutableDataU(), buffer->StrideU(),
                            buffer->MutableDataV(), buffer->StrideV(),
                            0, 0,
                            p->cam_w,p->cam_h,
                            buffer->width(), buffer->height(),
                            libyuv::kRotate0, libyuv::FOURCC_YUYV);

                            int width = p->cam_w;
                        int height = p->cam_h;

                        
                        
                        if (conversionResult >= 0)
                        {
                            webrtc::VideoFrame videoFrame(buffer, webrtc::VideoRotation::kVideoRotation_0, rtc::TimeNanos());
                            if ((p->cam_w == 0) && (p->cam_h == 0)) {
                                _broadcaster.OnFrame(videoFrame);

                            }
                            else
                            {

                                if (height == 0) {
                                    height = (videoFrame.height() * width) / videoFrame.width();
                                }
                                else if (width == 0) {
                                    width = (videoFrame.width() * height) / videoFrame.height();
                                }
                                
                                
                                width = 1080;
                                height = 640;
                                int stride_y = width;
                                int stride_uv = (width + 1) / 2;

                                rtc::scoped_refptr<webrtc::I420Buffer> scaled_buffer = webrtc::I420Buffer::Create(width, height, stride_y, stride_uv, stride_uv);
                                scaled_buffer->ScaleFrom(*videoFrame.video_frame_buffer()->ToI420());
                                webrtc::VideoFrame frame = webrtc::VideoFrame(scaled_buffer, webrtc::kVideoRotation_0, rtc::TimeNanos());
                                count++;

                                //if(count == 2)
                                {
                                    _broadcaster.OnFrame(frame);
                                    count = 0;
                                }
                                
                                
                            //if(_op->GetType()!=RenderPosition::FRONT)
                            
                            }
                        }
                    }
                    else
                    { 
                        //if(_op->GetType()==RenderPosition::DASHBOARD || _op->GetType()==RenderPosition::DASHBOARD_m)
                        if((_op->GetType() == _op->_ctx0->m_Gsml_Info.m_device) && _op->_ctx0->m_Gsml_Info.m_record && !count)
                        {

                            printf("---%ld----%d---%d---\r\n",_op->_ctx0->m_Gsml_Info.m_time,_op->_ctx0->m_Gsml_Info.m_record,_op->_ctx0->m_Gsml_Info.m_device);

                            time_t rawtime;
                            struct tm* info;
                            time(&rawtime);

                            info = localtime(&rawtime);

                            uint8_t secret[64] = { 0 };

                            sprintf((char*)secret, "log//%d-%.2d-%.2d_%.2d-%.2d-%.2d--%d.yuv", info->tm_year + 1900, info->tm_mon + 1, info->tm_mday,info->tm_hour,info->tm_min,info->tm_sec,
                            _op->_ctx0->m_Gsml_Info.m_device);
                            printf("%s\r\n",secret);

                            File_fd = open((const char *)secret,O_WRONLY | O_CREAT | O_NOCTTY | O_TRUNC);

                            write(File_fd,p->g_buff[v4l2_buf.index].start,v4l2_buf.length);

                            if(_op->_ctx0->m_Gsml_Info.m_time)//time
                            {
                                _curStopTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
                                count = 1;
                            }
                            else//notime
                            {
                                _op->_ctx0->m_Gsml_Info.m_record = 0;
                                close(File_fd);
                            }
                            
                            
                        }
                        else if((_op->GetType() == _op->_ctx0->m_Gsml_Info.m_device) && _op->_ctx0->m_Gsml_Info.m_record && count)
                        {
                            long long _dstStopTick = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

                            if(_dstStopTick - _curStopTick >= 2000)
                            {
                                _op->_ctx0->m_Gsml_Info.m_record = 0;
                                count = 0;
                                close(File_fd);
                            }
                            else
                            {
                                write(File_fd,p->g_buff[v4l2_buf.index].start,v4l2_buf.length);
                            }
                                
                        }
                        else if((_op->GetType() == _op->_ctx0->m_Gsml_Info.m_device) && !_op->_ctx0->m_Gsml_Info.m_record && count)
                        {
                            write(File_fd,p->g_buff[v4l2_buf.index].start,v4l2_buf.length);
                            _op->_ctx0->m_Gsml_Info.m_record = 0;
                            count = 0;
                            close(File_fd);
                        }
                            
                    }

                    if (ioctl(p->cam_fd, VIDIOC_QBUF, &v4l2_buf))
                        printf("Failed to queue camera buffers: %s (%d)\r\n",strerror(errno), errno);
                }
				
		}
        if(!CameraRecord)
            std::this_thread::sleep_for(std::chrono::milliseconds(30)); 
             
	} 
            
	
	stop_streams();
    close_cam();


}
void GSMLCapturer::Destroy() {
	Stop();
}

void GSMLCapturer::AddOrUpdateSink(
	rtc::VideoSinkInterface<webrtc::VideoFrame>* sink,
	const rtc::VideoSinkWants& wants) {
	_broadcaster.AddOrUpdateSink(sink, wants);
}
 void GSMLCapturer::RemoveSink(rtc::VideoSinkInterface<webrtc::VideoFrame>* sink)
{
	_broadcaster.RemoveSink(sink);
}
bool GSMLCapturer::prepare_buffer()
{
   //int32_t count=_op->GetType()==RenderPosition::FRONT_BACK?2:1;
   //for(int32_t i=0;i<count;i++)
   { 
       context_t * p=_op->_ctx0;
      p->g_buff = (nv_buffer *)malloc(V4L2_BUFFERS_NUM * sizeof(nv_buffer));
    if ( p->g_buff == NULL)
        ERROR_RETURN("Failed to allocate global buffer context");

    if ( p->capture_dmabuf) {
        if (!request_camera_buff(p))
            ERROR_RETURN("Failed to set up camera buff");
    } else {
        if (!request_camera_buff_mmap(p))
            ERROR_RETURN("Failed to set up camera buff");
    }
     INFO("Succeed in preparing stream buffers");
   }
   
    return true;
}
bool GSMLCapturer::request_camera_buff( context_t * p)
{
    
  //  for(int32_t i=0;i<count;i++)
    { 
    //    context_t * p=&_ctx[i];
        struct v4l2_requestbuffers rb;
        memset(&rb, 0, sizeof(rb));
        rb.count = V4L2_BUFFERS_NUM;
        rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        rb.memory = V4L2_MEMORY_DMABUF;
        if (ioctl( p->cam_fd, VIDIOC_REQBUFS, &rb) < 0)
            ERROR_RETURN("Failed to request v4l2 buffers: %s (%d)",
                    strerror(errno), errno);
        if (rb.count != V4L2_BUFFERS_NUM)
            ERROR_RETURN("V4l2 buffer number is not as desired");

        for (unsigned int index = 0; index < V4L2_BUFFERS_NUM; index++)
        {
            struct v4l2_buffer buf;

            /* Query camera v4l2 buf length */
            memset(&buf, 0, sizeof buf);
            buf.index = index;
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_DMABUF;

            if (ioctl( p->cam_fd, VIDIOC_QUERYBUF, &buf) < 0)
                ERROR_RETURN("Failed to query buff: %s (%d)",
                        strerror(errno), errno);

            /* TODO: add support for multi-planer
            Enqueue empty v4l2 buff into camera capture plane */
            buf.m.fd = (unsigned long) p->g_buff[index].dmabuff_fd;
            if (buf.length !=  p->g_buff[index].size)
            {
                WARN("Camera v4l2 buf length is not expected");
                p->g_buff[index].size = buf.length;
            }

            if (ioctl( p->cam_fd, VIDIOC_QBUF, &buf) < 0)
                ERROR_RETURN("Failed to enqueue buffers: %s (%d)",
                        strerror(errno), errno);
        } 
    }   
    return true;
}

bool GSMLCapturer::stop_streams()
{
   enum v4l2_buf_type type;
    //int32_t count=_op->GetType()==RenderPosition::FRONT_BACK?2:1;
    //for(int32_t i=0;i<count;i++)
    { 
        context_t * p=_op->_ctx0;
          for (unsigned int index = 0; index < V4L2_BUFFERS_NUM; index++)
           {
               
             if(munmap(p->g_buff[index].start,p->g_buff[index].size)==-1)
             {
                ERROR_RETURN("munmap failed: %s (%d)", strerror(errno), errno);
             }
           }
    /* Stop v4l2 streaming */
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (ioctl( p->cam_fd, VIDIOC_STREAMOFF, &type))
            ERROR_RETURN("Failed to stop streaming: %s (%d)",
                    strerror(errno), errno);

        INFO("Camera video streaming off ...");
     
      
    }
    return true;
}

bool GSMLCapturer::start_streams()
{
	  enum v4l2_buf_type type;
 //   int32_t count=_op->GetType()==RenderPosition::FRONT_BACK?2:1;
   // for(int32_t i=0;i<count;i++)
    { 
        context_t * p=_op->_ctx0;
       
    /* Start v4l2 streaming */
        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        if (ioctl( p->cam_fd, VIDIOC_STREAMON, &type) < 0)
            ERROR_RETURN("Failed to start streaming: %s (%d)",
                    strerror(errno), errno);
       

      
    
	 usleep(200);
     INFO("Camera video streaming on ...");
    }
  
    return true;
} 

bool GSMLCapturer::request_camera_buff_mmap(context_t * p)
{
    struct v4l2_requestbuffers rb;
 //   int32_t count=_op->GetType()==RenderPosition::FRONT_BACK?2:1;
 //   for(int32_t i=0;i<count;i++)
    { 
      
        memset(&rb, 0, sizeof(rb));
        rb.count = V4L2_BUFFERS_NUM;
        rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        rb.memory = V4L2_MEMORY_MMAP;
        if (ioctl( p->cam_fd, VIDIOC_REQBUFS, &rb) < 0)
            ERROR_RETURN("Failed to request v4l2 buffers: %s (%d)",
                    strerror(errno), errno);
        if (rb.count != V4L2_BUFFERS_NUM)
            ERROR_RETURN("V4l2 buffer number is not as desired");

        for (unsigned int index = 0; index < V4L2_BUFFERS_NUM; index++)
        {
            struct v4l2_buffer buf;

            /* Query camera v4l2 buf length */
            memset(&buf, 0, sizeof buf);
            buf.index = index;
            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

            buf.memory = V4L2_MEMORY_MMAP;
            if (ioctl(p->cam_fd, VIDIOC_QUERYBUF, &buf) < 0)
                ERROR_RETURN("Failed to query buff: %s (%d)",
                        strerror(errno), errno);

            p->g_buff[index].size = buf.length;
            p->g_buff[index].start = (unsigned char *)
                mmap (NULL /* start anywhere */,
                        buf.length,
                        PROT_READ | PROT_WRITE /* required */,
                        MAP_SHARED /* recommended */,
                        p->cam_fd, buf.m.offset);
            if (MAP_FAILED ==  p->g_buff[index].start)
                ERROR_RETURN("Failed to map buffers");

            if (ioctl( p->cam_fd, VIDIOC_QBUF, &buf) < 0)
                ERROR_RETURN("Failed to enqueue buffers: %s (%d)",
                        strerror(errno), errno);
        }
    }    
    return true;
}
void GSMLCapturer::close_cam()
{
   //   int32_t count=_op->GetType()==RenderPosition::FRONT_BACK?2:1;
    //for(int32_t i=0;i<count;i++)
    { 
        context_t * p=_op->_ctx0;
         
        if(p->g_buff!=nullptr)
          {
           
              free(p->g_buff);
              p->g_buff = nullptr;
          } 
        if(p->cam_fd>0)
           close(p->cam_fd);
        
    }
    free(_op->_ctx0);
}
void GSMLCapturer::Stop()
{
    count = 0;
    CameraRecord = 0;
    _op->_ctx0->m_Gsml_Info.m_device = 10;
    _op->_ctx0->m_Gsml_Info.m_record = 0;
        _op->_ctx0->m_Gsml_Info.m_time = 0;
    _run=false;
    //if(_thread.joinable())
        _thread.join();
}