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;
}
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 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;
}
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;
}
/**
* @brief f_size - returns the size of the file
*
* @param f_name - name of the file including path
*
* @return success/fail
*/
signed long f_size(const char *f_name)
{
struct stat f_stat;
if (stat(f_name, &f_stat)) {
F_ERROR("Could not stat %s\n", f_name);
return FILE_ERROR;
}
F_INFO("File size is %d\n", (unsigned int)f_stat.st_size);
return f_stat.st_size;
}
/**
* @brief f_read - read from file
*
* @param buffer buffer
* @param read_size size to read
*
* @return num bytes read
*/
signed int f_read(unsigned char *buffer, unsigned int read_size)
{
int ret;
if (file == NULL) {
F_ERROR("file is not open\n");
return FILE_ERROR;
}
ret = fread(buffer, 1, read_size, file);
F_INFO("read operation returned %d\n", ret);
return ret;
}
/**
* @brief f_seek - seek to location in file
*
* @param offset in file
*
* @return status
*/
int f_seek(long offset)
{
int ret;
if (file == NULL) {
F_ERROR("file is not open\n");
return FILE_ERROR;
}
ret = fseek(file, offset, SEEK_SET);
F_INFO("seek operation returned %d\n", ret);
return ret;
}
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 _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);
}
/**
* @brief f_open - open the file
*
* @param f_name file name
*
* @return success/fail
*/
signed char f_open(const char *f_name)
{
if (file != NULL) {
F_ERROR("file %s already open\n", f_name);
return FILE_ERROR;
}
file = fopen(f_name, "r");
if (file == NULL) {
F_ERROR("could not open file %s\n", f_name);
return FILE_ERROR;
}
F_INFO("file %s opened\n", (char *)f_name);
return FILE_OK;
}
bool RC_visualFollow::init(void* pKiss)
{
CHECK_F(!this->ActionBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
m_roll.reset();
m_pitch.reset();
m_yaw.reset();
m_alt.reset();
F_ERROR_F(pK->v("targetX", &m_roll.m_targetPos));
F_ERROR_F(pK->v("targetY", &m_pitch.m_targetPos));
F_INFO(pK->v("ROIsizeFrom", &m_ROIsizeFrom));
F_INFO(pK->v("ROIsizeTo", &m_ROIsizeTo));
F_INFO(pK->v("ROIsizeStep", &m_ROIsizeStep));
//setup UI
Kiss* pC;
pC = pK->o("assist");
if (!pC->empty())
{
m_pUIassist = new UI();
F_FATAL_F(m_pUIassist->init(pC));
}
pC = pK->o("drawRect");
if (!pC->empty())
{
m_pUIdrawRect = new UI();
F_FATAL_F(m_pUIdrawRect->init(pC));
}
pK->m_pInst = this;
return true;
}
/**
* @brief f_close - close the file
*
* @return pass/fail
*/
signed char f_close(void)
{
if (file == NULL) {
F_ERROR("file is not open\n");
return FILE_ERROR;
}
if (fclose(file)) {
F_ERROR("could not close file!\n");
file = NULL;
return FILE_ERROR;
}
file = NULL;
F_INFO("file closed\n");
return FILE_OK;
}
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 _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 _AutoPilot::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
//create action instance
Kiss* pCC = pK->o("action");
CHECK_T(pCC->empty());
Kiss** pItr = pCC->getChildItr();
int i = 0;
while (pItr[i])
{
Kiss* pAction = pItr[i];
i++;
bool bInst = false;
F_INFO(pAction->v("bInst", &bInst));
if (!bInst)continue;
if (m_nAction >= N_ACTION)LOG(FATAL);
ActionBase** pA = &m_pAction[m_nAction];
m_nAction++;
//Add action modules below
ADD_ACTION(RC_visualFollow);
ADD_ACTION(APMcopter_landing);
ADD_ACTION(APMcopter_guided);
ADD_ACTION(HM_base);
ADD_ACTION(HM_follow);
ADD_ACTION(APMrover_base);
ADD_ACTION(APMrover_follow);
//Add action modules above
LOG_E("Unknown action class: "+pAction->m_class);
}
return true;
}
bool HM_follow::init(void* pKiss)
{
CHECK_F(this->ActionBase::init(pKiss) == false);
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
F_INFO(pK->v("targetX", &m_destX));
F_INFO(pK->v("targetY", &m_destY));
F_INFO(pK->v("targetArea", &m_destArea));
F_INFO(pK->v("targetClass", &m_targetClass));
F_INFO(pK->v("speedP", &m_speedP));
F_INFO(pK->v("steerP", &m_steerP));
F_INFO(pK->v("filterWindow", &m_filterWindow));
m_pTargetX->startMedian(m_filterWindow);
m_pTargetY->startMedian(m_filterWindow);
m_pTargetArea->startMedian(m_filterWindow);
return true;
}
bool _Bullseye::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
//format params
F_ERROR_F(pK->v("areaRatio", &m_areaRatio));
F_ERROR_F(pK->v("minSize", &m_minMarkerSize));
F_INFO(pK->v("method", &m_method));
F_INFO(pK->v("medBlueKsize", &m_kSize));
F_INFO(pK->v("HoughMinDist", &m_houghMinDist));
F_INFO(pK->v("HoughParam1", &m_houghParam1));
F_INFO(pK->v("HoughParam2", &m_houghParam2));
F_INFO(pK->v("HoughMinR", &m_houghMinR));
F_INFO(pK->v("HoughMaxR", &m_houghMaxR));
m_pFrame = new Frame();
return true;
}
