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