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;
}
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 _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 _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;
}
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;
}
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;
}
bool _ROITracker::link(void)
{
NULL_F(m_pKiss);
Kiss* pK = (Kiss*)m_pKiss;
//link instance
string iName = "";
F_ERROR_F(pK->v("_Stream",&iName));
m_pStream = (_StreamBase*)(pK->root()->getChildInstByName(&iName));
//TODO: link variables to Automaton
return true;
}
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;
}
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 RC_base::init(void* pKiss)
{
CHECK_F(this->ActionBase::init(pKiss)==false);
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
RC_PID cPID;
RC_CHANNEL RC;
Kiss* pCC;
pCC = pK->o("roll");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidRoll = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcRoll = RC;
pCC = pK->o("pitch");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidPitch = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcPitch = RC;
pCC = pK->o("alt");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidAlt = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcAlt = RC;
pCC = pK->o("yaw");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidYaw = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcYaw = RC;
return true;
}
