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