bool FootstepNavigation::replan() { if (!updateStart()) { ROS_ERROR("Start pose not accessible!"); return false; } bool path_existed = ivPlanner.pathExists(); // calculate path by replanning (if no planning information exists // this call is equal to ivPlanner.plan()) if (ivPlanner.replan()) { startExecution(); return true; } else if (path_existed) { ROS_INFO("Replanning unsuccessful. Reseting previous planning " "information."); if (ivPlanner.plan()) { startExecution(); return true; } } // path planning unsuccessful ivExecutingFootsteps = false; return false; }
bool FootstepNavigation::plan() { if (!updateStart()) { ROS_ERROR("Start pose not accessible!"); return false; } if (ivPlanner.plan()) { startExecution(); return true; } // path planning unsuccessful return false; }
void Cluster::updateOffer(const MemberId& updater, uint64_t updateeInt, Lock& l) { // NOTE: deliverEventQueue has been stopped at the update offer by // deliveredEvent in case an update is required. if (state == LEFT) return; MemberId updatee(updateeInt); boost::optional<Url> url = map.updateOffer(updater, updatee); if (updater == self) { assert(state == OFFER); if (url) // My offer was first. updateStart(updatee, *url, l); else { // Another offer was first. QPID_LOG(info, *this << " cancelled offer to " << updatee << " unstall"); setReady(l); makeOffer(map.firstJoiner(), l); // Maybe make another offer. deliverEventQueue.start(); // Go back to normal processing } } else if (updatee == self && url) { assert(state == JOINER); state = UPDATEE; acl = broker.getAcl(); broker.setAcl(0); // Disable ACL during update QPID_LOG(notice, *this << " receiving update from " << updater); checkUpdateIn(l); } else { QPID_LOG(info, *this << " unstall, ignore update " << updater << " to " << updatee); deliverEventQueue.start(); // Not involved in update. } if (updatee != self && url) { QPID_LOG(debug, debugSnapshot()); if (mAgent) mAgent->clusterUpdate(); // Updatee will call clusterUpdate() via checkUpdateIn() when update completes } }