static int
distanceToNode (NodeGrid nodes, int x, int y, int x2, int y2)
{
int val;
int best = 9999;
if (x == x2 and y == y2)
return 0;
nodes[x][y].mVisited++; //visit this node
if (y > 0 and !nodes[x][y-1].mVisited) {
val = distanceToNode (nodes, x, y-1, x2, y2);
if (val < best) best = val;
}
if (y < NODEROWS-1 and !nodes[x][y+1].mVisited) {
val = distanceToNode (nodes, x, y+1, x2, y2);
if (val < best) best = val;
}
if (x < NODECOLS-1 and !nodes[x+1][y].mVisited) {
val = distanceToNode (nodes, x+1, y, x2, y2);
if (val < best) best = val;
}
if (x > 0 and !nodes[x-1][y].mVisited) {
val = distanceToNode (nodes, x-1, y, x2, y2);
if (val < best) best = val;
}
nodes[x][y].mVisited--; //
return best + 1;
}
void SpecificWorker::manageReachedPose()
{
float schmittTriggerThreshold = 20;
float THRESHOLD_POSE = 100;
std::string m =" ";
bool changed = false;
QMutexLocker locker(mutex);
AGMModel::SPtr newModel(new AGMModel(worldModel));
for (AGMModel::iterator symbol_itr=newModel->begin(); symbol_itr!=newModel->end(); symbol_itr++)
{
AGMModelSymbol::SPtr node = *symbol_itr;
if (node->symboltype() == "pose")
{
/// Compute distance and new state
float d2n;
try
{
QVec arm = innerModel->transformS("world", "robot");
QVec obj = innerModel->transformS("world", node->getAttribute("imName"));
(arm-obj);//.print("error");
d2n = distanceToNode("robot", newModel, node);
//qDebug()<<"distance "<<d2n;
}
catch(...)
{
printf("Ref: robot: %p\n", (void *)innerModel->getNode("robot"));
printf("Pose: %s: %p\n", node->getAttribute("imName").c_str(), (void *)innerModel->getNode(node->getAttribute("imName").c_str()));
exit(1);
}
for (AGMModelSymbol::iterator edge_itr=node->edgesBegin(newModel); edge_itr!=node->edgesEnd(newModel); edge_itr++)
{
AGMModelEdge &edge = *edge_itr;
//*********** FIX
if (edge->getLabel() == "reach")
{
//TODO: FIX ==> Update wasIn mug status
AGMModelSymbol::SPtr mug = newModel->getSymbolByIdentifier(50); //50 ==> bluemug
int32_t inTableID = -1;
int32_t wasInTableID = -1;
for (auto edgeMug = mug->edgesBegin(newModel); edgeMug != mug->edgesEnd(newModel); edgeMug++)
{
if(newModel->getSymbolByIdentifier(edgeMug->getSymbolPair().second)->symboltype() == "object")
{
printf("symbol: %d\n", edgeMug->getSymbolPair().second);
bool doChange = false;
AGMModelSymbol::SPtr container = newModel->getSymbolByIdentifier(edgeMug->getSymbolPair().second);
for (auto edgeMug2 = container->edgesBegin(newModel); edgeMug2 != container->edgesEnd(newModel); edgeMug2++)
{
if (edgeMug2->getLabel() == "table")
doChange = true;
}
if (doChange)
{
printf("doChange\n");
if (edgeMug->getLabel() == "wasIn")
{
wasInTableID = edgeMug->getSymbolPair().second;
}
else if (edgeMug->getLabel() == "in")
{
inTableID = edgeMug->getSymbolPair().second;
}
}
}
}
qDebug()<<"inTable"<<inTableID<<"was"<<wasInTableID;
if(inTableID != -1 and wasInTableID != -1 and inTableID != wasInTableID)
{
changed = true;
newModel->removeEdgeByIdentifiers(mug->identifier, wasInTableID, "wasIn");
newModel->addEdgeByIdentifiers( mug->identifier, inTableID, "wasIn");
}
}
//***************
if (edge->getLabel() == "reach" and d2n > THRESHOLD_POSE+schmittTriggerThreshold )
{
edge->setLabel("noReach");
printf("pose %d STOPS REACH\n", node->identifier);
m += " action " + action + " edge->toString() "+ edge->toString(newModel);
changed = true;
rDebug2(("pose %d no-reach") % node->identifier);
}
else if (edge->getLabel() == "noReach" and d2n < THRESHOLD_POSE/*-schmittTriggerThreshold*/)
{
edge->setLabel("reach");
printf("___ %s ___\n", edge->getLabel().c_str());
printf("pose %d STARTS REACH\n", node->identifier);
m += " action " + action + " edge->toString() "+ edge->toString(newModel);
changed = true;
rDebug2(("pose %d reach") % node->identifier);
}
}
}
}
/// Publish new model if changed
if (changed)
{
printf("PUBLISH!!!! version%d\n", newModel->version);
sendModificationProposal(newModel, worldModel, m);
}
}
