本文整理汇总了C++中NodeTree::sizeNodes方法的典型用法代码示例。如果您正苦于以下问题:C++ NodeTree::sizeNodes方法的具体用法?C++ NodeTree::sizeNodes怎么用?C++ NodeTree::sizeNodes使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类NodeTree
的用法示例。
在下文中一共展示了NodeTree::sizeNodes方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: rrtgrow
NodeTree* rrtgrow(const std::vector<float> &start,const std::vector<float> &goal,float goalbias,std::vector<float> upper,std::vector<float> lower,OpenRAVE::EnvironmentBasePtr env, OpenRAVE::RobotBasePtr robot)
{
RRTNode* startNode = new RRTNode(start);
RRTNode* goalNode = new RRTNode(goal);
setgoalConfig(goal);
setGoalBias(goalbias);
NodeTree* initPath = new NodeTree(startNode);
NodeTree* path= new NodeTree();
NodeTree* finalPath=new NodeTree();
RRTNode* nearestNode = NULL;
// initPath->setgoalflag(false);
goalflag = false;
std::vector<float>::const_iterator it;
std::cout<<std::endl<<"Given:"<<std::endl<<"Goal:"<<goal[0]<<","<<goal[1]<<","<<goal[2]<<","<<goal[3]<<","<<goal[4]<<","<<goal[5]<<","<<goal[6]<<std::endl;
std::cout<<std::endl<<"Start:"<<start[0]<<","<<start[1]<<","<<start[2]<<","<<start[3]<<","<<start[4]<<","<<start[5]<<","<<start[6]<<std::endl;
for (int i=0; i<10000;++i){
std::cout<<std::endl<<"RRT-iteration"<<i<<std::endl;
RRTNode* sampledNode = initPath->getRamdomSample(upper,lower,goalNode);
if(!checkifCollision(sampledNode->getConfig(),env,robot)){
nearestNode = initPath->nearestNeighbour(sampledNode);
std::vector<float> nn(nearestNode->getConfig().begin(),nearestNode->getConfig().end());
std::cout<<"Nearest found:["<<nn[0]<<","<<nn[1]<<","<<nn[2]<<","<<nn[3]<<","<<nn[4]<<","<<nn[5]<<","<<nn[6]<<"]"<<std::endl;
// std::cout<<"Nearest Node found..."<<std::endl;
path = initPath->connectNodes(sampledNode, nearestNode, goalNode,initPath->stepSize(),env,robot);
}
else
continue;
if(path->sizeNodes()==1){
std::cout<<"restart subpath.."<<std::endl;
continue;
}
else if(getNearestDistance(path->lastNode()->getConfig(),goal)==0){
// std::cout<<"found found..."<<std::endl;
initPath->addNodeTree(path);
std::cout<<"...Final..."<<std::endl;
for(it=path->lastNode()->getConfig().begin(); it!=path->lastNode()->getConfig().end(); ++it){
std::cout<<"final Node:"<<(*it)<<std::endl;
}
break;
}
else{
initPath->addNodeTree(path);
std::cout<<"Intermediate Tree..."<<std::endl;
// finalPath = initPath->getPath(initPath->sizeNodes()-1);
}
}
std::cout<<" Path found..."<<std::endl;
finalPath = initPath->getPath(initPath->sizeNodes()-1);
std::cout<<" initPath Size..."<<initPath->sizeNodes()<<std::endl;
std::cout<<" FinalPath Size..."<<finalPath->sizeNodes()<<std::endl;
return finalPath;
}
示例2: connectNodes
//.........这里部分代码省略.........
// 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;
}