//get path from root to node at index NodeTree* getPath(unsigned int index){ RRTNode* node = _nodes[index]; NodeTree* path = new NodeTree(node); while(node->getParent() != NULL) { path->addNode(node); // insert node = node->getParent(); } path->addNode(node); // std::reverse(path->_nodes.begin(),path->_nodes.end()); return path; }
NodeTree* connectNodes(RRTNode* sampledNode, RRTNode* nearestNode,RRTNode* goalNode, float stepsize,OpenRAVE::EnvironmentBasePtr env, OpenRAVE::RobotBasePtr robot) { std::vector<float> goal = goalNode->getConfig(); std::cout<<"Connecting..."<<std::endl; std::vector<float> targetConfig(sampledNode->getConfig().begin(),sampledNode->getConfig().end()); std::vector<float> unitConfig; NodeTree* intermediateTree; // NodeTree* q = new NodeTree(); std::cout<<"Entered connect"<<std::endl<<std::endl; bool flag = true; // for first node additions std::vector<float> nearestNodeConfig(nearestNode->getConfig().begin(),nearestNode->getConfig().end()); RRTNode* currentNode; RRTNode* prevNode = nearestNode; for (int itr=0; itr<10000; ++itr){ // avoid infinite loops, change as required std::cout<<std::endl<<"Enter connectloop... iteration"<<itr<<std::endl; std::vector<float>::const_iterator it; std::vector<float> prevConfig(nearestNodeConfig.begin(),nearestNodeConfig.end()); float ndist = getNearestDistance(targetConfig,nearestNodeConfig); std::cout<<"Sampledistance:"<<ndist<<std::endl; if (ndist > stepsize){ std::cout<<"Step:"<<itr<<std::endl; for(int i = 0; i < 7; ++i){ unitConfig.push_back((targetConfig[i] - nearestNodeConfig[i])/ndist); nearestNodeConfig[i] += stepsize * unitConfig[i]; // find unit vector } // for(it=nearestNodeConfig.begin(); it!=nearestNodeConfig.end(); ++it){ // std::cout<<"New nearestNode:"<<(*it)<<std::endl; // } if(getNearestDistance(nearestNodeConfig,prevConfig)){ // check if moved to new location, remove if fails if(!checkifCollision(nearestNodeConfig,env,robot)){ currentNode = new RRTNode(nearestNodeConfig, prevNode); prevNode = currentNode; //check this // std::cout<<"prev-> curr:"<<std::endl; //print contents of added node std::cout<<"...New node created:"<<std::endl; for(it=currentNode->getConfig().begin(); it!=currentNode->getConfig().end(); ++it){ std::cout<<(*it)<<std::endl; } // for(it=currentNode->getParent()->getConfig().begin(); it!=currentNode->getParent()->getConfig().end(); ++it){ // std::cout<<"currentNodeparent:"<<(*it)<<std::endl; // } // for(it=prevNode->getConfig().begin(); it!=prevNode->getConfig().end(); ++it){ // std::cout<<"prevNode:"<<(*it)<<std::endl; // } if (flag) { intermediateTree = new NodeTree(currentNode); flag = false; } else { intermediateTree->addNode(currentNode); } std::cout<<"...New node Added:"<<std::endl; } else { // std::cout<<"in dist >step"<<std::endl; if (flag) { intermediateTree = new NodeTree(); flag = false; } break; //check if NULL } } // if new sampled node is the same as previous else { // std::cout<<"Not moved"<<std::endl; if (flag) { intermediateTree = new NodeTree(); flag = false; } break; } } // if the distance within the goal threashold range else if(ndist <= 1.0) //q->errorfactor() { std::cout<<std::endl<<"..Step :"<<itr<<"->inside epsilon "<<std::endl; //if the distance is very close if(!checkifCollision(nearestNodeConfig,env,robot)) { std::cout<<"...Entered Goal Zone..."<<std::endl; if (goalflag) // if sampled node was goal { currentNode = new RRTNode(goal, prevNode); if (flag) { intermediateTree = new NodeTree(currentNode); flag = false; } else { intermediateTree->addNode(currentNode); } std::cout<<"............Goal reached..."<<std::endl; break; } // else sampled node was reached and added else { currentNode = new RRTNode(nearestNodeConfig, prevNode); if (flag) { intermediateTree = new NodeTree(currentNode); flag = false; } else { intermediateTree->addNode(currentNode); } std::cout<<"............Sample reached..."<<std::endl; break; } } else { // std::cout<<"in dist<step"<<std::endl; if (flag) { intermediateTree = new NodeTree(); flag = false; } break; } } else { std::cout<<"(o_o)(o_o)(o_o)(o_o)(o_o).."<<std::endl; } } std::cout<<"Tree returned"<<intermediateTree->sizeNodes()<<std::endl; return intermediateTree; }