void copyTree(VisualNode* node_t, NodeTree& tree_t,
const VisualNode* node_s, const NodeTree& tree_s) {
auto& na_t = tree_t.getNA();
const auto& na_s = tree_s.getNA();
stack<VisualNode*> stk_t;
stack<const VisualNode*> stk_s;
stk_s.push(node_s);
stk_t.push(node_t);
while (stk_s.size() > 0) {
const VisualNode* n = stk_s.top(); stk_s.pop();
VisualNode* next = stk_t.top(); stk_t.pop();
auto kids = n->getNumberOfChildren();
next->setNumberOfChildren(kids, na_t);
next->setStatus(n->getStatus());
next->dirtyUp(na_t);
for (auto i = 0u; i < kids; ++i) {
stk_s.push(n->getChild(na_s, i));
stk_t.push(next->getChild(na_t, i));
}
}
}
void MainWindow::Search(wxTreeItemId search,bool toogle)
{
wxTreeItemIdValue cookie;
NodeTree *itemData = search.IsOk() ? (NodeTree *) tree->GetItemData(search)
:NULL;
if(itemData->getTipo()==N_World )
if(itemData->pointer.world->getNumObjects()!=0)
Search(tree->GetFirstChild(search,cookie),toogle);
if(itemData->menus.menu_positionable)
{
if(itemData->menus.menu_composed || itemData->pointer.positionableentity->getOwner()->getClassName()=="World")
{
if(toogle)
itemData->pointer.positionableentity->setDrawReferenceSystem();
else
itemData->pointer.positionableentity->setDrawReferenceSystem(false);
if(itemData->menus.menu_composed)
if(itemData->pointer.composedentity->getNumObjects()>0)
Search(tree->GetFirstChild(search,cookie),toogle);
}
}
if(tree->GetLastChild(tree->GetItemParent(search))==search)
return;
Search(tree->GetNextSibling(search),toogle);
}
int calculateMaxDepth(const NodeTree& nt) {
const auto& na = nt.getNA();
const auto* root = nt.getRoot();
return calcDepth(na, root);
}
void MainWindow::ShowSelection(wxCommandEvent& event)
{
int id=event.GetId();
bool d_box=toolbar->GetToolState(id);
NodeTree *itemData = tree->GetSelection().IsOk() ? (NodeTree *) tree->GetItemData(tree->GetSelection())
:NULL;
if(d_box)
{
tree->SetShowSelection(true);
if(!tree->GetSelection().IsOk()) return;
if(itemData->menus.menu_solid && tree->GetSelection()!=tree->GetRootItem())
{
itemData->pointer.solidentity->setDrawBox(true);
itemData->getSimu()->getChild()->Refresh();
}
}
else
{
tree->SetShowSelection(false);
if(!tree->GetSelection().IsOk()) return;
if(itemData->menus.menu_solid && tree->GetSelection()!=tree->GetRootItem())
{
itemData->pointer.solidentity->setDrawBox(false);
itemData->getSimu()->getChild()->Refresh();
}
}
}
void highlightSubtrees(NodeTree& nt, const std::vector<VisualNode*>& nodes) {
QMutexLocker lock(&nt.getMutex());
auto& na = nt.getNA();
auto* root = nt.getRoot();
root->unhideAll(na);
{
QMutexLocker lock(&nt.getLayoutMutex());
root->layout(na);
}
// unhighlight all
applyToEachNode(nt, [](VisualNode* n) {
n->setHighlighted(false);
});
for (auto node : nodes) {
node->setHighlighted(true);
}
// TODO: hide not highlighted
// HideNotHighlightedCursor hnhc(root, na);
// PostorderNodeVisitor<HideNotHighlightedCursor>(hnhc).run();
nt.treeModified();
}
void applyToEachNodePO(NodeTree& nt, const NodeAction& action) {
/// PO-traversal requires two stacks
std::stack<VisualNode*> stk_1;
std::stack<VisualNode*> stk_2;
auto* root = nt.getRoot();
auto& na = nt.getNA();
stk_1.push(root);
while (stk_1.size() > 0) {
auto* node = stk_1.top(); stk_1.pop();
stk_2.push(node);
for (auto i = 0u; i < node->getNumberOfChildren(); ++i) {
auto kid = node->getChild(na, i);
stk_1.push(kid);
}
}
while (stk_2.size() > 0) {
auto* node = stk_2.top(); stk_2.pop();
action(node);
}
}
std::vector<NodeTree<int>*>* GraphStructuredStack::getReachable(NodeTree<int>* start, int length) {
std::vector<NodeTree<int>*>* reachableList = new std::vector<NodeTree<int>*>();
std::queue<NodeTree<int>*> currentNodes;
std::queue<NodeTree<int>*> nextNodes;
currentNodes.push(start);
for (int i = 0; i < length; i++) {
while (!currentNodes.empty()) {
NodeTree<int>* currentNode = currentNodes.front();
currentNodes.pop();
std::vector<NodeTree<int>*> children = currentNode->getChildren();
//std::cout << currentNode->getData() << " has children ";
for (std::vector<NodeTree<int>*>::size_type j = 0; j < children.size(); j++) {
std::cout << children[j]->getData() << " ";
nextNodes.push(children[j]);
}
std::cout << std::endl;
}
currentNodes = nextNodes;
//No clear function, so go through and remove
while(!nextNodes.empty())
nextNodes.pop();
}
while (!currentNodes.empty()) {
reachableList->push_back(currentNodes.front());
//std::cout << currentNodes.front()->getData() << " is reachable from " << start->getData() << " by length " << length << std::endl;
currentNodes.pop();
}
return reachableList;
}
void HuffmanCompression::getCarEncode(){
codeTable = new QMap<char,QString>;
QListIterator<NodeTree*> it(*externalNodesList);
while(it.hasNext()){
NodeTree *n = it.next();
QString str = goThroughBranch(n);
codeTable->insert(n->getCharacter(),str);
}
}
//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;
}
void HuffmanCompression::unify(NodeTree *nodeL,NodeTree *nodeR){
unsigned int weight = nodeL->getweight()+nodeR->getweight();
NodeTree *tree = new NodeTree();
nodeL->setType(Constants::LEFT);
nodeR->setType(Constants::RIGHT);
nodeR->setParent(tree);
nodeL->setParent(tree);
tree->setLeft(nodeL);
tree->setRight(nodeR);
tree->setType(Constants::ROOT);
tree->setWeight(weight);
list->insert(tree);
if(list->size()==1)
return;
unify(list->getFirst(),list->getFirst());
}
void addChildren(VisualNode* node, NodeTree& nt, int kids) {
auto& na = nt.getNA();
node->setNumberOfChildren(kids, na);
node->dirtyUp(na);
auto& stats = nt.getStatistics();
stats.undetermined += kids;
int depth = tree_utils::calculateDepth(nt, *node) + 1;
int new_depth = depth + 1;
stats.maxDepth = std::max(stats.maxDepth, new_depth);
};
void applyToEachNode(NodeTree& nt, const NodeAction& action) {
auto& na = nt.getNA();
for (int i = 0; i < na.size(); ++i) {
action(na[i]);
}
}
void MainWindow::OnRobotSimPanelCtrl(wxCommandEvent& WXUNUSED(event))
{
wxTreeItemId itemId = tree->GetSelection();
NodeTree *itemData = itemId.IsOk() ? (NodeTree *) tree->GetItemData(itemId):NULL;
if(itemData->pointer.robotsim)
{
if(managewindow->CheckWindowsExist(itemData))
{
RobotSimPanel* robotSimCtrl;
robotSimCtrl = new RobotSimPanel(this,wxID_ANY,wxT("Move all Joints"),itemData);
robotSimCtrl->getTitle()->SetLabel(wxString(itemData->getNameTree()));
robotSimCtrl->setManageWindow(managewindow);
robotSimCtrl->Show(true);
wxLogStatus(wxT("Robot Sim Panel"));
}
}
}
void MainWindow::OnRobotSimGoTo(wxCommandEvent& WXUNUSED(event))
{
wxTreeItemId itemId = tree->GetSelection();
NodeTree *itemData = itemId.IsOk() ? (NodeTree *) tree->GetItemData(itemId):NULL;
if(itemData->pointer.robotsim)
{
if(managewindow->CheckWindowsExist(itemData))
{
RobotSimGoTo* robotSimGoTarget;
robotSimGoTarget = new RobotSimGoTo(this,wxID_ANY,wxT("GO TO TARGET"),itemData);
robotSimGoTarget->getTitle()->SetLabel(itemData->getNameTree());
robotSimGoTarget->setManageWindow(managewindow);
robotSimGoTarget->Show(true);
wxLogStatus(wxT("Robot Sim Go To Target"));
}
}
}
int calculateDepth(const NodeTree& nt, const VisualNode& node) {
int count = 0;
auto it = &node;
auto& na = nt.getNA();
while ( (it = it->getParent(na)) ) { count++; }
return count;
}
void MainWindow::OnNameItemTree(wxCommandEvent& WXUNUSED(event))
{
wxTreeItemId itemId = tree->GetSelection();
NodeTree *itemData = itemId .IsOk() ? (NodeTree *)tree->GetItemData(itemId ):NULL;
static wxString s_text;
s_text = wxGetTextFromUser(wxT("New name:"), wxT("Change Name Item Tree"),s_text, this);
char text[100];
strcpy(text,(const char*)s_text.mb_str(wxConvUTF8));
if(!s_text.empty() && itemData->menus.menu_world)
{
tree->SetItemText(itemId, s_text);
itemData->getSimu()->setName(s_text.ToStdString());
}
else if ( !s_text.empty() && itemData->pointer.positionableentity)
{
itemData->pointer.positionableentity->setName(text);
tree->SetItemText(itemId, s_text);
}
}
static void recursiveHierarchy(NodeTree& tree, CSFile *csf, int idx, int cloneoffset)
{
for (int i = 0; i < csf->nodes[idx].numChildren; i++){
tree.setNodeParent((NodeTree::nodeID)csf->nodes[idx].children[i] + cloneoffset,(NodeTree::nodeID)idx + cloneoffset);
}
for (int i = 0; i < csf->nodes[idx].numChildren; i++){
recursiveHierarchy(tree,csf,csf->nodes[idx].children[i],cloneoffset);
}
}
void MainWindow::OnSimpleJointMove( wxCommandEvent& WXUNUSED(event))
{
wxTreeItemId itemId = tree->GetSelection();
NodeTree *itemData = itemId.IsOk() ? (NodeTree *) tree->GetItemData(itemId):NULL;
wxTreeItemId parentItem=tree->GetItemParent(itemId);
NodeTree *parentData = parentItem.IsOk() ? (NodeTree *) tree->GetItemData(parentItem):NULL;
if(itemData->pointer.simplejoint)
{
if(managewindow->CheckWindowsExist(itemData))
{
RobotSimPanel* robotSimCtrl;
robotSimCtrl = new RobotSimPanel(this,wxID_ANY,wxT ("Move only one Joint"),itemData,parentData,true);
robotSimCtrl->getTitle()->SetLabel(wxString(itemData->getNameTree()));
robotSimCtrl->setManageWindow(managewindow);
robotSimCtrl->Show(true);
wxLogStatus(wxT("Robot Sim Panel/Joint"));
}
}
}
bool compareSubtrees(const NodeTree& nt, const VisualNode& root1,
const VisualNode& root2) {
// compare roots
bool equal = compareNodes(root1, root2);
if (!equal) return false;
// if nodes have children, compare them recursively:
for (auto i = 0u; i < root1.getNumberOfChildren(); ++i) {
auto new_root_1 = nt.getChild(root1, i);
auto new_root_2 = nt.getChild(root2, i);
bool equal = compareSubtrees(nt, *new_root_1, *new_root_2);
if (!equal) return false;
}
return true;
}
void applyToEachNode(NodeTree& nt, VisualNode* node, const NodeAction& action) {
auto& na = nt.getNA();
std::stack<VisualNode*> stk;
stk.push(node);
while(stk.size() > 0) {
auto* curr_node = stk.top(); stk.pop();
action(curr_node);
for (auto i = 0u; i < curr_node->getNumberOfChildren(); ++i) {
auto child = curr_node->getChild(na, i);
stk.push(child);
}
}
}
Statistics gatherNodeStats(NodeTree& nt) {
Statistics stats;
auto* root = nt.getRoot();
applyToEachNode(nt, root, [&stats](VisualNode* n) {
switch (n->getStatus()) {
case SOLVED:
stats.solutions += 1; break;
case FAILED:
stats.failures += 1; break;
case BRANCH:
stats.choices += 1; break;
case UNDETERMINED:
stats.undetermined += 1; break;
default:
break;
}
});
stats.maxDepth = calculateMaxDepth(nt);
return stats;
}
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;
}
/*!
Load the plugin options.
\param server The server object to use.
*/
int
PluginsManager::loadOptions (Server *server)
{
int ret = 0;
string key ("server.plugins");
NodeTree<string>* node = server->getNodeTree (key);
if (node == NULL)
return 0;
list<NodeTree<string>*> *children = node->getChildren ();
if (children == NULL)
return 0;
string namespaceKey ("namespace");
string pluginKey ("plugin");
string globalKey ("global");
string enabledKey ("enabled");
for (list<NodeTree<string>*>::iterator it = children->begin ();
it != children->end ();
it++)
{
NodeTree<string>* node = *it;
string plugin;
string namespaceName;
bool global = false;
bool enabled = false;
if (node->getAttr (namespaceKey))
namespaceName.assign (*node->getAttr (namespaceKey));
if (node->getAttr (pluginKey))
plugin.assign (*node->getAttr (pluginKey));
if (node->getAttr (globalKey))
global = stringcmpi (*node->getAttr (globalKey), "YES") == 0;
if (node->getAttr (enabledKey))
enabled = stringcmpi (*node->getAttr (enabledKey), "YES") == 0;
if (plugin.length ())
addPluginInfo (plugin, new PluginInfo (plugin, enabled, global));
else
{
server->log (MYSERVER_LOG_MSG_WARNING,
_("Invalid plugin name in PLUGIN block."));
ret = -1;
}
}
return ret;
}
//--------------------------------------------
//-------- FINDING IDENTICAL SUBTREES --------
//--------------------------------------------
// TODO/NOTE(maxim): This algorithm isn't supposed to work
// (new groups pushed to the end), but I've failed
// to find any cases where it produces a wrong result
GroupsOfNodes_t findIdenticalShapes(NodeTree& nt) {
/// ------ 0) group by height ------
std::vector<Group> groups = groupByHeight(nt);
/// ------ 1) assign a group id to each node -------
std::unordered_map<const VisualNode*, PosInGroups> node2groupID;
for (auto group_idx = 1u; group_idx < groups.size(); ++group_idx) {
auto& group = groups[group_idx];
for (auto i = 0u; i < group.items.size(); ++i) {
auto* node = group.items[i].node;
node2groupID[node].g_id = group_idx;
node2groupID[node].inner_idx = i;
}
}
// printGroups(groups);
// qDebug() << " ";
/// ------ 2) select the first block (with height 1)
for (auto group_id = 0u; group_id < groups.size(); ++group_id) {
for (auto alt = 0; alt < 2; ++alt) {
auto& block = groups[group_id];
std::vector<int> groups_to_split;
// qDebug() << "left children:";
for (auto& e : block.items) {
if (e.alt == alt) {
/// 3.1) get its parent
auto& na = nt.getNA();
auto parent = e.node->getParent(na);
// std::cerr << parent->debug_id << " ";
/// 3.2 )find it in groups
auto location = detail::findNodeInGroups(groups, node2groupID, parent);
// std::cerr << parent->debug_id << " in group: " << location.first << "\n";
/// group g_idx will potentially need splitting
auto g_idx = location.first;
groups_to_split.push_back(g_idx); /// NOTE(maxim): has duplicate elements (?)
detail::separateNode(groups[g_idx], node2groupID, location.second);
// qDebug() << "node: " << e.node->debug_id;
// qDebug() << "separate: " << parent->debug_id;
/// split only affected groups
}
}
// qDebug() << "groups before:";
// printGroups(groups);
detail::splitGroups(groups, groups_to_split, node2groupID);
// qDebug() << "groups after:";
// printGroups(groups);
// qDebug() << " ";
groups_to_split.clear();
// qDebug() << "groups: " << groups;
}
}
/// ----- sort the groups -----
#ifdef MAXIM_DEBUG
sort(begin(groups), end(groups), [](const Group& lhs, const Group& rhs) {
if (lhs.items.size() == 0 && rhs.items.size() == 0) {
return false;
}
if (lhs.items.size() == 0) {
return false;
}
if (rhs.items.size() == 0) {
return true;
}
return lhs.items[0].node->debug_id < rhs.items[0].node->debug_id;
});
#endif
/// convert groups to a vector of vectors of Nodes
std::vector<std::vector<VisualNode*>> result(groups.size());
for (auto i = 0u; i < groups.size(); ++i) {
auto& items = groups[i].items;
result[i].reserve(items.size());
// qDebug() << "copying " << items.size() << " elements";
for (auto j = 0u; j < items.size(); ++j) {
result[i].push_back(items[j].node);
}
}
return result;
}
void MainWindow::OnLinkTo(wxCommandEvent& event)
{
if(listWorlds.size()>0)
{
int id=event.GetId();
wxTreeItemId itemId = tree->GetSelection();
NodeTree *itemData = itemId.IsOk() ? (NodeTree *) tree->GetItemData(itemId):NULL;
if(itemData->pointer.positionableentity)
{
simuWorld=itemData->getSimu();
if (id==ID_LINKTO)
{
state=1;
wxSetCursor(wxCURSOR_POINT_LEFT);
simuWorld->SetEntityToLink(itemData->pointer.positionableentity);
wxLogStatus(wxT("Select Item(Link to)"));
id=NULL;
}
if (id==ID_UNLINK)
{
if (showLinks==true) tree->EraseMarks(itemData);
itemData->pointer.positionableentity->LinkTo(NULL); //Deslinkar
if(GetTreeStructureState()==true)
{
simuWorld->InitializeItemsVectors();
tree->UpdateTree(simuWorld);
tree->showTreeStructure(true);
}
wxLogStatus(wxT("Unlink done"));
state=0;
}
}
if (id==ID_SHOWLINKS)
{
showLinks=treeToolbar->GetToolState(id);
if(showLinks)
{
tree->setShowLinks(true);
treeToolbar->EnableTool(ID_TREESTRUCTURE,false);
}
else
{
tree->setShowLinks(false);
treeToolbar->EnableTool(ID_TREESTRUCTURE,true);
}
}
if (id==ID_TREESTRUCTURE)
{
treeStruc=treeToolbar->GetToolState(id);
if(treeStruc)
{
tree->showTreeStructure(true);
treeToolbar->EnableTool(ID_SHOWLINKS,false);
}
else
{
tree->showTreeStructure(false);
treeToolbar->EnableTool(ID_SHOWLINKS,true);
}
}
}
}
void debug(int argc, char* argv[])
{
#ifdef RODOLPHE
#endif
#ifdef SAMUEL
NodeTree nTree;
// ARRAYTREE
#if 0
nTree.insertNLeaves(15);
#if 1
ArrayTree aTree(nTree);
aTree.dumpToStdout();
std::cout << aTree.to_str() << std::endl;
assert(aTree.check(aTree.getRoot()));
std::cout << "Check 1 Ok" << std::endl;
#endif
#if 0
int subTree = aTree.degraph(1);
std::cout << "aTree._root=" << aTree.getRoot() << std::endl;
aTree.dumpToStdout();
std::cout << aTree.to_str() << std::endl;
assert(aTree.check(0));
std::cout << "Check 2 Ok" << std::endl;
#endif
#if 0
std::cout << aTree.regraph(1, aTree.getRoot()) << std::endl;
std::cout << "aTree._root=" << aTree.getRoot() << std::endl;
aTree.dumpToStdout();
std::cout << aTree.to_str() << std::endl;
assert(aTree.check(0));
std::cout << "Check 3 Ok" << std::endl;
#endif
#if 1
std::vector<int> nodes = aTree.SPR_list_init(3);
aTree.SPR_list(3, nodes);
std::cout << aTree.to_str() << std::endl;
assert(aTree.check(aTree.getRoot()));
std::cout << "Check 4 Ok" << std::endl;
#endif
// NODETREE
#else
nTree.insertNLeaves(10);
NodeTree copy(nTree);
std::cout << nTree.to_str() << std::endl;
std::cout << nTree.check() << std::endl;
std::cout << copy.to_str() << std::endl;
std::cout << copy.check() << std::endl;
NodeTree* A = &nTree;
NodeTree* B = ©
Node* nodeA = (*A).nodeAt(1);
Node* nodeB = (*B).nodeAt(1);
std::cout << nodeA->to_str() << std::endl;
std::cout << nodeB->to_str() << std::endl;
#if 0
std::vector<Node*> nodesA = (*A).SPR_list_init(nodeA);
(*A).SPR_list(nodeA, nodesA);
std::vector<Node*> nodesB = (*B).SPR_list_init(nodeB);
(*B).SPR_list(nodeB, nodesB);
#else
(*A).SPR_ite(nodeA);
(*B).SPR_rec(nodeB);
#endif
std::cout << (*A).to_str() << std::endl;
std::cout << (*A).getRoot()->to_str() << std::endl;
std::cout << (*A).check() << std::endl;
std::cout << (*B).to_str() << std::endl;
std::cout << (*B).getRoot()->to_str() << std::endl;
std::cout << (*B).check() << std::endl;
#endif
#endif
}
/*!
Get the value for name in the hash dictionary.
If the value is not found then defValue is returned.
*/
const char *Server::getData (const char *name, const char *defValue)
{
NodeTree<string> *s = hashedData.get (name);
return s ? s->getValue ()->c_str () : defValue;
}
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;
}
