116 lines
6.5 KiB
C++
116 lines
6.5 KiB
C++
// Baseline V4L2/CAN + 0920 TensorRT/NPP, with a narrow ctypes ABI.
|
|
#include "v4l2_capture.h"
|
|
#include "can_comm.h"
|
|
#include "detector.hpp"
|
|
#include "uyvy_converter.hpp"
|
|
#include <linux/videodev2.h>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <condition_variable>
|
|
#include <cstring>
|
|
#include <mutex>
|
|
#include <thread>
|
|
|
|
namespace {
|
|
using Clock=std::chrono::steady_clock;
|
|
double nowMs(){return std::chrono::duration<double,std::milli>(Clock::now().time_since_epoch()).count();}
|
|
thread_local std::string error;
|
|
std::mutex canMutex;
|
|
int positions[4]={-1,-1,-1,-1};
|
|
double canTime=0;
|
|
void receiveCan(int64_t, uint32_t id, const uint8_t* data, uint32_t len){
|
|
if(id!=0x391||len<4)return;
|
|
std::lock_guard<std::mutex> lock(canMutex);
|
|
// Baseline comments specify bytes 1.0..4.7 (not overlapping bits 0,1,2,3).
|
|
for(int i=0;i<4;++i)positions[i]=data[i];
|
|
canTime=nowMs();
|
|
}
|
|
struct Engine {
|
|
fangjia::TrtLogger logger;
|
|
std::unique_ptr<cms::Detector> detector;
|
|
std::unique_ptr<fangjia::RtmwEngine> pose;
|
|
cudaStream_t stream{};
|
|
int width,height,maxPeople;
|
|
Engine(const char* dir,int w,int h,int n,float confidence):width(w),height(h),maxPeople(n){
|
|
detector=std::make_unique<cms::Detector>(std::filesystem::path(dir)/"yolo11s.engine",logger,confidence,n);
|
|
pose=std::make_unique<fangjia::RtmwEngine>(std::filesystem::path(dir)/"rtmw_l_384.engine",logger,w,h,n);
|
|
CUDA_CHECK(cudaStreamCreateWithFlags(&stream,cudaStreamNonBlocking));
|
|
try {pose->warmup(stream);detector->infer(cv::Mat(h,w,CV_8UC3,cv::Scalar()),stream);}catch(...){cudaStreamDestroy(stream);throw;}
|
|
}
|
|
~Engine(){if(stream)cudaStreamDestroy(stream);}
|
|
};
|
|
struct Camera {
|
|
V4l2Capture capture;
|
|
std::unique_ptr<cms::UyvyConverter> converter;
|
|
std::thread worker;
|
|
std::mutex mutex;
|
|
std::condition_variable ready;
|
|
std::atomic<bool> stop{false};
|
|
std::shared_ptr<const cms::BgrImage> latest;
|
|
uint64_t generation=0, consumed=0, sequence=0;
|
|
double timestamp=0,driverTimestamp=0;
|
|
int width,height;
|
|
bool canEnabled=false;
|
|
std::string failure;
|
|
Camera(const char* device,int w,int h,int fps,bool can):width(w),height(h){
|
|
if(!capture.open(device)||!capture.setFormat(w,h,V4L2_PIX_FMT_UYVY))throw std::runtime_error("Camera open/format failed");
|
|
if(capture.width()!=uint32_t(w)||capture.height()!=uint32_t(h))throw std::runtime_error("Camera negotiated unexpected dimensions");
|
|
capture.setFps(fps);
|
|
if(!capture.initMmap(4)||!capture.startStreaming())throw std::runtime_error("Camera streaming failed");
|
|
converter=std::make_unique<cms::UyvyConverter>(w,h);
|
|
if(can){CanComm::set_msg_callback(receiveCan);canEnabled=CanComm::can_start();}
|
|
try {worker=std::thread([this]{loop();});}catch(...){if(canEnabled)CanComm::can_stop();throw;}
|
|
}
|
|
~Camera(){stop=true;ready.notify_all();if(worker.joinable())worker.join();capture.close();if(canEnabled)CanComm::can_stop();}
|
|
void loop(){
|
|
try {
|
|
int misses=0;
|
|
while(!stop){
|
|
V4l2Capture::Frame frame;
|
|
if(!capture.getFrame(frame,1000)){if(++misses>=5)throw std::runtime_error("Camera timed out for 5 seconds");continue;}
|
|
double received=nowMs();
|
|
misses=0;
|
|
auto stride=frame.bytes_per_line?frame.bytes_per_line:width*2;
|
|
if(frame.size<size_t(stride)*height){capture.releaseFrame(frame);continue;}
|
|
std::shared_ptr<const cms::BgrImage> image;
|
|
try {image=converter->convert(frame.data,stride);}catch(...){capture.releaseFrame(frame);throw;}
|
|
auto seq=frame.sequence;double driverTs=frame.timestamp_us/1000.0;
|
|
capture.releaseFrame(frame);
|
|
{std::lock_guard<std::mutex> lock(mutex);latest=std::move(image);sequence=seq;timestamp=received;driverTimestamp=driverTs;++generation;}
|
|
ready.notify_one();
|
|
}
|
|
}catch(std::exception const&e){std::lock_guard<std::mutex> lock(mutex);failure=e.what();stop=true;ready.notify_all();}
|
|
}
|
|
};
|
|
}
|
|
extern "C" {
|
|
const char* vap_error(){return error.c_str();}
|
|
void* vap_engine(const char* dir,int w,int h,int n,float confidence){try {return new Engine(dir,w,h,n,confidence);}catch(std::exception const&e){error=e.what();return nullptr;}}
|
|
void vap_engine_close(void* p){delete static_cast<Engine*>(p);}
|
|
int vap_detect(void* p,unsigned char* pixels,int w,int h,float* out){try {
|
|
auto& e=*static_cast<Engine*>(p);auto boxes=e.detector->infer(cv::Mat(h,w,CV_8UC3,pixels),e.stream);
|
|
for(size_t i=0;i<boxes.size();++i){auto& b=boxes[i];float v[]={b.x1,b.y1,b.x2,b.y2,b.score};std::memcpy(out+i*5,v,sizeof(v));}
|
|
return boxes.size();
|
|
}catch(std::exception const&e){error=e.what();return -1;}}
|
|
int vap_pose(void* p,unsigned char* pixels,int w,int h,const float* boxes,int count,float* out){try {
|
|
auto& e=*static_cast<Engine*>(p);
|
|
if(count<0||count>e.maxPeople||w>e.width||h>e.height)throw std::runtime_error("Pose input exceeds configured capacity");
|
|
std::vector<fangjia::Box> input;for(int i=0;i<count;++i)input.push_back({boxes[i*4],boxes[i*4+1],boxes[i*4+2],boxes[i*4+3],1,0});
|
|
auto poses=e.pose->infer(cv::Mat(h,w,CV_8UC3,pixels),input,e.stream);
|
|
for(size_t i=0;i<poses.size();++i)for(int k=0;k<133;++k){auto& pt=poses[i].keypoints[k];auto* v=out+(i*133+k)*3;v[0]=pt.x;v[1]=pt.y;v[2]=pt.score;}
|
|
return poses.size();
|
|
}catch(std::exception const&e){error=e.what();return -1;}}
|
|
void* vap_camera(const char* device,int w,int h,int fps,int can){try{return new Camera(device,w,h,fps,can);}catch(std::exception const&e){error=e.what();return nullptr;}}
|
|
void vap_camera_close(void* p){delete static_cast<Camera*>(p);}
|
|
int vap_read(void* p,unsigned char* out,double* meta){try {
|
|
auto& c=*static_cast<Camera*>(p);std::shared_ptr<const cms::BgrImage> image;
|
|
{std::unique_lock<std::mutex> lock(c.mutex);
|
|
if(!c.ready.wait_for(lock,std::chrono::seconds(6),[&]{return c.generation>c.consumed||c.stop;}))throw std::runtime_error("No camera frame");
|
|
if(!c.failure.empty())throw std::runtime_error(c.failure);
|
|
if(c.stop)return 0;
|
|
image=c.latest;meta[0]=c.sequence;meta[1]=c.timestamp;meta[2]=c.generation-c.consumed-1;meta[3]=c.driverTimestamp;c.consumed=c.generation;}
|
|
std::memcpy(out,image->pixels.data(),image->pixels.size());return 1;
|
|
}catch(std::exception const&e){error=e.what();return -1;}}
|
|
void vap_can(double* out){std::lock_guard<std::mutex> lock(canMutex);for(int i=0;i<4;++i)out[i]=positions[i];out[4]=canTime?nowMs()-canTime:-1;}
|
|
}
|