//Input Port #0: Buffer_Size = 1, Params_Type = ProcessorMulti_Processor_Control_Params, Data_Type = ProcessorMulti_Processor_Control_Data
//Input Port #1: Buffer_Size = 1, Params_Type = SensorInternalEvent_Sensor_Joystick_Params, Data_Type = SensorInternalEvent_Sensor_Joystick_Data
bool DECOFUNC(processMultiDrainData)(void * paramsPtr, void * varsPtr, QVector<QVector<void *> > drainParams, QVector<QVector<void *> > drainData)
{
VisualizationMulti_VisualMisc_ControlJoy_Params * params=(VisualizationMulti_VisualMisc_ControlJoy_Params *)paramsPtr;
VisualizationMulti_VisualMisc_ControlJoy_Vars * vars=(VisualizationMulti_VisualMisc_ControlJoy_Vars *)varsPtr;
QVector<ProcessorMulti_Processor_Control_Params *> drainparams_0; copyQVector(drainparams_0,drainParams[0]);
QVector<SensorInternalEvent_Sensor_Joystick_Params *> drainparams_1; copyQVector(drainparams_1,drainParams[1]);
QVector<ProcessorMulti_Processor_Control_Data *> draindata_0; copyQVector(draindata_0,drainData[0]);
QVector<SensorInternalEvent_Sensor_Joystick_Data *> draindata_1; copyQVector(draindata_1,drainData[1]);
if(draindata_0.size()==0){return 0;}
if(draindata_1.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata_index.
*/
ProcessorMulti_Processor_Control_Data* controldata = draindata_0.front();
SensorInternalEvent_Sensor_Joystick_Data* joystick_data = draindata_1.front();
if(controldata->isManualControl == 1)//manual control
{
QImage image(vars->width, vars->height, QImage::Format_ARGB32);
QPainter painter;
//vars->joystick_label->resize(vars->width, vars->height);
painter.begin(&image);
painter.setBackground(QBrush(QColor(241,241,241)));
painter.eraseRect(0,0, vars->width, vars->height);
if(joystick_data->forward)
painter.drawImage(vars->width/2-16, vars->height/2-56, vars->forwardArrowImg);
if(joystick_data->left)
painter.drawImage(vars->width/2 -56, vars->height/2-16, vars->leftArrowImg);
if(joystick_data->right)
painter.drawImage(vars->width/2+24, vars->height/2-16, vars->rightArrowImg);
if(joystick_data->back)
painter.drawImage(vars->width/2-16, vars->height/2+24, vars->backArrowImg);
painter.end();
vars->label->setPixmap(QPixmap::fromImage(image));
}
else //Auto control
{
QFont serifFont("Times", vars->fontSize);
vars->label->setFont(serifFont);
QPalette pa;
pa.setColor(QPalette::WindowText,Qt::red);
vars->label->setPalette(pa);
//vars->label->setAlignment(Qt::AlignLeft);
QString outputstr = QString("Left_Motor: %1\nRight_Motor: %2\n").arg(controldata->left_motor).arg(controldata->right_motor);
vars->label->setText(outputstr);
}
//vars->label->setFixedHeight(250);
return 1;
}
//Input Port #0: Buffer_Size = 0, Params_Type = Simulator_Sensor_Camera_Params, Data_Type = Simulator_Sensor_Camera_Data
bool DECOFUNC(processMonoInputData)(void * paramsPtr, void * varsPtr, QVector<void *> inputParams, QVector<void *> inputData, void * outputData, QList<int> & outputPortIndex)
{
ProcessorMono_Algorithm_Detection_Camera_DBM_Params * params=(ProcessorMono_Algorithm_Detection_Camera_DBM_Params *)paramsPtr;
ProcessorMono_Algorithm_Detection_Camera_DBM_Vars * vars=(ProcessorMono_Algorithm_Detection_Camera_DBM_Vars *)varsPtr;
QVector<Simulator_Sensor_Camera_Params *> inputparams; copyQVector(inputparams,inputParams);
QVector<Simulator_Sensor_Camera_Data *> inputdata; copyQVector(inputdata,inputData);
ProcessorMono_Algorithm_Detection_Camera_DBM_Data * outputdata=(ProcessorMono_Algorithm_Detection_Camera_DBM_Data *)outputData;
outputPortIndex=QList<int>();
if(inputdata.size()==0){return 0;}
/*======Please Program below======*/
/*
Step 1: process inputdata, then store it into outputdata.
Step 2 [optional]: determine the outputPortIndex. (if not, outputdata will be sent by all ports)
E.g. outputPortIndex=QList<int>()<<(outportindex1)<<(outportindex2)...
*/
if(vars->init)
{
params->cameraid=inputparams[0]->cameraid;
params->size=inputparams[0]->size;
params->calibmat=inputparams[0]->calibmat;
params->distcoef=inputparams[0]->distcoef;
params->rotation=inputparams[0]->rotation;
params->translation=inputparams[0]->translation;
params->rectsize=inputparams[0]->rectsize;
params->rectrotation=inputparams[0]->rectrotation;
params->rectprojection=inputparams[0]->rectprojection;
params->refcamrectrot=inputparams[0]->refcamrectrot;
vars->init=0;
}
cv::Size imgsize=inputdata[0]->image.size();
imgsize.width=int(imgsize.width*vars->compressrate);
imgsize.height=int(imgsize.height*vars->compressrate);
cv::resize(inputdata[0]->image,outputdata->image,imgsize);
outputdata->timestamp=inputdata[0]->timestamp;
std::vector<cv::LatentSvmDetector::ObjectDetection> objects;
vars->detector.detect(outputdata->image,objects,vars->threshold1,vars->threadnum);
outputdata->image=inputdata[0]->image;
int i,n=objects.size();
outputdata->objects.clear();
for(i=0;i<n;i++)
{
if(objects[i].score>=vars->threshold2)
{
objects[i].rect.x/=vars->compressrate;
objects[i].rect.y/=vars->compressrate;
objects[i].rect.width/=vars->compressrate;
objects[i].rect.height/=vars->compressrate;
outputdata->objects.push_back(objects[i]);
}
}
return 1;
}
//Input Port #0: Buffer_Size = 1, Params_Type = ProcessorMulti_Processor_SimpleCollect_Params, Data_Type = ProcessorMulti_Processor_SimpleCollect_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
VisualizationMono_Processor_SimpleCollect_Params * params=(VisualizationMono_Processor_SimpleCollect_Params *)paramsPtr;
VisualizationMono_Processor_SimpleCollect_Vars * vars=(VisualizationMono_Processor_SimpleCollect_Vars *)varsPtr;
QVector<ProcessorMulti_Processor_SimpleCollect_Params *> drainparams; copyQVector(drainparams,drainParams);
QVector<ProcessorMulti_Processor_SimpleCollect_Data *> draindata; copyQVector(draindata,drainData);
if(draindata.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata.
*/
vars->label->setText(draindata.front()->simplestatus);
return 1;
}
//Input Port #0: Buffer_Size = 10, Params_Type = ProcessorMulti_Processor_Control_Params, Data_Type = ProcessorMulti_Processor_Control_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
TransmitterMono_Gazebo_OrderGenerate_Params * params=(TransmitterMono_Gazebo_OrderGenerate_Params *)paramsPtr;
TransmitterMono_Gazebo_OrderGenerate_Vars * vars=(TransmitterMono_Gazebo_OrderGenerate_Vars *)varsPtr;
QVector<ProcessorMulti_Processor_Control_Params *> drainparams; copyQVector(drainparams,drainParams);
QVector<ProcessorMulti_Processor_Control_Data *> draindata; copyQVector(draindata,drainData);
if(draindata.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata.
*/
ProcessorMulti_Processor_Control_Data* control = draindata.front();
vars->controlPub->sendMessage(-control->left_vel, -control->right_vel);
return 1;
}
//Input Port #0: Buffer_Size = 0, Params_Type = ProcessorMulti_Algorithm_Integration_VelodyneCamera_Params, Data_Type = ProcessorMulti_Algorithm_Integration_VelodyneCamera_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
StorageMono_Algorithm_Integration_VelodyneCamera_Params * params=(StorageMono_Algorithm_Integration_VelodyneCamera_Params *)paramsPtr;
StorageMono_Algorithm_Integration_VelodyneCamera_Vars * vars=(StorageMono_Algorithm_Integration_VelodyneCamera_Vars *)varsPtr;
QVector<ProcessorMulti_Algorithm_Integration_VelodyneCamera_Params *> drainparams;
copyQVector(drainparams,drainParams);
QVector<ProcessorMulti_Algorithm_Integration_VelodyneCamera_Data *> draindata;
copyQVector(draindata,drainData);
if(draindata.size()==0) {
return 0;
}
/*======Please Program below======*/
/*
Function: process draindata.
*/
return 1;
}
//Input Port #0: Buffer_Size = 0, Params_Type = ProcessorMulti_Algorithm_Integration_SegmentationCamera_Params, Data_Type = ProcessorMulti_Algorithm_Integration_SegmentationCamera_Data
//Input Port #1: Buffer_Size = 0, Params_Type = SensorInternalEvent_Algorithm_Detection_DPM_Params, Data_Type = SensorInternalEvent_Algorithm_Detection_DPM_Data
bool DECOFUNC(processMultiInputData)(void * paramsPtr, void * varsPtr, QVector<QVector<void *> > inputParams, QVector<QVector<void *> > inputData, void * outputData, QList<int> & outputPortIndex)
{
ProcessorMulti_Algorithm_Integration_SegmentationCameraDPM_Params * params=(ProcessorMulti_Algorithm_Integration_SegmentationCameraDPM_Params *)paramsPtr;
ProcessorMulti_Algorithm_Integration_SegmentationCameraDPM_Vars * vars=(ProcessorMulti_Algorithm_Integration_SegmentationCameraDPM_Vars *)varsPtr;
QVector<ProcessorMulti_Algorithm_Integration_SegmentationCamera_Params *> inputparams_0; copyQVector(inputparams_0,inputParams[0]);
QVector<SensorInternalEvent_Algorithm_Detection_DPM_Params *> inputparams_1; copyQVector(inputparams_1,inputParams[1]);
QVector<ProcessorMulti_Algorithm_Integration_SegmentationCamera_Data *> inputdata_0; copyQVector(inputdata_0,inputData[0]);
QVector<SensorInternalEvent_Algorithm_Detection_DPM_Data *> inputdata_1; copyQVector(inputdata_1,inputData[1]);
ProcessorMulti_Algorithm_Integration_SegmentationCameraDPM_Data * outputdata=(ProcessorMulti_Algorithm_Integration_SegmentationCameraDPM_Data *)outputData;
outputPortIndex=QList<int>();
if(inputdata_0.size()==0){return 0;}
if(inputdata_1.size()==0){return 0;}
/*======Please Program below======*/
/*
Step 1: process inputdata_index, then store it into outputdata.
Step 2 [optional]: determine the outputPortIndex. (if not, outputdata will be sent by all ports)
E.g. outputPortIndex=QList<int>()<<(outportindex1)<<(outportindex2)...
*/
return 1;
}
//Input Port #0: Buffer_Size = 10, Params_Type = SensorInternalEvent_Sensor_Joystick_Params, Data_Type = SensorInternalEvent_Sensor_Joystick_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
SourceDrainMono_Sensor_stm32comm_Params * params=(SourceDrainMono_Sensor_stm32comm_Params *)paramsPtr;
SourceDrainMono_Sensor_stm32comm_Vars * vars=(SourceDrainMono_Sensor_stm32comm_Vars *)varsPtr;
QVector<SensorInternalEvent_Sensor_Joystick_Params *> drainparams; copyQVector(drainparams,drainParams);
QVector<SensorInternalEvent_Sensor_Joystick_Data *> draindata; copyQVector(draindata,drainData);
if(draindata.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata.
*/
SensorInternalEvent_Sensor_Joystick_Data* inputdata = draindata.front();
double left_vel = inputdata->linear_vel + params->WheelBase*inputdata->angular_vel/2;
double right_vel = inputdata->linear_vel - params->WheelBase*inputdata->angular_vel/2;
// double left_motor = left_vel*25*4096/(2*vars->pi*params->WheelRadius)/100; //编码器1024线，4倍频后转一圈4096脉冲，电机是25:1(单位脉冲/10ms)
// double right_motor = right_vel*25*4096/(2*vars->pi*params->WheelRadius)/100; //编码器1024线，4倍频后转一圈4096脉冲，电机是25:1(单位脉冲/10ms)
///20150328 没装编码器前的测试
short left_motor = left_vel * 1000;
short right_motor = right_vel * 1000;
char dataput[7];
char data_size = 4;
dataput[0] = params->send_packhead.at(0);
dataput[1] = data_size;
*(short*)&dataput[2] = (short)left_motor;
*(short*)&dataput[4] = (short)right_motor;
dataput[6] = params->send_packtail.at(0);
int n = vars->serialport->write(dataput, 7);
if(n < 0)
{
return 0;
}
return 1;
}
//Input Port #0: Buffer_Size = 1, Params_Type = SensorTimer_Sensor_Laser_Params, Data_Type = SensorTimer_Sensor_Laser_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
VisualizationMono_Sensor_Laser_Params * params=(VisualizationMono_Sensor_Laser_Params *)paramsPtr;
VisualizationMono_Sensor_Laser_Vars * vars=(VisualizationMono_Sensor_Laser_Vars *)varsPtr;
QVector<SensorTimer_Sensor_Laser_Params *> drainparams; copyQVector(drainparams,drainParams);
QVector<SensorTimer_Sensor_Laser_Data *> draindata; copyQVector(draindata,drainData);
if(draindata.size()==0)
{
vars->beams->setText("No Data");
return 0;
}
/*======Please Program below======*/
/*
Function: process draindata.
*/
vars->startangle=90+drainparams.front()->first_step*0.25;
vars->endtangle=90+drainparams.front()->last_step*0.25;
vars->resolution=drainparams.front()->skip_step*0.25;
QImage image;
if(params->frontonly)
{
image=QImage(params->imageradius*2,params->imageradius,QImage::Format_RGBA8888);
}
else
{
image=QImage(params->imageradius*2,params->imageradius*2,QImage::Format_RGBA8888);
}
image.fill(32);
vars->painter.begin(&image);
QPen centerpen(QColor(255,0,0,255));
QPen gridpen(QColor(0,255,0,128));
QPen beampen(QColor(0,0,255,196));
QPen textpen(Qt::black);
int i,n;
vars->painter.setPen(gridpen);
vars->painter.setBrush(Qt::NoBrush);
n=params->range/params->interval;
int radiusstep=params->imageradius/n;
int centerx=params->imageradius;
int centery=params->imageradius;
for(i=1;i<=n;i++)
{
int radius=i*radiusstep;
vars->painter.drawEllipse(QPoint(centerx,centery),radius,radius);
}
centerx=params->imageradius-params->calib_width/2;
centery=params->imageradius - params->calib_height;
vars->painter.setPen(centerpen);
vars->painter.setBrush(QBrush(Qt::red,Qt::SolidPattern));
vars->painter.drawEllipse(QPoint(centerx,centery),2,2);
vars->painter.setPen(beampen);
double ratio=double(params->imageradius)/double(params->range);
n=draindata.front()->datasize;
double pi=3.1415926535897932384626433832795/180.0;
//left laser visual
for(i=0;i<n;i++)
{
double theta=(vars->startangle+vars->resolution*i)*pi;
double distance=ratio*(draindata.front()->ldata[i]);
int x=int(distance*cos(theta)+0.5);
int y=int(-distance*sin(theta)+0.5);
if(params->laserbeam)
{
vars->painter.drawLine(centerx,centery,x+centerx,y+centery);
}
else
{
vars->painter.drawEllipse(x+centerx,y+centery,2,2);
}
}
//right laser visual
centerx=params->imageradius + params->calib_width/2;
centery=params->imageradius - params->calib_height;
vars->painter.setPen(centerpen);
vars->painter.setBrush(QBrush(Qt::red,Qt::SolidPattern));
vars->painter.drawEllipse(QPoint(centerx,centery),2,2);
vars->painter.setPen(beampen);
for(i=0;i<n;i++)
{
double theta=(vars->startangle+vars->resolution*i)*pi;
double distance=ratio*(draindata.front()->rdata[i]);
int x=int(distance*cos(theta)+0.5);
int y=int(-distance*sin(theta)+0.5);
if(params->laserbeam)
{
vars->painter.drawLine(centerx,centery,x+centerx,y+centery);
}
else
{
vars->painter.drawEllipse(x+centerx,y+centery,2,2);
}
}
QFontMetrics fm=vars->painter.fontMetrics();
int height=fm.ascent()+fm.descent();
vars->painter.setPen(textpen);
vars->painter.drawText(0,height,QString("Interval = %1 cm").arg(params->interval));
vars->painter.drawText(0,height*2,QString("System Time: %1").arg(draindata.front()->qtimestamp.toString("HH:mm:ss:zzz")));
vars->painter.drawText(0,height*3,QString("URG Time: %1").arg(draindata.front()->timestamp));
vars->painter.end();
vars->beams->setPixmap(QPixmap::fromImage(image));
return 1;
return 1;
}
//Input Port #0: Buffer_Size = 10, Params_Type = ProcessorMulti_Processor_PathGenerator_Params, Data_Type = ProcessorMulti_Processor_PathGenerator_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
VisualizationMono_Processor_PathGenerator_Params * params=(VisualizationMono_Processor_PathGenerator_Params *)paramsPtr;
VisualizationMono_Processor_PathGenerator_Vars * vars=(VisualizationMono_Processor_PathGenerator_Vars *)varsPtr;
QVector<ProcessorMulti_Processor_PathGenerator_Params *> drainparams; copyQVector(drainparams,drainParams);
QVector<ProcessorMulti_Processor_PathGenerator_Data *> draindata; copyQVector(draindata,drainData);
if(draindata.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata.
*/
ProcessorMulti_Processor_PathGenerator_Data* drawdata = draindata.front();
vars->indexLabel->setText(QString::number(drawdata->index) + " " + QString::number(drawdata->timestamp));
glNewList(vars->displaylistbase,GL_COMPILE_AND_EXECUTE);
glTranslatef(0, 0, -10);
drawWheelchair(drawdata->startPoint);
//draw offline trajectory
glLineWidth(1);
glColor3f(0.6,0.6,0);
glBegin(GL_LINE_STRIP);
for(int i=0; i<vars->offlinetrajec.size(); i++)
{
glVertex3f(vars->offlinetrajec[i].x, vars->offlinetrajec[i].y, 0);
}
glEnd();
//draw map
glColor3f(0, 0, 0);
glPointSize(1);
glBegin(GL_POINTS);
for (int i = 0; i < 1600; i++) {
for (int j = 0; j < 1600; j++) {
if (vars->map[i][j] > 0) {
glVertex3f((i - vars->offsetx) / 10.0, (j - vars->offsety) / 10.0, 0);
}
}
}
glEnd();
// laser point
glColor3f(1, 0, 0);
glPointSize(1.5);
glBegin(GL_POINTS);
cout << drawdata->URGData_size << endl;
for (int i = 0; i < drawdata->URGData_size; i++) {
if (drawdata->urg_valid[0][i]) {
double lx = drawdata->urg_data_point[0][i][0];
double ly = drawdata->urg_data_point[0][i][1];
glVertex3f(lx, ly, 0);
}
if (drawdata->urg_valid[1][i]) {
double rx = drawdata->urg_data_point[1][i][0];
double ry = drawdata->urg_data_point[1][i][1];
glVertex3f(rx, ry, 0);
}
}
glEnd();
for (int i = 0; i < drawdata->URGData_size; i++) {
if (drawdata->urg_valid[0][i]) {
double lx = drawdata->urg_data_point[0][i][0];
double ly = drawdata->urg_data_point[0][i][1];
int gridx = int(lx * 10);
int gridy = int(ly * 10);
vars->map[gridx + vars->offsetx][gridy + vars->offsety] = 1;
}
if (drawdata->urg_valid[1][i]) {
double rx = drawdata->urg_data_point[1][i][0];
double ry = drawdata->urg_data_point[1][i][1];
int gridx = int(rx * 10);
int gridy = int(ry * 10);
vars->map[gridx + vars->offsetx][gridy + vars->offsety] = 1;
}
}
//draw online trajectory
if(dist(drawdata->startPoint.x,drawdata->startPoint.y, vars->lastPos.x, vars->lastPos.y)>1e-4)
{
vars->onlinetrajec.push_back(cv::Point2d(drawdata->startPoint.x,drawdata->startPoint.y));
}
glLineWidth(3);
glColor3f(0,0,1);
glBegin(GL_LINES);
for(int i=0; i<vars->onlinetrajec.size();i++)
{
glVertex3f(vars->onlinetrajec[i].x, vars->onlinetrajec[i].y, 0);
}
glEnd();
if (!drawdata->stop) {
glColor3f(0, 1, 0);
glLineWidth(1);
for(int i=0; i < drawdata->trajSets.size();i++)
{
glBegin(GL_LINE_STRIP);
for(int j=0; j < drawdata->trajSets[i].trajdat.size(); j++)
{
glVertex3f(drawdata->trajSets[i].trajdat[j].x, drawdata->trajSets[i].trajdat[j].y, 0);
}
glEnd();
}
glLineWidth(3);
if (drawdata->trajSets[drawdata->index].valid) {
glColor3f(1, 0, 0);
} else {
glColor3f(0, 0, 0);
}
glBegin(GL_LINE_STRIP);
for(int i=0; i<drawdata->trajSets[drawdata->index].trajdat.size(); i++)
{
glVertex3f(drawdata->trajSets[drawdata->index].trajdat[i].x, drawdata->trajSets[drawdata->index].trajdat[i].y, 0);
}
glEnd();
//draw target point
// glPointSize(6);
// glColor3f(0,0.3,0);
// glBegin(GL_POINTS);
// glVertex3f(drawdata->targetPoint.x, drawdata->targetPoint.y, 0);
// glEnd();
}
glEndList();
Eigen::Matrix4d CameraPos = vars->glviewer->getCameraPose();
double dx = drawdata->startPoint.x - vars->lastPos.x;
double dy = drawdata->startPoint.y - vars->lastPos.y;
vars->lastPos = cv::Point2f(drawdata->startPoint.x, drawdata->startPoint.y);
cout << dx << ' ' << dy << endl;
CameraPos(0,3) += dx;
CameraPos(1,3) += dy;
vars->glviewer->setCameraPose(CameraPos);
// vars->glviewer->makeCurrent();
vars->glviewer->update();
return 1;
}
//Input Port #0: Buffer_Size = 0, Params_Type = ProcessorMono_Algorithm_Segmentation_Velodyne_GroundObjects_Params, Data_Type = ProcessorMono_Algorithm_Segmentation_Velodyne_GroundObjects_Data
//Input Port #1: Buffer_Size = 0, Params_Type = ProcessorMono_Algorithm_Detection_Camera_DBM_Params, Data_Type = ProcessorMono_Algorithm_Detection_Camera_DBM_Data
bool DECOFUNC(processMultiDrainData)(void * paramsPtr, void * varsPtr, QVector<QVector<void *> > drainParams, QVector<QVector<void *> > drainData)
{
VisualizationMulti_Visualization_Integration_GroundObjectsDBM_Params * params=(VisualizationMulti_Visualization_Integration_GroundObjectsDBM_Params *)paramsPtr;
VisualizationMulti_Visualization_Integration_GroundObjectsDBM_Vars * vars=(VisualizationMulti_Visualization_Integration_GroundObjectsDBM_Vars *)varsPtr;
QVector<ProcessorMono_Algorithm_Segmentation_Velodyne_GroundObjects_Params *> drainparams_0; copyQVector(drainparams_0,drainParams[0]);
QVector<ProcessorMono_Algorithm_Detection_Camera_DBM_Params *> drainparams_1; copyQVector(drainparams_1,drainParams[1]);
QVector<ProcessorMono_Algorithm_Segmentation_Velodyne_GroundObjects_Data *> draindata_0; copyQVector(draindata_0,drainData[0]);
QVector<ProcessorMono_Algorithm_Detection_Camera_DBM_Data *> draindata_1; copyQVector(draindata_1,drainData[1]);
if(draindata_0.size()==0){return 0;}
if(draindata_1.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata_index.
*/
if(vars->init)
{
cv::Mat T=cv::Mat::eye(4,4,CV_32F);
int i,j;
for(i=0;i<4;i++)
{
for(j=0;j<4;j++)
{
T.at<float>(i,j)=drainparams_0[0]->calib2camera(i,j);
}
}
cv::Mat R=drainparams_1[0]->refcamrectrot;
cv::Mat P=drainparams_1[0]->rectprojection;
vars->FP=P*R*T;
vars->init=0;
}
int pointnum=draindata_0[0]->ground->size();
cv::Mat veldata=cv::Mat(pointnum,4,CV_32F,draindata_0[0]->ground->points.data(),sizeof(pcl::PointXYZ));
cv::Mat velpoints=cv::Mat::ones(pointnum,4,CV_32F);
cv::Mat tmppoints=velpoints(cv::Rect(0,0,3,pointnum));
veldata(cv::Rect(0,0,3,pointnum)).copyTo(tmppoints);
cv::Mat groundimgpoint=velpoints*vars->FP.t();
cv::Rect rectimage=cv::Rect(0,0,drainparams_1[0]->rectsize.width-1,drainparams_1[0]->rectsize.height-1);
cv::Mat image=draindata_1[0]->image.clone();
int i,n=groundimgpoint.rows;
float maxscale=0;
for(i=0;i<n;i++)
{
if(maxscale<groundimgpoint.at<float>(i,2))
{
maxscale=groundimgpoint.at<float>(i,2);
}
}
for(i=0;i<n;i++)
{
float scale=groundimgpoint.at<float>(i,2);
if(scale>0)
{
int tmpi=int(scale/maxscale*255.0);
uchar r=vars->groundcolormap.at<cv::Vec3b>(tmpi/16,tmpi%16)[0];
uchar g=vars->groundcolormap.at<cv::Vec3b>(tmpi/16,tmpi%16)[1];
uchar b=vars->groundcolormap.at<cv::Vec3b>(tmpi/16,tmpi%16)[2];
cv::Point2f drawpoint=cv::Point2f(groundimgpoint.at<float>(i,0)/scale,groundimgpoint.at<float>(i,1)/scale);
if(drawpoint.inside(rectimage))
{
switch(image.channels())
{
case 1:
cv::circle(image,drawpoint,vars->radius,cv::Scalar(r),-1);
case 3:
cv::circle(image,drawpoint,vars->radius,cv::Scalar(r,g,b),-1);
default:
continue;
}
}
}
}
pointnum=draindata_0[0]->objects->size();
veldata=cv::Mat(pointnum,4,CV_32F,draindata_0[0]->objects->points.data(),sizeof(pcl::PointXYZ));
velpoints=cv::Mat::ones(pointnum,4,CV_32F);
tmppoints=velpoints(cv::Rect(0,0,3,pointnum));
veldata(cv::Rect(0,0,3,pointnum)).copyTo(tmppoints);
cv::Mat objectsimgpoint=velpoints*vars->FP.t();
n=objectsimgpoint.rows;
maxscale=0;
for(i=0;i<n;i++)
{
if(maxscale<objectsimgpoint.at<float>(i,2))
{
maxscale=objectsimgpoint.at<float>(i,2);
}
}
for(i=0;i<n;i++)
{
float scale=objectsimgpoint.at<float>(i,2);
if(scale>0)
{
int tmpi=int(scale/maxscale*255.0);
uchar r=vars->objectcolormap.at<cv::Vec3b>(tmpi/16,tmpi%16)[0];
uchar g=vars->objectcolormap.at<cv::Vec3b>(tmpi/16,tmpi%16)[1];
uchar b=vars->objectcolormap.at<cv::Vec3b>(tmpi/16,tmpi%16)[2];
cv::Point2f drawpoint=cv::Point2f(objectsimgpoint.at<float>(i,0)/scale,objectsimgpoint.at<float>(i,1)/scale);
if(drawpoint.inside(rectimage))
{
switch(image.channels())
{
case 1:
cv::circle(image,drawpoint,vars->radius,cv::Scalar(r),-1);
case 3:
cv::circle(image,drawpoint,vars->radius,cv::Scalar(r,g,b),-1);
default:
continue;
}
}
}
}
if(image.type()==CV_8UC1)
{
QImage img(image.data, image.cols, image.rows, image.step, QImage::Format_Indexed8);
img.setColorTable(vars->colorTable);
vars->image->setPixmap(QPixmap::fromImage(img));
}
else if(image.type()==CV_8UC3)
{
QImage img(image.data, image.cols, image.rows, image.step, QImage::Format_RGB888);
img=img.rgbSwapped();
vars->image->setPixmap(QPixmap::fromImage(img));
}
else
{
vars->image->setText("Not Supported");
return 0;
}
if(image.type()==CV_8UC1)
{
int i,n=draindata_1.front()->objects.size();
for(i=0;i<n;i++)
{
cv::rectangle(image,draindata_1.front()->objects[i].rect,cv::Scalar(255),vars->thickness);
}
QImage img(image.data, image.cols, image.rows, image.step, QImage::Format_Indexed8);
img.setColorTable(vars->colorTable);
vars->image->setPixmap(QPixmap::fromImage(img));
return 1;
}
else if(image.type()==CV_8UC3)
{
int i,n=draindata_1.front()->objects.size();
for(i=0;i<n;i++)
{
int colorindex=draindata_1.front()->objects[i].classID % 6;
cv::rectangle(image,draindata_1.front()->objects[i].rect,vars->boxcolors[colorindex],vars->thickness);
cv::line(image,cv::Point(draindata_1.front()->objects[i].rect.x,draindata_1.front()->objects[i].rect.y)
,cv::Point(draindata_1.front()->objects[i].rect.x+draindata_1.front()->objects[i].rect.width,draindata_1.front()->objects[i].rect.y+draindata_1.front()->objects[i].rect.height)
,vars->boxcolors[colorindex],vars->thickness);
cv::line(image,cv::Point(draindata_1.front()->objects[i].rect.x+draindata_1.front()->objects[i].rect.width,draindata_1.front()->objects[i].rect.y)
,cv::Point(draindata_1.front()->objects[i].rect.x,draindata_1.front()->objects[i].rect.y+draindata_1.front()->objects[i].rect.height)
,vars->boxcolors[colorindex],vars->thickness);
}
QImage img(image.data, image.cols, image.rows, image.step, QImage::Format_RGB888);
img=img.rgbSwapped();
vars->image->setPixmap(QPixmap::fromImage(img));
return 1;
}
else
{
vars->image->setText("Not Supported");
return 0;
}
return 1;
}
//Input Port #0: Buffer_Size = 0, Params_Type = SensorInternalEvent_Sensor_Camera_Params, Data_Type = SensorInternalEvent_Sensor_Camera_Data
//Input Port #1: Buffer_Size = 0, Params_Type = SensorInternalEvent_Algorithm_Detection_DPM_Params, Data_Type = SensorInternalEvent_Algorithm_Detection_DPM_Data
bool DECOFUNC(processMultiDrainData)(void * paramsPtr, void * varsPtr, QVector<QVector<void *> > drainParams, QVector<QVector<void *> > drainData)
{
VisualizationMulti_Algorithm_Detection_DPM_Params * params=(VisualizationMulti_Algorithm_Detection_DPM_Params *)paramsPtr;
VisualizationMulti_Algorithm_Detection_DPM_Vars * vars=(VisualizationMulti_Algorithm_Detection_DPM_Vars *)varsPtr;
QVector<SensorInternalEvent_Sensor_Camera_Params *> drainparams_0; copyQVector(drainparams_0,drainParams[0]);
QVector<SensorInternalEvent_Algorithm_Detection_DPM_Params *> drainparams_1; copyQVector(drainparams_1,drainParams[1]);
QVector<SensorInternalEvent_Sensor_Camera_Data *> draindata_0; copyQVector(draindata_0,drainData[0]);
QVector<SensorInternalEvent_Algorithm_Detection_DPM_Data *> draindata_1; copyQVector(draindata_1,drainData[1]);
// if(draindata_0.size()==0){return 0;}
// if(draindata_1.size()==0){return 0;}
/*======Please Program below======*/
/*
Function: process draindata_index.
*/
int i,n=draindata_0.size();
for(i=n-1;i>=0;i--)
{
VisualizationMulti_Algorithm_Detection_DPM_Vars::CameraBufferData data;
data.timestamp=draindata_0[i]->timestamp;
data.cvimage=draindata_0[i]->cvimage.clone();
vars->camerabuffer.push_back(data);
}
int j,m=draindata_1.size();
for(j=m-1;j>=0;j--)
{
VisualizationMulti_Algorithm_Detection_DPM_Vars::DPMBufferData data;
data.timestamp=draindata_1[j]->timestamp;
data.cvdetection=draindata_1[j]->cvdetection;
vars->dpmbuffer.push_back(data);
}
i=0;n=vars->camerabuffer.size();
j=0;m=vars->dpmbuffer.size();
if(n==0||m==0)
{
return 0;
}
int delta=0;
while(i<n&&j<m)
{
QTime cameratimestamp=vars->camerabuffer[i].timestamp;
QTime dpmtimestamp=vars->dpmbuffer[j].timestamp;
int tmpdelta=cameratimestamp.msecsTo(dpmtimestamp);
if(tmpdelta!=0)
{
if(delta==0)
{
delta=tmpdelta;
if(tmpdelta>0)
{
i++;
}
else
{
j++;
}
}
else
{
if(abs(tmpdelta)<abs(delta))
{
delta=tmpdelta;
if(tmpdelta>0)
{
i++;
}
else
{
j++;
}
}
else
{
if(delta>0)
{
i--;
}
else
{
j--;
}
break;
}
}
}
else
{
break;
}
}
if(i==n)
{
vars->camerabuffer.erase(vars->camerabuffer.begin(),vars->camerabuffer.begin()+i-1);
vars->dpmbuffer.erase(vars->dpmbuffer.begin(),vars->dpmbuffer.begin()+j);
return 0;
}
if(j==m)
{
vars->camerabuffer.erase(vars->camerabuffer.begin(),vars->camerabuffer.begin()+i);
vars->dpmbuffer.erase(vars->dpmbuffer.begin(),vars->dpmbuffer.begin()+j-1);
return 0;
}
vars->tabwidget->setTabText(0,vars->camerabuffer[i].timestamp.toString("HH:mm:ss:zzz"));
cv::Mat image=vars->camerabuffer[i].cvimage;
if(image.type()==CV_8UC3)
{
int k,l=vars->dpmbuffer[j].cvdetection.size();
for(k=0;k<l;k++)
{
cv::rectangle(image,vars->dpmbuffer[j].cvdetection[k],cv::Scalar(255,0,0),2);
}
QImage img(image.data,image.cols,image.rows,image.step,QImage::Format_RGB888);
vars->viewer->setPixmap(QPixmap::fromImage(img));
vars->viewer->resize(img.size());
}
else if(image.type()==CV_8UC1)
{
int k,l=vars->dpmbuffer[j].cvdetection.size();
for(k=0;k<l;k++)
{
cv::rectangle(image,vars->dpmbuffer[j].cvdetection[k],cv::Scalar(255),2);
}
QImage img(image.data,image.cols,image.rows,image.step,QImage::Format_Indexed8);
img.setColorTable(vars->colorTable);
vars->viewer->setPixmap(QPixmap::fromImage(img));
vars->viewer->resize(img.size());
}
else
{
vars->viewer->setText("Not Supported");
return 0;
}
vars->camerabuffer.erase(vars->camerabuffer.begin(),vars->camerabuffer.begin()+i+1);
vars->dpmbuffer.erase(vars->dpmbuffer.begin(),vars->dpmbuffer.begin()+j+1);
return 1;
}
//Input Port #0: Buffer_Size = 1, Params_Type = SourceDrainMono_Sensor_stm32comm_Params, Data_Type = SourceDrainMono_Sensor_stm32comm_Data
bool DECOFUNC(processMonoDrainData)(void * paramsPtr, void * varsPtr, QVector<void *> drainParams, QVector<void *> drainData)
{
VisualizationMono_Sensor_stm32comm_Params * params=(VisualizationMono_Sensor_stm32comm_Params *)paramsPtr;
VisualizationMono_Sensor_stm32comm_Vars * vars=(VisualizationMono_Sensor_stm32comm_Vars *)varsPtr;
QVector<SourceDrainMono_Sensor_stm32comm_Params *> drainparams; copyQVector(drainparams,drainParams);
QVector<SourceDrainMono_Sensor_stm32comm_Data *> draindata; copyQVector(draindata,drainData);
/*======Please Program below======*/
/*
Function: process draindata.
*/
// vars->comm_label->setText(QString("YAW: %1\nrightodom: %2\nleftodom: %3\nrightspeed: %4\nleftspeed: %5\n").
// arg(draindata.front()->yaw).
// arg(draindata.front()->rightodom).arg(draindata.front()->leftodom).
// arg(draindata.front()->rightspeed).arg(draindata.front()->leftspeed));
if(draindata.size()==0)
{
vars->qmap->setText("No Data");
return 0;
}
/*======Please Program below======*/
/*
Function: process draindata.
*/
int minshiftx = 1, minshifty = 1;
int i,j,dx,dy;
cv::Point2d p;
p.x = ((int) (draindata.front()->x / params->pixelsize)) * params->pixelsize;
p.y = ((int) (draindata.front()->y / params->pixelsize)) * params->pixelsize;
dx = (p.x-vars->mapzero.x)/params->pixelsize;
dy = (p.y-vars->mapzero.y)/params->pixelsize;
if ( ! (abs(dx)<=minshiftx && abs(dy)<=minshifty))//ÐèÒªÒƶ¯
{
vars->mapzero = p;
if (abs(dx)>=vars->map.cols/2 || abs(dy)>=vars->map.rows/2)//Òƶ¯¹ýŽó£¬Ö±œÓÖØÖÃ
{
vars->map.setTo(0);
}
else
{
cv::Mat M(2,3,CV_32F);
M.at<float>(0,0)=1;M.at<float>(0,1)=0;M.at<float>(0,2)=-dx;
M.at<float>(1,0)=0;M.at<float>(1,1)=1;M.at<float>(1,2)=dy;
cv::warpAffine(vars->map,vars->map,M,vars->map.size());
}
}
//QtÊÇbgrµÄÑÕÉ«
cv::Scalar c_red = CV_RGB(0,0,255);
cv::Scalar c_blue = CV_RGB(255,144,30);
cv::Scalar c_green = CV_RGB(37,225,132);
cv::Scalar c_gold = CV_RGB(1,215,255);
cv::Point2d mapp = cv::Point2d(draindata.front()->x,draindata.front()->y);
mapp -= vars->mapzero;
mapp.x = mapp.x / params->pixelsize + params->mapsize/2;
mapp.y = mapp.y / params->pixelsize + params->mapsize/2;
circle(vars->map,mapp,2,c_gold,-1);
//Ê®Ã×Ò»žöÍøžñ
cv::Mat gridmap3(cv::Size(params->mapsize,params->mapsize),CV_8UC3);
gridmap3.setTo(0);
cv::Mat gridmap1(cv::Size(params->mapsize,params->mapsize),CV_8UC1);
gridmap1.setTo(0);
int grid = 10.0 / params->pixelsize;
for(int i = params->mapsize % grid ; i < params->mapsize ; i+= grid)
{
cv::line(gridmap3,cv::Point(i,0),cv::Point(i,params->mapsize),c_blue,1);
cv::line(gridmap1,cv::Point(i,0),cv::Point(i,params->mapsize),cv::Scalar(255),1);
}
for(int i = params->mapsize - grid ; i > 0 ; i -= grid)
{
cv::line(gridmap3,cv::Point(0,i),cv::Point(params->mapsize,i),c_blue,1);
cv::line(gridmap1,cv::Point(0,i),cv::Point(params->mapsize,i),cv::Scalar(255),1);
}
cv::bitwise_not(gridmap1,gridmap1);
vars->map.copyTo(gridmap3,gridmap1);
cv::circle(gridmap3,cv::Point(params->mapsize/2,params->mapsize/2),1,c_red);
//compass
gridmap3(cv::Rect(params->mapsize-grid+1,0,grid-1,grid-1)).setTo(0);
cv::Point composso = cv::Point(params->mapsize-grid/2,grid/2);
cv::circle(gridmap3,composso,grid/2-1,c_green,1,CV_AA);
cv::line(gridmap3,composso,cv::Point(composso.x + cos(draindata.front()->theta)*(grid-1)/2-1, composso.y - sin(draindata.front()->theta)*(grid-1)/2-1), c_green,1,CV_AA);
//speed + odometry + (x,y)
gridmap3(cv::Rect(0,0,2*grid-1,grid-1)).setTo(0);
cv::putText(gridmap3,QString("(%1, %2)").arg(draindata.front()->x).arg(draindata.front()->y).toStdString(),
cv::Point(0,20),CV_FONT_HERSHEY_SCRIPT_SIMPLEX,0.5,c_red,1,CV_AA);
cv::putText(gridmap3,QString("leftspeed: %1 m/s").arg(draindata.front()->theta).toStdString(),
cv::Point(0,40),CV_FONT_HERSHEY_SCRIPT_SIMPLEX,0.5,c_red,1,CV_AA);
cv::putText(gridmap3,QString("rightspeed: %1 m").arg(draindata.front()->rightodom).toStdString(),
cv::Point(0,60),CV_FONT_HERSHEY_SCRIPT_SIMPLEX,0.5,c_red,1,CV_AA);
cv::putText(gridmap3,QString("leftodom: %1 m").arg(draindata.front()->leftodom).toStdString(),
cv::Point(0,80),CV_FONT_HERSHEY_SCRIPT_SIMPLEX,0.5,c_red,1,CV_AA);
cv::putText(gridmap3,QString("angle: %1 deg").arg(draindata.front()->theta).toStdString(),
cv::Point(0,100),CV_FONT_HERSHEY_SCRIPT_SIMPLEX,0.5,c_red,1,CV_AA);
int timestamp=((draindata.front()->timestamp.hour()*60+draindata.front()->timestamp.minute())*60
+draindata.front()->timestamp.second())*1000+draindata.front()->timestamp.msec();
cv::putText(gridmap3,QString("%1 ms").arg(timestamp).toStdString(),
cv::Point(0,120),CV_FONT_HERSHEY_SCRIPT_SIMPLEX,0.5,c_red,1,CV_AA);
int showWidth = vars->showheight;
cv::Mat showImage (cv::Size(showWidth, vars->showheight),CV_8UC3);
cv::resize(gridmap3,showImage,showImage.size());
QImage colorimg=QImage((const uchar*)(showImage.data),showImage.cols,showImage.rows, showImage.cols*showImage.channels(),QImage::Format_RGB888);
// QImage colorimg=QImage((const uchar*)(vars->map.data),vars->map.cols,vars->map.rows,QImage::Format_RGB888);
vars->qmap->setPixmap(QPixmap::fromImage(colorimg));
return 1;
}