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