aboutsummaryrefslogtreecommitdiffstats
path: root/nodes/ti_estop/src/estop.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'nodes/ti_estop/src/estop.cpp')
-rw-r--r--nodes/ti_estop/src/estop.cpp1182
1 files changed, 1182 insertions, 0 deletions
diff --git a/nodes/ti_estop/src/estop.cpp b/nodes/ti_estop/src/estop.cpp
new file mode 100644
index 0000000..bba6a5c
--- /dev/null
+++ b/nodes/ti_estop/src/estop.cpp
@@ -0,0 +1,1182 @@
1/*
2 *
3 * Copyright (c) 2020 Texas Instruments Incorporated
4 *
5 * All rights reserved not granted herein.
6 *
7 * Limited License.
8 *
9 * Texas Instruments Incorporated grants a world-wide, royalty-free, non-exclusive
10 * license under copyrights and patents it now or hereafter owns or controls to make,
11 * have made, use, import, offer to sell and sell ("Utilize") this software subject to the
12 * terms herein. With respect to the foregoing patent license, such license is granted
13 * solely to the extent that any such patent is necessary to Utilize the software alone.
14 * The patent license shall not apply to any combinations which include this software,
15 * other than combinations with devices manufactured by or for TI ("TI Devices").
16 * No hardware patent is licensed hereunder.
17 *
18 * Redistributions must preserve existing copyright notices and reproduce this license
19 * (including the above copyright notice and the disclaimer and (if applicable) source
20 * code license limitations below) in the documentation and/or other materials provided
21 * with the distribution
22 *
23 * Redistribution and use in binary form, without modification, are permitted provided
24 * that the following conditions are met:
25 *
26 * * No reverse engineering, decompilation, or disassembly of this software is
27 * permitted with respect to any software provided in binary form.
28 *
29 * * any redistribution and use ar./apps/ptk_demos/app_dof_sfm_fisheye/config/app.cfge licensed by TI for use only with TI Devices.
30 *
31 * * Nothing shall obligate TI to provide you with source code for the software
32 * licensed and provided to you in appCntxtect code.
33 *
34 * If software source code is provided to you, modification and redistribution of the
35 * source code are permitted provided that the following conditions are met:
36 *
37 * * any redistribution and use of the source code, including any resulting derivative
38 * works, are licensed by TI for use only with TI Devices.
39 *
40 * * any redistribution and use of any appCntxtect code compiled from the source code
41 * and any resulting derivative works, are licensed by TI for use only with TI Devices.
42 *
43 * Neither the name of Texas Instruments Incorporated nor the names of its suppliers
44 *
45 * may be used to endorse or promote products derived from this software without
46 * specific prior written permission.
47 *
48 * DISCLAIMER.
49 *
50 * THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "AS IS" AND ANY EXPRESS
51 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
52 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
53 * IN NO EVENT SHALL TI AND TI'S LICENSORS BE LIABLE FOR ANY DIRECT, INDIRECT,
54 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
55 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
56 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
57 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
58 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
59 * OF THE POSSIBILITY OF SUCH DAMAGE.
60 *
61 */
62
63#include <signal.h>
64#include <stdio.h>
65#include <stdlib.h>
66
67#include "estop.h"
68
69#include <app_ptk_demo_common.h>
70#include <app_ptk_demo_disparity.h>
71
72
73void ESTOP_APP_setPcParams(ESTOP_APP_Context *appCntxt);
74void ESTOP_APP_setOgParams(ESTOP_APP_Context *appCntxt);
75
76
77static char menu[] = {
78 "\n"
79 "\n ======================================================"
80 "\n Demo : AMR E-STOP "
81 "\n ======================================================"
82 "\n"
83 "\n p: Print performance statistics"
84 "\n"
85 "\n e: Export performance statistics"
86 "\n"
87 "\n x: Exit"
88 "\n"
89 "\n Enter Choice: "
90};
91
92
93void ESTOP_APP_setLDCCreateParams(ESTOP_APP_Context *appCntxt)
94{
95 SDELDCAPPLIB_createParams * createParams = &appCntxt->sdeLdcCreateParams;
96
97 createParams->leftLutFileName = appCntxt->left_LUT_file_name;
98 createParams->rightLutFileName = appCntxt->right_LUT_file_name;
99
100 createParams->width = appCntxt->width;
101 createParams->height = appCntxt->height;
102 createParams->inputFormat = appCntxt->inputFormat;
103 createParams->pipelineDepth = appCntxt->pipelineDepth;
104}
105
106void ESTOP_APP_setSLSdeCreateParams(ESTOP_APP_Context *appCntxt)
107{
108 SL_SDEAPPLIB_createParams * createParams = &appCntxt->slSdeCreateParams;
109 createParams->sdeCfg = appCntxt->sde_params;
110
111 if (appCntxt->sde_params.disparity_min == 0)
112 {
113 createParams->minDisparity = 0;
114 }
115 else if (appCntxt->sde_params.disparity_min == 1)
116 {
117 createParams->minDisparity = -3;
118 }
119
120 if (appCntxt->sde_params.disparity_max == 0)
121 {
122 createParams->maxDisparity = 63;
123 }
124 else if (appCntxt->sde_params.disparity_max == 1)
125 {
126 createParams->maxDisparity = 127;
127 }
128 else if (appCntxt->sde_params.disparity_max == 2)
129 {
130 createParams->maxDisparity = 191;
131 }
132
133 createParams->width = appCntxt->width;
134 createParams->height = appCntxt->height;
135 createParams->inputFormat = appCntxt->inputFormat;
136 createParams->pipelineDepth = appCntxt->pipelineDepth;
137}
138
139void ESTOP_APP_setMLSdeCreateParams(ESTOP_APP_Context *appCntxt)
140{
141 ML_SDEAPPLIB_createParams * createParams = &appCntxt->mlSdeCreateParams;
142 createParams->sdeCfg = appCntxt->sde_params;
143
144 if (appCntxt->sde_params.disparity_min == 0)
145 {
146 createParams->minDisparity = 0;
147 }
148 else if (appCntxt->sde_params.disparity_min == 1)
149 {
150 createParams->minDisparity = -3;
151 }
152
153 if (appCntxt->sde_params.disparity_max == 0)
154 {
155 createParams->maxDisparity = 63;
156 }
157 else if (appCntxt->sde_params.disparity_max == 1)
158 {
159 createParams->maxDisparity = 127;
160 }
161 else if (appCntxt->sde_params.disparity_max == 2)
162 {
163 createParams->maxDisparity = 191;
164 }
165
166 createParams->inputFormat = appCntxt->inputFormat;
167 createParams->numLayers = appCntxt->numLayers;
168 createParams->enableMedianFilter = appCntxt->ppMedianFilterEnable;
169 createParams->width = appCntxt->width;
170 createParams->height = appCntxt->height;
171 createParams->pipelineDepth = appCntxt->pipelineDepth;
172}
173
174void ESTOP_APP_setSSDetectCreateParams(ESTOP_APP_Context *appCntxt)
175{
176 SS_DETECT_APPLIB_createParams * createParams = &appCntxt->ssDetectCreateParams;
177
178 createParams->width = appCntxt->width;
179 createParams->height = appCntxt->height;
180 createParams->tensorWidth = appCntxt->tensor_width;
181 createParams->tensorHeight = appCntxt->tensor_height;
182
183 createParams->inputFormat = appCntxt->inputFormat;
184 createParams->exportGraph = appCntxt->exportGraph;
185 createParams->rtLogEnable = appCntxt->rtLogEnable;
186 createParams->vxEvtAppValBase = 0;
187
188 ESTOP_APP_setPcParams(appCntxt);
189 ESTOP_APP_setOgParams(appCntxt);
190}
191
192
193void ESTOP_APP_setPcParams(ESTOP_APP_Context *appCntxt)
194{
195 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.width = appCntxt->width;
196 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.height = appCntxt->height;
197 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.tensorWidth = appCntxt->tensor_width;
198 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.tensorHeight = appCntxt->tensor_height;
199 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.dsFactor = 4;
200 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.confidenceTh = appCntxt->confidence_threshold;
201 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.dsWidth =
202 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.width / appCntxt->ssDetectCreateParams.pcCfg.cfgParams.dsFactor;
203 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.dsHeight =
204 appCntxt->ssDetectCreateParams.pcCfg.cfgParams.height / appCntxt->ssDetectCreateParams.pcCfg.cfgParams.dsFactor;
205
206 appCntxt->ssDetectCreateParams.pcCfg.camParams.camHeight = appCntxt->camHeight;
207 appCntxt->ssDetectCreateParams.pcCfg.camParams.camRoll = appCntxt->camRoll;
208 appCntxt->ssDetectCreateParams.pcCfg.camParams.camPitch = appCntxt->camPitch;
209 appCntxt->ssDetectCreateParams.pcCfg.camParams.camYaw = appCntxt->camYaw;
210 appCntxt->ssDetectCreateParams.pcCfg.camParams.sinPitch = sin(appCntxt->camPitch);
211 appCntxt->ssDetectCreateParams.pcCfg.camParams.cosPitch = cos(appCntxt->camPitch);
212 appCntxt->ssDetectCreateParams.pcCfg.camParams.baseline = appCntxt->baseline;
213 appCntxt->ssDetectCreateParams.pcCfg.camParams.dcx = appCntxt->distCenterX;
214 appCntxt->ssDetectCreateParams.pcCfg.camParams.dcy = appCntxt->distCenterY;
215 appCntxt->ssDetectCreateParams.pcCfg.camParams.f = appCntxt->focalLength;
216}
217
218
219void ESTOP_APP_setOgParams(ESTOP_APP_Context *appCntxt)
220{
221 appCntxt->ssDetectCreateParams.ogCfg.cfgParams.width = appCntxt->width;
222 appCntxt->ssDetectCreateParams.ogCfg.cfgParams.height = appCntxt->height;
223 appCntxt->ssDetectCreateParams.ogCfg.cfgParams.enableSpatialObjMerge = appCntxt->enableSpatialObjMerge;
224 appCntxt->ssDetectCreateParams.ogCfg.cfgParams.enableTemporalObjMerge = appCntxt->enableTemporalObjMerge;
225 appCntxt->ssDetectCreateParams.ogCfg.cfgParams.enableTemporalObjSmoothing = appCntxt->enableTemporalObjSmoothing;
226 appCntxt->ssDetectCreateParams.ogCfg.cfgParams.objectDistanceMode = appCntxt->objectDistanceMode;
227
228 appCntxt->ssDetectCreateParams.ogCfg.camParams.camHeight = appCntxt->camHeight;
229 appCntxt->ssDetectCreateParams.ogCfg.camParams.camRoll = appCntxt->camRoll;
230 appCntxt->ssDetectCreateParams.ogCfg.camParams.camPitch = appCntxt->camPitch;
231 appCntxt->ssDetectCreateParams.ogCfg.camParams.camYaw = appCntxt->camYaw;
232 appCntxt->ssDetectCreateParams.ogCfg.camParams.sinPitch = sin(appCntxt->camPitch);
233 appCntxt->ssDetectCreateParams.ogCfg.camParams.cosPitch = cos(appCntxt->camPitch);
234 appCntxt->ssDetectCreateParams.ogCfg.camParams.baseline = appCntxt->baseline;
235 appCntxt->ssDetectCreateParams.ogCfg.camParams.dcx = appCntxt->distCenterX;
236 appCntxt->ssDetectCreateParams.ogCfg.camParams.dcy = appCntxt->distCenterY;
237 appCntxt->ssDetectCreateParams.ogCfg.camParams.f = appCntxt->focalLength;
238
239 appCntxt->ssDetectCreateParams.ogCfg.ogParams.xGridSize = appCntxt->xGridSize;
240 appCntxt->ssDetectCreateParams.ogCfg.ogParams.yGridSize = appCntxt->yGridSize;
241 appCntxt->ssDetectCreateParams.ogCfg.ogParams.xMinRange = appCntxt->xMinRange;
242 appCntxt->ssDetectCreateParams.ogCfg.ogParams.xMaxRange = appCntxt->xMaxRange;
243 appCntxt->ssDetectCreateParams.ogCfg.ogParams.yMinRange = appCntxt->yMinRange;
244 appCntxt->ssDetectCreateParams.ogCfg.ogParams.yMaxRange = appCntxt->yMaxRange;
245 appCntxt->ssDetectCreateParams.ogCfg.ogParams.xGridNum = appCntxt->xGridNum;
246 appCntxt->ssDetectCreateParams.ogCfg.ogParams.yGridNum = appCntxt->yGridNum;
247 appCntxt->ssDetectCreateParams.ogCfg.ogParams.thCnt = appCntxt->thCnt;
248 appCntxt->ssDetectCreateParams.ogCfg.ogParams.thObjCnt = appCntxt->thObjCnt;
249 appCntxt->ssDetectCreateParams.ogCfg.ogParams.maxNumObject = appCntxt->maxNumObject;
250 appCntxt->ssDetectCreateParams.ogCfg.ogParams.cNeighNum = appCntxt->cNeighNum;
251}
252
253void ESTOP_APP_setAllParams(ESTOP_APP_Context *appCntxt)
254{
255 appCntxt->vxEvtAppValBase = 0;
256
257 /* LDC params */
258 ESTOP_APP_setLDCCreateParams(appCntxt);
259
260 /* SDE params */
261 if (appCntxt->sdeAlgoType == 0)
262 {
263 ESTOP_APP_setSLSdeCreateParams(appCntxt);
264 }
265 else if (appCntxt->sdeAlgoType == 1)
266 {
267 ESTOP_APP_setMLSdeCreateParams(appCntxt);
268 }
269
270 ESTOP_APP_setSSDetectCreateParams(appCntxt);
271}
272
273
274vx_status ESTOP_APP_init(ESTOP_APP_Context *appCntxt)
275{
276 int32_t status;
277 vx_status vxStatus = VX_SUCCESS;
278
279 // Create the DLR Model handle
280 CM_DLRCreateParams params;
281
282 params.modelPath = appCntxt->dlrModelPath;
283 params.devType = DLR_DEVTYPE_CPU;
284 params.devId = 0;
285
286 status = CM_dlrNodeCntxtInit(&appCntxt->dlrObj, &params);
287 if (status < 0)
288 {
289 PTK_printf("[%s:%d] CM_dlrNodeCntxtInit() failed.\n",
290 __FUNCTION__, __LINE__);
291
292 vxStatus = VX_FAILURE;
293 }
294
295#if 0
296 if (vxStatus == (vx_status)VX_SUCCESS)
297 {
298 status = appInit();
299 if (status < 0)
300 {
301 PTK_printf("[%s:%d] appInit() failed.\n",
302 __FUNCTION__, __LINE__);
303
304 vxStatus = VX_FAILURE;
305 }
306 }
307#endif
308
309 // OpenVX initialization
310 if (vxStatus == (vx_status)VX_SUCCESS)
311 {
312 appCntxt->vxContext = vxCreateContext();
313 if (appCntxt->vxContext == NULL)
314 {
315 PTK_printf("[%s:%d] vxCreateContext() failed.\n",
316 __FUNCTION__, __LINE__);
317
318 vxStatus = VX_FAILURE;
319 }
320 }
321
322 // create graph
323 if (vxStatus == (vx_status)VX_SUCCESS)
324 {
325 appCntxt->vxGraph = vxCreateGraph(appCntxt->vxContext);
326 if (appCntxt->vxGraph == NULL)
327 {
328 PTK_printf("[%s:%d] vxCreateGraph() failed\n",
329 __FUNCTION__, __LINE__);
330 vxStatus = VX_FAILURE;
331 } else
332 {
333 vxSetReferenceName((vx_reference)appCntxt->vxGraph, "AMR E-Stop Graph");
334 }
335 }
336
337 if (vxStatus == (vx_status)VX_SUCCESS)
338 {
339 /* load TILDL kernels */
340 //tivxTIDLLoadKernels(appCntxt->vxContext);
341
342 /* load image processing kernel */
343 tivxImgProcLoadKernels(appCntxt->vxContext);
344
345 /* Load HWA kernels */
346 tivxHwaLoadKernels(appCntxt->vxContext);
347
348 /* Load stereo kernels */
349 tivxStereoLoadKernels(appCntxt->vxContext);
350 }
351
352 /*
353 * 1 Setup Stereo LDC nodes
354 */
355 if (vxStatus == (vx_status)VX_SUCCESS)
356 {
357 vxStatus = ESTOP_APP_init_LDC(appCntxt);
358 }
359
360 /*
361 * 2 Setup SDE nodes
362 */
363 if (vxStatus == (vx_status)VX_SUCCESS)
364 {
365 vxStatus = ESTOP_APP_init_SDE(appCntxt);
366 }
367
368 /*
369 * 3 Setup Semantic Segmentation nodes
370 */
371 if (vxStatus == (vx_status)VX_SUCCESS)
372 {
373 vxStatus = ESTOP_APP_init_SS(appCntxt);
374 }
375
376 /*
377 * 4 Setup OG map based detection nodes
378 */
379 if (vxStatus == (vx_status)VX_SUCCESS)
380 {
381 vxStatus = ESTOP_APP_init_SS_Detection(appCntxt);
382 }
383
384 if (vxStatus == (vx_status)VX_SUCCESS)
385 {
386 appPerfPointSetName(&appCntxt->estopPerf , "Emergency Stop GRAPH");
387
388 /*
389 * set up the pipeline.
390 */
391 vxStatus = ESTOP_APP_setupPipeline(appCntxt);
392
393 /* Verify graph */
394 if (vxStatus == (vx_status)VX_SUCCESS)
395 {
396 vxStatus = vxVerifyGraph(appCntxt->vxGraph);
397 if (vxStatus != (vx_status)VX_SUCCESS)
398 {
399 PTK_printf("[%s:%d] vxVerifyGraph() failed\n",
400 __FUNCTION__, __LINE__);
401 }
402 }
403
404 /* Set the MSC coefficients. */
405 if (vxStatus == (vx_status)VX_SUCCESS)
406 {
407 CM_ScalerNodeCntxt *scalerObj = &appCntxt->scalerObj;
408 vxStatus = CM_scalerNodeCntxtSetCoeff(scalerObj);
409
410 if (vxStatus != (vx_status)VX_SUCCESS)
411 {
412 PTK_printf("[%s:%d] SEMSEG_CNN_APPLIB_setCoeff() failed\n",
413 __FUNCTION__, __LINE__);
414 }
415 }
416 } else
417 {
418 vxStatus == (vx_status)VX_FAILURE;
419 }
420
421 if (vxStatus == (vx_status)VX_SUCCESS)
422 {
423 // init sacler for ML SDE
424 if (appCntxt->sdeAlgoType == 1)
425 {
426 ML_SDEAPPLIB_initScaler(appCntxt->mlSdeHdl);
427 }
428
429 if (appCntxt->exportGraph == 1)
430 {
431 tivxExportGraphToDot(appCntxt->vxGraph, ".", "vx_app_estop");
432 }
433
434 if (appCntxt->rtLogEnable == 1)
435 {
436 tivxLogRtTraceEnable(appCntxt->vxGraph);
437 }
438
439
440 appCntxt->exitOutputThread = false;
441 appCntxt->outputCtrlSem = new UTILS::Semaphore(0);
442
443 appCntxt->preProcSem = new UTILS::Semaphore(0);
444 appCntxt->exitPreprocThread = false;
445
446 appCntxt->dlrDataReadySem = new UTILS::Semaphore(0);
447 appCntxt->exitDlrThread = false;
448
449 appCntxt->postProcSem = new UTILS::Semaphore(0);
450 appCntxt->exitPostprocThread = false;
451
452 appCntxt->exitInputDataProcess = false;
453 appCntxt->state = ESTOP_APP_STATE_INIT;
454
455 ESTOP_APP_reset(appCntxt);
456 }
457
458 return vxStatus;
459}
460
461vx_status ESTOP_APP_init_camInfo(ESTOP_APP_Context *appCntxt,
462 uint32_t width,
463 uint32_t height,
464 double f,
465 double dx,
466 double dy)
467{
468 appCntxt->width = (uint16_t) width;
469 appCntxt->height = (uint16_t) height;
470 appCntxt->focalLength = (float)f;
471 appCntxt->distCenterX = (float)dx;
472 appCntxt->distCenterY = (float)dy;
473
474 return (vx_status)VX_SUCCESS;
475}
476
477vx_status ESTOP_APP_run(ESTOP_APP_Context *appCntxt,
478 const vx_uint8 * inputLeftImage, const vx_uint8 * inputRightImage,
479 vx_uint64 timestamp)
480{
481 uint32_t i;
482 vx_status vxStatus;
483
484 // For profiling
485 chrono::time_point<chrono::system_clock> start, end;
486 float diff;
487
488 ESTOP_APP_graphParams* gpDesc;
489
490 vxStatus = ESTOP_APP_popFreeInputDesc(appCntxt, &gpDesc);
491 if (vxStatus != (vx_status)VX_SUCCESS)
492 {
493 PTK_printf("[%s:%d] ESTOP_APP_popFreeInputDesc() failed\n",
494 __FUNCTION__, __LINE__);
495 }
496
497 if (vxStatus == (vx_status)VX_SUCCESS)
498 {
499 start = GET_TIME();
500 vxStatus = ptkdemo_copy_data_to_image(inputLeftImage, gpDesc->vxInputLeftImage);
501
502 end = GET_TIME();
503 diff = GET_DIFF(start, end);
504 CM_reportProctime("input_image_load", diff);
505
506 if (vxStatus != (vx_status)VX_SUCCESS)
507 {
508 PTK_printf("[%s:%d] ptkdemo_copy_data_to_image() failed\n",
509 __FUNCTION__, __LINE__);
510 }
511 }
512
513 if (vxStatus == (vx_status)VX_SUCCESS)
514 {
515 vxStatus = ptkdemo_copy_data_to_image(inputRightImage, gpDesc->vxInputRightImage);
516 if (vxStatus != (vx_status)VX_SUCCESS)
517 {
518 PTK_printf("[%s:%d] ptkdemo_copy_data_to_image() failed\n",
519 __FUNCTION__, __LINE__);
520 }
521 }
522
523 // then run AMR applib
524 if (vxStatus == (vx_status)VX_SUCCESS)
525 {
526 // set time stamp
527 *(gpDesc->timestamp) = timestamp;
528
529 vxStatus = ESTOP_APP_process(appCntxt, gpDesc);
530
531 if (vxStatus != (vx_status)VX_SUCCESS)
532 {
533 PTK_printf("[%s:%d] ESTOP_APP_process() failed\n",
534 __FUNCTION__, __LINE__);
535 }
536 }
537
538 return vxStatus;
539}
540
541void ESTOP_APP_deInit(ESTOP_APP_Context *appCntxt)
542{
543 int32_t status;
544 uint8_t i;
545
546 // release input image object
547 vxReleaseImage(&appCntxt->vxLeftRectImage);
548 for (i = 0; i < appCntxt->pipelineDepth; i++)
549 {
550 vxReleaseImage(&appCntxt->vxInputLeftImage[i]);
551 vxReleaseImage(&appCntxt->vxInputRightImage[i]);
552 vxReleaseImage(&appCntxt->vxRightRectImage[i]);
553 }
554
555 // release dispairty object
556 if (appCntxt->sdeAlgoType == 0)
557 {
558 for (i = 0; i < appCntxt->pipelineDepth; i++)
559 {
560 vxReleaseImage(&appCntxt->vxSde16BitOutput[i]);
561 }
562 } else
563 {
564 if (appCntxt->ppMedianFilterEnable)
565 {
566 for (i = 0; i < appCntxt->pipelineDepth; i++)
567 {
568 vxReleaseImage(&appCntxt->vxMedianFilteredDisparity[i]);
569 }
570 }
571
572 for (i = 0; i < appCntxt->pipelineDepth; i++)
573 {
574 vxReleaseImage(&appCntxt->vxMergeDisparityL0[i]);
575 }
576 }
577
578 // release graph
579 vxReleaseGraph(&appCntxt->vxGraph);
580
581 // Unload HWA kernels
582 tivxStereoUnLoadKernels(appCntxt->vxContext);
583 tivxImgProcUnLoadKernels(appCntxt->vxContext);
584 tivxHwaUnLoadKernels(appCntxt->vxContext);
585
586 /* Release the context. */
587 vxReleaseContext(&appCntxt->vxContext);
588
589 /* Delete the DLR Model handle. */
590 status = CM_dlrNodeCntxtDeInit(&appCntxt->dlrObj);
591
592 if (status < 0)
593 {
594 PTK_printf("[%s:%d] CM_dlrNodeCntxtDeInit() failed.\n",
595 __FUNCTION__, __LINE__);
596 }
597
598#if 0
599 status = appDeInit();
600 PTK_assert(status == 0);
601#endif
602}
603
604static void ESTOP_APP_exitProcThreads(ESTOP_APP_Context *appCntxt)
605{
606 vx_status vxStatus;
607
608 appCntxt->exitInputDataProcess = true;
609
610 /* Let the event handler thread exit. */
611 vxStatus = vxSendUserEvent(appCntxt->vxContext,
612 ESTOP_APP_USER_EVT_EXIT,
613 NULL);
614
615 if (vxStatus != VX_SUCCESS)
616 {
617 PTK_printf("[%s:%d] vxSendUserEvent() failed.\n");
618 }
619
620 /* Set the exit flag for the pre-process thread. */
621 appCntxt->exitPreprocThread = true;
622
623 /* Wake-up the pre-process thread. */
624 if (appCntxt->preProcSem)
625 {
626 appCntxt->preProcSem->notify();
627 }
628
629 /* Set the exit flag for the DLR thread. */
630 appCntxt->exitDlrThread = true;
631
632 /* Wake-up the DLR thread. */
633 if (appCntxt->dlrDataReadySem)
634 {
635 appCntxt->dlrDataReadySem->notify();
636 }
637
638 /* Set the exit flag for the post-process thread. */
639 appCntxt->exitPostprocThread = true;
640
641 /* Wake-up the post-process thread. */
642 if (appCntxt->postProcSem)
643 {
644 appCntxt->postProcSem->notify();
645 }
646
647 /* Let the display thread exit. */
648 appCntxt->exitOutputThread = true;
649
650 if (appCntxt->outputCtrlSem)
651 {
652 appCntxt->outputCtrlSem->notify();
653 }
654
655
656 /* Wait for the thread exit */
657 if (appCntxt->evtHdlrThread.joinable())
658 {
659 appCntxt->evtHdlrThread.join();
660 }
661
662 if (appCntxt->preProcThread.joinable())
663 {
664 appCntxt->preProcThread.join();
665 }
666
667 if (appCntxt->dlrThread.joinable())
668 {
669 appCntxt->dlrThread.join();
670 }
671
672 if (appCntxt->postProcThread.joinable())
673 {
674 appCntxt->postProcThread.join();
675 }
676
677
678 /* Delete the semaphores. */
679 if (appCntxt->outputCtrlSem)
680 {
681 delete appCntxt->outputCtrlSem;
682 }
683
684 if (appCntxt->preProcSem)
685 {
686 delete appCntxt->preProcSem;
687 }
688
689 if (appCntxt->dlrDataReadySem)
690 {
691 delete appCntxt->dlrDataReadySem;
692 }
693
694 if (appCntxt->postProcSem)
695 {
696 delete appCntxt->postProcSem;
697 }
698
699}
700
701void ESTOP_APP_cleanupHdlr(ESTOP_APP_Context *appCntxt)
702{
703 if (appCntxt->state == ESTOP_APP_STATE_INVALID)
704 {
705 return;
706 }
707
708 appCntxt->state = ESTOP_APP_STATE_INVALID;
709
710 /* Wait for the threads to exit. */
711 ESTOP_APP_exitProcThreads(appCntxt);
712
713 PTK_printf("\nPress ENTER key to exit.\n");
714 fflush(stdout);
715 getchar();
716
717 PTK_printf("========= BEGIN:PERFORMANCE STATS SUMMARY =========\n");
718 appPerfStatsPrintAll();
719 ESTOP_APP_printStats(appCntxt);
720
721 CM_printProctime();
722 PTK_printf("========= END:PERFORMANCE STATS SUMMARY ===========\n\n");
723
724 if (appCntxt->rtLogEnable == 1)
725 {
726 char name[256];
727
728 snprintf(name, 255, "%s.bin", ESTOP_APP_PERF_OUT_FILE);
729 tivxLogRtTraceExportToFile(name);
730 }
731
732 /* Release the objects. */
733 SDELDCAPPLIB_delete(&appCntxt->sdeLdcHdl);
734
735 if (appCntxt->sdeAlgoType == 0)
736 {
737 SL_SDEAPPLIB_delete(&appCntxt->slSdeHdl);
738 }
739 else if (appCntxt->sdeAlgoType == 1)
740 {
741 ML_SDEAPPLIB_delete(&appCntxt->mlSdeHdl);
742 }
743
744 SS_DETECT_APPLIB_delete(&appCntxt->ssDetectHdl);
745
746 ESTOP_APP_deinit_SS(appCntxt);
747
748 ESTOP_APP_deInit(appCntxt);
749
750 PTK_printf("[%s] Clean-up complete.\n", __FUNCTION__);
751}
752
753void ESTOP_APP_reset(ESTOP_APP_Context * appCntxt)
754{
755 vx_status vxStatus;
756
757 vx_node ogNode = SS_DETECT_APPLIB_getOGNode(appCntxt->ssDetectHdl);
758 vxStatus = tivxNodeSendCommand(ogNode,
759 0,
760 TIVX_KERNEL_OCCUPANCY_GRID_DETECTION_RESET,
761 NULL,
762 0);
763 if (vxStatus != VX_SUCCESS)
764 {
765 PTK_printf("[%s:%d] tivxNodeSendCommand() failed.\n",
766 __FUNCTION__,
767 __LINE__);
768
769 }
770
771 appCntxt->startPerfCapt = false;
772}
773
774static void ESTOP_APP_evtHdlrThread(ESTOP_APP_Context *appCntxt)
775{
776 vx_event_t evt;
777 vx_status vxStatus = VX_SUCCESS;
778
779 PTK_printf("[%s] Launched.\n", __FUNCTION__);
780
781 /* Clear any pending events. The third argument is do_not_block = true. */
782 while (vxStatus == VX_SUCCESS)
783 {
784 vxStatus = vxWaitEvent(appCntxt->vxContext, &evt, vx_true_e);
785 }
786
787 while (true)
788 {
789 vxStatus = vxWaitEvent(appCntxt->vxContext, &evt, vx_false_e);
790
791 if (vxStatus == VX_SUCCESS)
792 {
793 if (evt.type == VX_EVENT_USER)
794 {
795 if (evt.app_value == ESTOP_APP_USER_EVT_EXIT)
796 {
797 break;
798 }
799 else if (evt.app_value == ESTOP_APP_CNN_OUT_AVAIL_EVENT)
800 {
801 ESTOP_APP_graphParams * desc;
802 vxStatus = ESTOP_APP_getPostprocInputDesc(appCntxt, &desc);
803
804 // enqueue vxOutTensor
805 vxStatus = vxGraphParameterEnqueueReadyRef(appCntxt->vxGraph,
806 appCntxt->numGraphParams - 1,
807 (vx_reference*)&desc->vxOutTensor,
808 1);
809 if (vxStatus != (vx_status)VX_SUCCESS)
810 {
811 PTK_printf("[%s:%d] vxGraphParameterEnqueueReadyRef(%d) "
812 "failed\n", __FUNCTION__, __LINE__, appCntxt->numGraphParams - 1);
813
814 }
815 }
816 }
817
818 if (evt.type == VX_EVENT_GRAPH_COMPLETED || evt.type == VX_EVENT_NODE_COMPLETED)
819 {
820 ESTOP_APP_processEvent(appCntxt, &evt);
821 }
822 }
823
824 } // while (true)
825
826 PTK_printf("[%s] Exiting.\n", __FUNCTION__);
827}
828
829
830static int32_t ESTOP_APP_userControlThread(ESTOP_APP_Context *appCntxt)
831{
832 int32_t status;
833 vx_status vxStatus = VX_SUCCESS;
834
835 uint32_t done = 0;
836
837 appPerfStatsResetAll();
838
839 while (!done)
840 {
841 char ch;
842
843 PTK_printf(menu);
844 ch = getchar();
845 PTK_printf("\n");
846
847 switch (ch)
848 {
849 case 'p':
850 appPerfStatsPrintAll();
851 ESTOP_APP_printStats(appCntxt);
852
853 CM_printProctime();
854 CM_resetProctime();
855 break;
856
857 case 'e':
858 {
859 FILE *fp;
860 const char *name = ESTOP_APP_PERF_OUT_FILE;
861
862 fp = appPerfStatsExportOpenFile(".", (char *)name);
863
864 if (fp != NULL)
865 {
866 ESTOP_APP_exportStats(appCntxt, fp, true);
867
868 appPerfStatsExportCloseFile(fp);
869 }
870 else
871 {
872 PTK_printf("Could not open [%s] for exporting "
873 "performance data\n", name);
874 }
875 }
876 break;
877
878 case 'x':
879 done = 1;
880 break;
881 } // switch(ch)
882
883 /* Consume the newline character. */
884 if (ch != '\n')
885 {
886 getchar();
887 }
888 } // while (!done)
889
890 appCntxt->state = ESTOP_APP_STATE_SHUTDOWN;
891 PTK_printf("[%s:%d] Waiting for the graph to finish.\n",
892 __FUNCTION__, __LINE__);
893
894 vxStatus = ESTOP_APP_waitGraph(appCntxt);
895
896 if (vxStatus != (vx_status)VX_SUCCESS)
897 {
898 PTK_printf("[%s:%d] ESTOP_APP_waitGraph() failed\n",
899 __FUNCTION__, __LINE__);
900 }
901
902 ESTOP_APP_cleanupHdlr(appCntxt);
903
904 PTK_printf("\nDEMO FINISHED!\n");
905
906 return vxStatus;
907}
908
909static void ESTOP_APP_preProcThread(ESTOP_APP_Context *appCntxt)
910{
911 // For profiling
912 chrono::time_point<chrono::system_clock> start, end;
913 float diff;
914
915 PTK_printf("[%s] Launched.\n", __FUNCTION__);
916
917 while (true)
918 {
919 ESTOP_APP_graphParams *desc;
920 vx_status vxStatus;
921
922 /* Wait for the input buffer availability. */
923 appCntxt->preProcSem->wait();
924
925 /* Check if we need to exit the thread. */
926 if (appCntxt->exitPreprocThread == true)
927 {
928 break;
929 }
930
931 vxStatus = ESTOP_APP_popPreprocInputDesc(appCntxt, &desc);
932
933 if (vxStatus == (vx_status)VX_SUCCESS)
934 {
935 start = GET_TIME();
936 vxStatus =
937 ESTOP_APP_CNN_preProcess(appCntxt,
938 desc->vxScalerOut,
939 desc->dlrInputBuff);
940
941 end = GET_TIME();
942 diff = GET_DIFF(start, end);
943 CM_reportProctime("Preprocessing", diff);
944
945 if (vxStatus != (vx_status)VX_SUCCESS)
946 {
947 PTK_printf("[%s:%d] ESTOP_APP_CNN_preProcess() "
948 "failed.\n",
949 __FUNCTION__, __LINE__);
950 }
951 else
952 {
953 /* Push the descriptor to the DLR queue. */
954 ESTOP_APP_enqueDLRInputDesc(appCntxt, desc);
955
956 /* Wakeup the DLR thread. The DLR thread will process the
957 * descriptor at the head of the queue.
958 */
959 appCntxt->dlrDataReadySem->notify();
960 }
961
962 } // if (vxStatus == (vx_status)VX_SUCCESS)
963
964 } // while (true)
965
966 PTK_printf("[%s] Exiting.\n", __FUNCTION__);
967}
968
969
970static void ESTOP_APP_dlrThread(ESTOP_APP_Context *appCntxt)
971{
972 CM_DLRNodeCntxt *dlrObj;
973 CM_DLRNodeInputInfo *dlrInput;
974 CM_DLRNodeOutputInfo *dlrOutput;
975
976 // For profiling
977 chrono::time_point<chrono::system_clock> start, end;
978 float diff;
979
980 dlrObj = &appCntxt->dlrObj;
981 dlrInput = &dlrObj->input;
982 dlrOutput = &dlrObj->output;
983
984 PTK_printf("[%s] Launched.\n", __FUNCTION__);
985
986 while (true)
987 {
988 ESTOP_APP_graphParams *desc;
989 vx_status vxStatus;
990
991 /* Wait fot the input buffer. */
992 appCntxt->dlrDataReadySem->wait();
993
994 if (appCntxt->exitDlrThread == true)
995 {
996 break;
997 }
998
999 vxStatus = ESTOP_APP_popDLRInputDesc(appCntxt, &desc);
1000
1001 if (vxStatus == (vx_status)VX_SUCCESS)
1002 {
1003 int32_t status;
1004
1005 dlrInput->info[0].data = desc->dlrInputBuff;
1006 dlrOutput->info[0].data = desc->dlrOutputBuff;
1007
1008 start = GET_TIME();
1009 status = CM_dlrNodeCntxtProcess(dlrObj, dlrInput, dlrOutput);
1010
1011 end = GET_TIME();
1012 diff = GET_DIFF(start, end);
1013 CM_reportProctime("DLR-node", diff);
1014
1015 if (status < 0)
1016 {
1017 PTK_printf("[%s:%d] CM_dlrNodeCntxtProcess() failed.\n",
1018 __FUNCTION__, __LINE__);
1019 }
1020 else
1021 {
1022 /* Push the descriptor to the DLR queue. */
1023 ESTOP_APP_enquePostprocInputDesc(appCntxt, desc);
1024
1025 /* Wakeup the post-process thread. The post-process thread will
1026 * process the descriptor at the head of the queue.
1027 */
1028 appCntxt->postProcSem->notify();
1029 }
1030
1031 } // if (vxStatus == (vx_status)VX_SUCCESS)
1032
1033 } // while (true)
1034
1035 PTK_printf("[%s] Exiting.\n", __FUNCTION__);
1036
1037}
1038
1039static void ESTOP_APP_postProcThread(ESTOP_APP_Context *appCntxt)
1040{
1041 // For profiling
1042 chrono::time_point<chrono::system_clock> start, end;
1043 float diff;
1044
1045 PTK_printf("[%s] Launched.\n", __FUNCTION__);
1046
1047 while (true)
1048 {
1049 ESTOP_APP_graphParams *desc;
1050 vx_status vxStatus;
1051
1052 /* Wait for the input buffer availability. */
1053 appCntxt->postProcSem->wait();
1054
1055 /* Check if we need to exit the thread. */
1056 if (appCntxt->exitPostprocThread == true)
1057 {
1058 break;
1059 }
1060
1061 vxStatus = ESTOP_APP_getPostprocInputDesc(appCntxt, &desc);
1062
1063 if (vxStatus == (vx_status)VX_SUCCESS)
1064 {
1065 if (appCntxt->enablePostProcNode)
1066 {
1067 /* Create the output object for display. */
1068 start = GET_TIME();
1069 vxStatus =
1070 ESTOP_APP_CNN_postProcess(appCntxt,
1071 desc->vxScalerOut,
1072 desc->dlrOutputBuff);
1073
1074 end = GET_TIME();
1075 diff = GET_DIFF(start, end);
1076 CM_reportProctime("Postprocess", diff);
1077
1078 if (vxStatus != (vx_status)VX_SUCCESS)
1079 {
1080 PTK_printf("[%s:%d] SEMSEG_CNN_postProcess() "
1081 "failed.\n",
1082 __FUNCTION__, __LINE__);
1083 }
1084 }
1085
1086 // we should create output tensor always
1087 {
1088 /* Create an output tensor. */
1089 start = GET_TIME();
1090 vxStatus =
1091 ESTOP_APP_createOutTensor(appCntxt,
1092 desc->vxOutTensor,
1093 desc->dlrOutputBuff);
1094
1095 end = GET_TIME();
1096 diff = GET_DIFF(start, end);
1097 CM_reportProctime("Output_tensor_creation", diff);
1098
1099 if (vxStatus != (vx_status)VX_SUCCESS)
1100 {
1101 PTK_printf("[%s:%d] SEMSEG_CNN_createOutTensor() "
1102 "failed.\n",
1103 __FUNCTION__, __LINE__);
1104 }
1105 }
1106
1107 if (vxStatus == (vx_status)VX_SUCCESS)
1108 {
1109 /* Let the usr know that output is ready. */
1110 vxStatus =
1111 vxSendUserEvent(appCntxt->vxContext,
1112 ESTOP_APP_CNN_OUT_AVAIL_EVENT,
1113 NULL);
1114
1115 if (vxStatus != (vx_status)VX_SUCCESS)
1116 {
1117 PTK_printf("[%s:%d] vxSendUserEvent() failed.\n",
1118 __FUNCTION__, __LINE__);
1119 }
1120 }
1121
1122 } // if (vxStatus == (vx_status)VX_SUCCESS)
1123
1124 } // while (true)
1125
1126 PTK_printf("[%s] Exiting.\n", __FUNCTION__);
1127}
1128
1129
1130void ESTOP_APP_launchProcThreads(ESTOP_APP_Context *appCntxt)
1131{
1132 /* Launch the input data thread. */
1133 if (appCntxt->is_interactive)
1134 {
1135 appCntxt->userCtrlThread =
1136 std::thread(ESTOP_APP_userControlThread, appCntxt);
1137
1138 appCntxt->userCtrlThread.detach();
1139 }
1140
1141 /* Launch the event handler thread. */
1142 appCntxt->evtHdlrThread =
1143 std::thread(ESTOP_APP_evtHdlrThread, appCntxt);
1144
1145 /* Launch CNN pre-processing thread */
1146 appCntxt->preProcThread =
1147 std::thread(ESTOP_APP_preProcThread, appCntxt);
1148
1149 /* Launch the dlr thread */
1150 appCntxt->dlrThread =
1151 std::thread(ESTOP_APP_dlrThread, appCntxt);
1152
1153 /* launch post-processing thread. */
1154 appCntxt->postProcThread =
1155 std::thread(ESTOP_APP_postProcThread, appCntxt);
1156}
1157
1158void ESTOP_APP_intSigHandler(ESTOP_APP_Context *appCntxt)
1159{
1160 if (appCntxt->state != ESTOP_APP_STATE_INVALID)
1161 {
1162 /* Wait for the threads to exit. */
1163 vx_status vxStatus = VX_SUCCESS;
1164
1165 appCntxt->state = ESTOP_APP_STATE_SHUTDOWN;
1166 PTK_printf("[%s:%d] Waiting for the graph to finish.\n",
1167 __FUNCTION__, __LINE__);
1168
1169 vxStatus = ESTOP_APP_waitGraph(appCntxt);
1170
1171 if (vxStatus != (vx_status)VX_SUCCESS)
1172 {
1173 PTK_printf("[%s:%d] ESTOP_APP_waitGraph() failed\n",
1174 __FUNCTION__, __LINE__);
1175 }
1176
1177 ESTOP_APP_cleanupHdlr(appCntxt);
1178 PTK_printf("\nDEMO FINISHED!\n");
1179 }
1180
1181 exit(0);
1182}
generated by cgit v1.2.3 (git 2.39.2)