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;
}
