/**
* \en
* Added value to end tag with name `sname'. Don't deletes tag.
* \_en
* \ru
* Рекурсивная функция поиска и замены тегов на их значения в node.
* Не заменяет теги, а дописывает значения в конец.
* Для удаления тэгов используйте функцию cleanUpTags()
* \_ru
* \param node - \en context for searching \_en \ru узел, с которого осуществляется поиск. \_ru
* \param sname - \en tag name \_en \ru имя тега для поиска \_ru
* \see cleanUpTags() getNodeTags() insertRowValues()
*/
void
aMSOTemplate::searchTags(QDomNode node, const QString &sname )
{
QDomNode n = node.lastChild();
while( !n.isNull() )
{
bool res = getNodeTags(n, sname, false);
if( res )
{
insertRowValues(n);
}
else
{
res = getNodeTags(n, sname, true);
if(res)
{
insertTagsValues(n, sname);
}
else
{
searchTags(n, sname);
}
}
n = n.previousSibling();
}
}
/**
* \en
* insert new row in table and replace tag to value
* \_en
* \ru
* Вставляет новую строку в таблицу, заменяет теги на значения, удаляет тег секции из строки таблицы.
* Выполняет рекурсивный поиск узла, содержащего строку таблицы. У этого узла есть
* специальное имя(w:r), которое распознается функцией. После того, как узел найден, строка строка дублируется,
* а из текущей строки удаляются все теги секции, чтобы избежать мнократного размножения строк таблицы.
* \_ru
* \param node - \en context for inserting \_en \ru узел, в который происходит вставка \_ru
* \see searchTags()
*/
void
aMSOTemplate::insertRowValues(QDomNode node)
{
QDomNode n = node;
while(!n.parentNode().isNull())
{
n = n.parentNode();
QDomElement e = n.toElement();
if( n.nodeName()=="Row" )
{
QDomAttr a = n.toElement().attributeNode( "ss:Index" );
n.parentNode().insertAfter(n.cloneNode(true),n);
clearTags(n,true);
QMap<QString,QString>::Iterator it;
for ( it = values.begin(); it != values.end(); ++it )
{
searchTags(n,it.key());
}
int rowIndex = a.value().toInt();
if (rowIndex == 0)
{
rowIndex = getRowIndex(n);
n.toElement().setAttribute("ss:Index",rowIndex);
}
n.nextSibling().toElement().setAttribute("ss:Index",rowIndex+1);
}
}
}
/**
* \en
* Execute replace tags to values.
* \_en
* \ru
* Выполняет подстановку значения параметра в шаблоне.
* Есть 2 типа тегов
* \arg обычные теги
* \arg секции - могут находиться ТОЛЬКО в строках таблицы.
* Для подстановки значений обычных тегов необходимо выполнить setValue() где name = PARAM (сейчас #define PARAM "param") а value - значение для подстановки. Потом выполнить exec() с параметром = имени тега.
* Для подстановки секций необходимо з
адать нужные параметры, используя setValue()
* а потом выполнить exec() с именем секции.
* exec может вызываться нужное число раз как для обычных тегов, так и для секций
* \_ru
* \param sname - \en name of parametr.\_en \ru
* имя параметра\_ru
*/
QString
aMSOTemplate::exec( const QString &sname )
{
setValue(sname, getValue(PARAM));
//search tags in content
QDomNode n = docTpl.lastChild();
while( !n.isNull() )
{
searchTags(n, sname);
n = n.previousSibling();
}
return docTpl.toString();
}
void SpecificWorker::compute( )
{
static float rot = 0.1f; // rads/sec
static float adv = 100.f; // mm/sec
static float turnSwitch = 1;
const float advIncLow = 0.8; // mm/sec
const float advIncHigh = 2.f; // mm/sec
const float rotInc = 0.25; // rads/sec
const float rotMax = 0.4; // rads/sec
const float advMax = 200; // milimetres/sec
const float distThreshold = 500; // milimetres
static int frame = 0;
static double last_t = tic();
RoboCompCamera::imgType img;
if( INPUTIFACE == Camera)
{
//For cameras
try
{
camera_proxy->getYImage(0,img, cState, bState);
memcpy(image_gray.data, &img[0], m_width*m_height*sizeof(uchar));
searchTags(image_gray);
}
catch(const Ice::Exception &e)
{
std::cout << "Error reading from Camera" << e << std::endl;
}
}
else if( INPUTIFACE == RGBD)
{
try
{
//For RGBD
RoboCompRGBD::ColorSeq colorseq;
RoboCompRGBD::DepthSeq depthseq;
rgbd_proxy->getRGB(colorseq, hState, bState);
memcpy(image_color.data , &colorseq[0], m_width*m_height*3);
cv::cvtColor(image_color, image_gray, CV_RGB2GRAY);
searchTags(image_gray);
}
catch(const Ice::Exception &e)
{
std::cout << "Error reading form RGBD " << e << std::endl;
}
}
else if( INPUTIFACE == RGBDBus)
{
qFatal("Support for RGBDBus is not implemented yet!");
}
else
qFatal("Input device not defined. Please specify one in the config file");
/*
vector< ::AprilTags::TagDetection> detections = m_tagDetector->extractTags(image_gray);
// print out each detection
cout << detections.size() << " tags detected:" << endl;
print_detection(detections);
//
if (m_draw)
{
for (uint i=0; i<detections.size(); i++)
{
detections[i].draw(image_gray);
}
//imshow("AprilTags", image_gray); // OpenCV call
}*/
// print out the frame rate at which image frames are being processed
frame++;
if (frame % 10 == 0)
{
double t = tic();
cout << " " << 10./(t-last_t) << " fps" << endl;
last_t = t;
}
try
{
RoboCompLaser::TLaserData ldata = laser_proxy->getLaserData();
std::sort( ldata.begin(), ldata.end(), [](RoboCompLaser::TData a, RoboCompLaser::TData b){ return a.dist < b.dist; }) ;
if( ldata.front().dist < distThreshold)
{
adv = adv * advIncLow;
rot = rot + turnSwitch * rotInc;
if( rot < -rotMax) rot = -rotMax;
if( rot > rotMax) rot = rotMax;
differentialrobot_proxy->setSpeedBase(adv, rot);
}
else
{
adv = adv * advIncHigh;
if( adv > advMax) adv = advMax;
rot = 0.f;
differentialrobot_proxy->setSpeedBase(adv, 0.f);
turnSwitch = -turnSwitch;
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
