bool APMrover_base::init(void* pKiss) { CHECK_F(this->ActionBase::init(pKiss)==false); Kiss* pK = (Kiss*)pKiss; pK->m_pInst = this; Kiss* pCC; APMrover_PID cPID; pCC = pK->o("steer"); CHECK_F(pCC->empty()); F_ERROR_F(pCC->v("P", &cPID.m_P)); F_INFO(pCC->v("I", &cPID.m_I)); F_INFO(pCC->v("Imax", &cPID.m_Imax)); F_INFO(pCC->v("D", &cPID.m_D)); F_INFO(pCC->v("dT", &cPID.m_dT)); m_pidSteer = cPID; pCC = pK->o("thrust"); CHECK_F(pCC->empty()); F_ERROR_F(pCC->v("P", &cPID.m_P)); F_INFO(pCC->v("I", &cPID.m_I)); F_INFO(pCC->v("Imax", &cPID.m_Imax)); F_INFO(pCC->v("D", &cPID.m_D)); F_INFO(pCC->v("dT", &cPID.m_dT)); m_pidThrust = cPID; //init controls m_lastHeartbeat = 0; m_iHeartbeat = 0; return true; }
bool _Automaton::init(void* pKiss) { CHECK_F(!this->_ThreadBase::init(pKiss)); Kiss* pK = (Kiss*)pKiss; pK->m_pInst = this; //create state instances Kiss** pItr = pK->getChildItr(); int i = 0; while (pItr[i]) { Kiss* pState = pItr[i++]; CHECK_F(m_nState >= N_STATE); F_ERROR_F(pState->v("state", &m_pStateName[m_nState])); m_nState++; } string startState = ""; F_ERROR_F(pK->v("startState", &startState)); m_iState = getStateIdx(&startState); CHECK_F(m_iState<0); return true; }
void throw_on_fatal() { loguru::set_fatal_handler([](const loguru::Message& message){ LOG_F(INFO, "Throwing exception..."); throw std::runtime_error(std::string(message.prefix) + message.message); }); { LOG_SCOPE_F(INFO, "CHECK_F throw + catch"); try { CHECK_F(false, "some CHECK_F message"); } catch (std::runtime_error& e) { LOG_F(INFO, "CHECK_F threw this: '%s'", e.what()); } } #if LOGURU_WITH_STREAMS { LOG_SCOPE_F(INFO, "CHECK_S throw + catch"); try { CHECK_S(false) << "Some CHECK_S message"; } catch (std::runtime_error& e) { LOG_F(INFO, "CHECK_S threw this: '%s'", e.what()); } } LOG_F(INFO, "Trying an uncaught exception:"); CHECK_S(false); #else CHECK_F(false); #endif // LOGURU_WITH_STREAMS }
bool _Automaton::transit(int nextStateIdx) { CHECK_F(nextStateIdx < 0); CHECK_F(nextStateIdx >= m_nState); m_iState = nextStateIdx; return true; }
VAO::VAO() { CHECK_FOR_GL_ERROR; CHECK_F(glGenVertexArraysAPPLE != nullptr); CHECK_F(glDeleteVertexArraysAPPLE != nullptr); CHECK_F(glBindVertexArrayAPPLE != nullptr); glGenVertexArraysAPPLE(1, &_id); CHECK_FOR_GL_ERROR; }
Texture* load_uncompressed_pvr_from_memory( const void* data, size_t num_bytes, TexParams params, std::string debug_name) { PvrHeader* header = (PvrHeader*)data; CHECK_F(strncmp(header->pvr_tag, "PVR!", 4) == 0, "Not a .pvr file"); uint32_t flags = header->flags; uint32_t format_flag = flags & 0xFF; const uint8_t* data_start = (const uint8_t*)data + sizeof(PvrHeader); ImageFormat format = ImageFormat::BGRA32; if (format_flag == kA8) { format = ImageFormat::Alpha8; } else { CHECK_F(format_flag == kBGRA8888, "PVR: kBGRA8888 (%x) expected, got %x", kBGRA8888, format_flag); } Size size{header->width, header->height}; CHECK_GT_F(header->mipmap_count, 0); if (params.filter == TexFilter::Mipmapped && !supports_mipmaps_for(size)) { params.filter = TexFilter::Linear; } #if 0 return new Texture(data_start, size, format, params, std::move(debug_name)); #else if (params.filter == TexFilter::Nearest || params.filter == TexFilter::Linear) { return new Texture(data_start, size, format, params, std::move(debug_name)); } else if (header->mipmap_count == 1) { params.filter = TexFilter::Linear; return new Texture(data_start, size, format, params, std::move(debug_name)); } else { params.filter = TexFilter::Mipmapped; auto tex = new Texture(nullptr, size, format, params, std::move(debug_name)); auto bytes_per_pixel = format_size(format); for (unsigned level=0; level<header->mipmap_count; ++level) { tex->set_mip_data(data_start, size, level); data_start += size.x * size.y * bytes_per_pixel; size.x = std::max(1u, size.x/2); size.y = std::max(1u, size.y/2); } return tex; } #endif }
int main(int argc, char* argv[]) { //Init Logger FLAGS_logtostderr = 1; google::InitGoogleLogging("OpenKAI"); printEnvironment(); //Load config LOG(INFO)<<"Kiss file:"<<argv[1]; string kissFile(argv[1]); CHECK_F(!g_file.open(&kissFile)); string* pKiss = g_file.readAll(); if(pKiss==NULL) { LOG(FATAL)<<"Kiss file not found"; return 1; } g_pKiss = new Kiss(); if(!g_pKiss->parse(pKiss)) { LOG(FATAL)<<"Kiss file parsing failed"; return 1; } g_file.close(); //Start Application g_pStart = new Startup(); g_pStart->start(g_pKiss); return 0; }
void ThreadPool::_thread_worker(size_t thread_nr) { char thread_name[32]; snprintf(thread_name, sizeof(thread_name) - 1, "pool_worker_%lu", thread_nr); loguru::set_thread_name(thread_name); while (true) { Job job; { std::unique_lock<std::mutex> lock(_mutex); _new_job_cond.wait(lock, [this]{ return !_job_queue.empty(); }); CHECK_F(!_job_queue.empty()); job = std::move(_job_queue.front()); _job_queue.pop_front(); } if (!job) { break; } job(); std::unique_lock<std::mutex> lock(_mutex); --_num_unfinished_jobs; _job_finished_cond.notify_all(); } }
bool _Flow::init(void* pKiss) { CHECK_F(!this->_ThreadBase::init(pKiss)); Kiss* pK = (Kiss*)pKiss; pK->m_pInst = this; string presetDir = ""; string labelFile; F_INFO(pK->root()->o("APP")->v("presetDir", &presetDir)); F_INFO(pK->v("bDepth", &m_bDepth)); F_INFO(pK->v("width", &m_width)); F_INFO(pK->v("height", &m_height)); F_INFO(pK->v("flowMax", &m_flowMax)); F_INFO(pK->v("colorFile", &labelFile)); m_pDepth = new Frame(); m_pFarn = cuda::FarnebackOpticalFlow::create(); m_pGrayFrames = new FrameGroup(); m_pGrayFrames->init(2); m_GDMat = GpuMat(m_height, m_width, CV_32FC1, Scalar(0)); return true; // m_labelColor = imread(presetDir+labelFile, 1); // m_pGpuLUT = cuda::createLookUpTable(m_labelColor); // m_pSeg = new CamFrame(); // m_flowMat = GpuMat(SMALL_WIDTH, SMALL_HEIGHT, CV_32FC2); }
bool _SSD::init(void* pKiss) { CHECK_F(!this->_ThreadBase::init(pKiss)); Kiss* pK = (Kiss*)pKiss; pK->m_pInst = this; //Setup Caffe Classifier string caffeDir = ""; string modelFile; string trainedFile; string meanFile; string labelFile; string presetDir = ""; F_INFO(pK->root()->o("APP")->v("presetDir", &presetDir)); F_INFO(pK->v("dir", &caffeDir)); F_FATAL_F(pK->v("modelFile", &modelFile)); F_FATAL_F(pK->v("trainedFile", &trainedFile)); F_FATAL_F(pK->v("meanFile", &meanFile)); F_FATAL_F(pK->v("labelFile", &labelFile)); F_INFO(pK->v("minConfidence", &m_confidence_threshold)); setup(caffeDir + modelFile, caffeDir + trainedFile, caffeDir + meanFile, presetDir + labelFile); LOG_I("Initialized"); m_pFrame = new Frame(); return true; }
void ThreadPool::add_void(const Job& job) { CHECK_F(!!job); std::lock_guard<std::mutex> lock(_mutex); _job_queue.push_back(job); ++_num_unfinished_jobs; _new_job_cond.notify_one(); }
bool _SSD::draw(void) { CHECK_F(!this->_ThreadBase::draw()); Window* pWin = (Window*)this->m_pWindow; Mat* pMat = pWin->getFrame()->getCMat(); return true; }
int NS(SendF) (NS_ARGS) { SETUP_mqctx MQ_FLT val; CHECK_F(val) CHECK_NOARGS ErrorMqToTclWithCheck(MqSendF(mqctx, val)); RETURN_TCL }
bool _Automaton::transit(string* pNextStateName) { NULL_F(pNextStateName); int iNext = getStateIdx(pNextStateName); CHECK_F(iNext<0); m_iState = iNext; return true; }
size_t HUF_compressWeights (void* dst, size_t dstSize, const void* weightTable, size_t wtSize) { BYTE* const ostart = (BYTE*) dst; BYTE* op = ostart; BYTE* const oend = ostart + dstSize; U32 maxSymbolValue = HUF_TABLELOG_MAX; U32 tableLog = MAX_FSE_TABLELOG_FOR_HUFF_HEADER; FSE_CTable CTable[FSE_CTABLE_SIZE_U32(MAX_FSE_TABLELOG_FOR_HUFF_HEADER, HUF_TABLELOG_MAX)]; BYTE scratchBuffer[1<<MAX_FSE_TABLELOG_FOR_HUFF_HEADER]; U32 count[HUF_TABLELOG_MAX+1]; S16 norm[HUF_TABLELOG_MAX+1]; /* init conditions */ if (wtSize <= 1) return 0; /* Not compressible */ /* Scan input and build symbol stats */ { CHECK_V_F(maxCount, FSE_count_simple(count, &maxSymbolValue, weightTable, wtSize) ); if (maxCount == wtSize) return 1; /* only a single symbol in src : rle */ if (maxCount == 1) return 0; /* each symbol present maximum once => not compressible */ } tableLog = FSE_optimalTableLog(tableLog, wtSize, maxSymbolValue); CHECK_F( FSE_normalizeCount(norm, tableLog, count, wtSize, maxSymbolValue) ); /* Write table description header */ { CHECK_V_F(hSize, FSE_writeNCount(op, oend-op, norm, maxSymbolValue, tableLog) ); op += hSize; } /* Compress */ CHECK_F( FSE_buildCTable_wksp(CTable, norm, maxSymbolValue, tableLog, scratchBuffer, sizeof(scratchBuffer)) ); { CHECK_V_F(cSize, FSE_compress_usingCTable(op, oend - op, weightTable, wtSize, CTable) ); if (cSize == 0) return 0; /* not enough space for compressed data */ op += cSize; } return op-ostart; }
bool APMrover_base::link(void) { CHECK_F(!this->ActionBase::link()); Kiss* pK = (Kiss*)m_pKiss; string iName = ""; F_INFO(pK->v("_Mavlink", &iName)); m_pMavlink = (_Mavlink*) (pK->root()->getChildInstByName(&iName)); return true; }
bool _Universe::init(void* pKiss) { CHECK_F(this->_ThreadBase::init(pKiss) == false); Kiss* pK = (Kiss*)pKiss; pK->m_pInst = this; F_ERROR_F(pK->v("frameLifetime", &m_frameLifeTime)); F_ERROR_F(pK->v("probMin", &m_objProbMin)); F_ERROR_F(pK->v("posDisparity", &m_disparity)); return true; }
bool _Bullseye::link(void) { CHECK_F(!this->_ThreadBase::link()); Kiss* pK = (Kiss*)m_pKiss; //link instance string iName = ""; F_ERROR_F(pK->v("_Stream",&iName)); m_pStream = (_StreamBase*)(pK->root()->getChildInstByName(&iName)); return true; }
void init_glew() { #if !GLLIB_GLES #ifndef GLEW_OK # error "No GLEW!" #endif static bool s_initialized = false; if (s_initialized) { return; } s_initialized = true; LOG_SCOPE_FUNCTION(INFO); CHECK_FOR_GL_ERROR; glewExperimental = true; GLenum glewErr = glewInit(); CHECK_F(glewErr == GLEW_OK, "Failed to initialize GLEW: %s", glewGetErrorString(glewErr)); glGetError(); // glew sometimes produces faux GL_INVALID_ENUM CHECK_FOR_GL_ERROR; CHECK_NOTNULL_F(glCreateProgram); char gl_version_string[64]; snprintf(gl_version_string, sizeof(gl_version_string) - 1, "GL_VERSION_%d_%d", GLLIB_OPENGL_VERSION / 100, (GLLIB_OPENGL_VERSION / 10) % 10); CHECK_F(glewIsSupported(gl_version_string), "Not supported: %s", gl_version_string); // CHECK_F(glewIsSupported("GL_VERSION_3_2")); if (glDebugMessageCallbackARB) { LOG_F(INFO, "ARB_debug_output supported"); glDebugMessageCallbackARB( on_gl_error, nullptr ); glEnable( GL_DEBUG_OUTPUT_SYNCHRONOUS_ARB ); } else { LOG_F(INFO, "ARB_debug_output not supported"); } #endif // !GLLIB_GLES CHECK_FOR_GL_ERROR; }
bool _Automaton::draw(void) { CHECK_F(!this->BASE::draw()); Window* pWin = (Window*)this->m_pWindow; Mat* pMat = pWin->getFrame()->getCMat(); putText(*pMat, *this->getName()+" State: " + m_pStateName[m_iState], *pWin->getTextPos(), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 1); pWin->lineNext(); return true; }
bool _AutoPilot::draw(void) { CHECK_F(!this->_ThreadBase::draw()); Window* pWin = (Window*)this->m_pWindow; Mat* pMat = pWin->getFrame()->getCMat(); for(int i=0; i<m_nAction; i++) { m_pAction[i]->draw(); } return true; }
bool _Mavlink::init(void* pKiss) { CHECK_F(!this->_ThreadBase::init(pKiss)); Kiss* pK = (Kiss*) pKiss; pK->m_pInst = this; Kiss* pCC = pK->o("input"); CHECK_F(pCC->empty()); m_pSerialPort = new SerialPort(); CHECK_F(!m_pSerialPort->init(pCC)); //init param m_systemID = 1; m_componentID = MAV_COMP_ID_PATHPLANNER; m_type = MAV_TYPE_ONBOARD_CONTROLLER; m_targetComponentID = 0; m_msg.sysid = 0; m_msg.compid = 0; m_status.packet_rx_drop_count = 0; return true; }
bool HM_follow::link(void) { CHECK_F(this->ActionBase::link()==false); Kiss* pK = (Kiss*)m_pKiss; string iName = ""; F_INFO(pK->v("HM_base", &iName)); m_pHM = (HM_base*) (pK->parent()->getChildInstByName(&iName)); F_INFO(pK->v("_Universe", &iName)); m_pUniv = (_Universe*) (pK->root()->getChildInstByName(&iName)); return true; }
static size_t ZSTDMT_flushStream_internal(ZSTDMT_CCtx* zcs, ZSTD_outBuffer* output, unsigned endFrame) { size_t const srcSize = zcs->inBuff.filled - zcs->dictSize; if (srcSize) DEBUGLOG(4, "flushing : %u bytes left to compress", (U32)srcSize); if ( ((srcSize > 0) || (endFrame && !zcs->frameEnded)) && (zcs->nextJobID <= zcs->doneJobID + zcs->jobIDMask) ) { CHECK_F( ZSTDMT_createCompressionJob(zcs, srcSize, endFrame) ); } /* check if there is any data available to flush */ DEBUGLOG(5, "zcs->doneJobID : %u ; zcs->nextJobID : %u ", zcs->doneJobID, zcs->nextJobID); return ZSTDMT_flushNextJob(zcs, output, 1); }
char *read_cookie(const char *cookie_file) { char *home, *path, cookie[COOKIE_MAX_SIZE], *buf; ssize_t cookie_size; int fd; if (!cookie_file) { home = getenv("HOME"); if (!home) return 0; path = alloc_printf("%s/%s", home, DEFAULT_COOKIE_FILE_NAME); } else { path = alloc_printf("%s", cookie_file); } CHECK_F(fd = open(path, O_RDONLY), "open(\"%s\")", path); CHECK_F(cookie_size = read(fd, cookie, COOKIE_MAX_SIZE), "read(\"%s\")", path); close(fd); buf = (char *)malloc(cookie_size + 1); memcpy(buf, cookie, cookie_size); buf[cookie_size] = 0; DEBUG_V("cookie from \"%s\" is %zu bytes", path, cookie_size); free(path); return buf; }
size_t ZSTDMT_compressStream(ZSTDMT_CCtx* zcs, ZSTD_outBuffer* output, ZSTD_inBuffer* input) { size_t const newJobThreshold = zcs->dictSize + zcs->targetSectionSize + zcs->marginSize; if (zcs->frameEnded) return ERROR(stage_wrong); /* current frame being ended. Only flush is allowed. Restart with init */ if (zcs->nbThreads==1) return ZSTD_compressStream(zcs->cstream, output, input); /* fill input buffer */ { size_t const toLoad = MIN(input->size - input->pos, zcs->inBuffSize - zcs->inBuff.filled); memcpy((char*)zcs->inBuff.buffer.start + zcs->inBuff.filled, input->src, toLoad); input->pos += toLoad; zcs->inBuff.filled += toLoad; } if ( (zcs->inBuff.filled >= newJobThreshold) /* filled enough : let's compress */ && (zcs->nextJobID <= zcs->doneJobID + zcs->jobIDMask) ) { /* avoid overwriting job round buffer */ CHECK_F( ZSTDMT_createCompressionJob(zcs, zcs->targetSectionSize, 0) ); } /* check for data to flush */ CHECK_F( ZSTDMT_flushNextJob(zcs, output, (zcs->inBuff.filled == zcs->inBuffSize)) ); /* block if it wasn't possible to create new job due to saturation */ /* recommended next input size : fill current input buffer */ return zcs->inBuffSize - zcs->inBuff.filled; /* note : could be zero when input buffer is fully filled and no more availability to create new job */ }
bool ActionBase::link(void) { CHECK_F(!this->BASE::link()); Kiss* pK = (Kiss*)m_pKiss; string iName=""; F_INFO(pK->v("_Automaton", &iName)); m_pAM = (_Automaton*) (pK->root()->getChildInstByName(&iName)); CHECK_T(m_pAM==NULL); iName=""; CHECK_T(pK->v("activeState", &iName)==false); m_iActiveState = m_pAM->getStateIdx(&iName); return true; }
bool RC_visualFollow::link(void) { CHECK_F(!this->ActionBase::link()); Kiss* pK = (Kiss*)m_pKiss; string iName = ""; F_INFO(pK->v("_RC", &iName)); m_pRC = (_RC*) (pK->root()->getChildInstByName(&iName)); F_INFO(pK->v("RC_base", &iName)); m_pRCconfig = (RC_base*) (pK->root()->getChildInstByName(&iName)); F_ERROR_F(pK->v("ROItracker", &iName)); m_pROITracker = (_ROITracker*) (pK->root()->getChildInstByName(&iName)); return true; }
bool _AutoPilot::link(void) { CHECK_F(!this->BASE::link()); Kiss* pK = (Kiss*)m_pKiss; int i; for(i=0; i<m_nAction; i++) { ActionBase* pA = m_pAction[i]; F_ERROR_F(pA->link()); } string iName=""; F_INFO(pK->v("_Automaton", &iName)); m_pAM = (_Automaton*) (pK->root()->getChildInstByName(&iName)); return true; }
bool _SSD::link(void) { CHECK_F(!this->_ThreadBase::link()); Kiss* pK = (Kiss*)m_pKiss; string iName = ""; F_ERROR_F(pK->v("_Stream",&iName)); m_pStream = (_StreamBase*)(pK->root()->getChildInstByName(&iName)); F_ERROR_F(pK->v("_Universe",&iName)); m_pUniverse = (_Universe*)(pK->root()->getChildInstByName(&iName)); for(int i=0; i<labels_.size(); i++) { m_pUniverse->addObjClass(&labels_.at(i), 0); } return true; }