void BranchBase::update(vertexName name) { if(!visited_nodes[name].first) { unexplored_nodes--; } updateVisited(name); map.updateEdgeCount(getLatestNode()->name_,name); distance += updateDistance(name); updateOdom(name); // double odomUnc = updateOdom(name); // odom_unc_cost += odomUnc; std::shared_ptr<Node> node_ptr = std::make_shared<Node>(name,0,distance,visited_nodes[name].first); node_ptr->stamp = *(timestamps_.rbegin()); node_ptr->position_ = node_list.size(); node_ptr->previous_offset_ = (double)getPreviousDistance(name); node_list.push_back(node_ptr); visited_nodes[name].second = node_ptr; if(odom_unc_cost > max_odom_unc) { please_kill_me = true; } if(max_odom_cost < odom_unc_cost) { max_odom_cost = odom_unc_cost; } }
double BranchBase::updateDistance(vertexName name) { return (map.at(getLatestNode()->name_,name).weight); }
const double BranchBase::getHeuristics() const { if(!node_list.size()) return(0); return /*map.unvisited_distance;*/-1e+07*(double)(getLatestNode()->previous_offset_); }
void MergeNode::run(ExecutionNode* previous) { if(nullptr == previous) { m_errors.insert(ExecutionNode::NO_PREVIOUS_ERROR, QObject::tr("No previous node before Merge operator")); return; } m_previousNode= previous; m_result->setPrevious(previous->getResult()); ExecutionNode* previousLast= nullptr; std::vector<Result*> pastResult; for(auto start : *m_startList) { ExecutionNode* last= getLatestNode(start); if(nullptr != last) { if(nullptr != previousLast) { auto startResult= start->getResult(); startResult->setPrevious(previousLast->getResult()); previousLast->setNextNode(start); } previousLast= last; Result* tmpResult= last->getResult(); while(nullptr != tmpResult) { DiceResult* dice= dynamic_cast<DiceResult*>(tmpResult); if(nullptr != dice) { ///@todo TODO improve here to set homogeneous while is really m_diceResult->setHomogeneous(false); for(auto& die : dice->getResultList()) { if(!m_diceResult->getResultList().contains(die) && (!die->hasBeenDisplayed())) { Die* tmpdie= new Die(); *tmpdie= *die; die->displayed(); m_diceResult->getResultList().append(tmpdie); } } } auto it= std::find_if(pastResult.begin(), pastResult.end(), [tmpResult](const Result* a) { return (a == tmpResult->getPrevious()); }); if(it == pastResult.end()) { pastResult.push_back(previousLast->getResult()); tmpResult= tmpResult->getPrevious(); } else { tmpResult->setPrevious(nullptr); tmpResult= nullptr; } } } } auto first= m_startList->front(); m_startList->clear(); m_startList->push_back(first); if(nullptr != m_nextNode) { m_nextNode->run(this); } }