void
FootstepNavigation::mapCallback(
const nav_msgs::OccupancyGridConstPtr& occupancy_map)
{
// stop execution if an execution was performed
if (ivExecutingFootsteps)
{
if (ivSafeExecution)
{
// interrupt the thread and wait until it has finished its execution
ivFootstepExecutionPtr->interrupt();
ivFootstepExecutionPtr->join();
}
else
{
ivFootstepsExecution.cancelAllGoals();
}
}
gridmap_2d::GridMap2DPtr map(new gridmap_2d::GridMap2D(occupancy_map));
ivIdMapFrame = map->getFrameID();
// updates the map and starts replanning if necessary
if (ivPlanner.updateMap(map))
{
replan();
}
}
//-----------------------------Interface function-----------------------------------------------------
//returns 1 if found a solution, and 0 otherwise
int ADPlanner::replan(double allocated_time_secs, vector<int>* solution_stateIDs_V)
{
int solcost;
return replan(allocated_time_secs, solution_stateIDs_V, &solcost);
}
void SimplePossessor::setModerateGoal(goal_moderate gm) {
this->gm = gm;
if (getCell()&&getCell()->getGame()&&getCell()->getGame()->settings.print_AI_goal_messages>=2)
std::cout << "goal M set [" << (void*) this << "]: " << gm.getDescription()
<< "\n";
onGoalModerateSet(gm);
replan();
}
void SimplePossessor::update(float dt) {
//Subtracts distance moved by Actor from patrol distance.
//If not moving, subtracts a small amount so that the next patrol dir can be assumed.
if (gi.type == GI::GI_PATROL || gi.type == GI::GI_WANDER)
if (default_order_mem.patrol_dist>0)
default_order_mem.patrol_dist -= std::max(getHost()->displacement.getMagnitude(),10*dt);
else
default_order_mem.patrol_pause_timer-=dt;
replan_cycle = 0;
replan();
}
void
FootstepNavigation::executeFootstepsFast()
{
if (ivPlanner.getPathSize() <= 1)
return;
// lock the planning and execution process
ivExecutingFootsteps = true;
// make sure the action client is connected to the action server
ivFootstepsExecution.waitForServer();
humanoid_nav_msgs::ExecFootstepsGoal goal;
State support_leg;
if (ivPlanner.getPathBegin()->getLeg() == RIGHT)
support_leg = ivPlanner.getStartFootRight();
else // leg == LEFT
support_leg = ivPlanner.getStartFootLeft();
if (getFootstepsFromPath(support_leg, 1, goal.footsteps))
{
goal.feedback_frequency = ivFeedbackFrequency;
ivControlStepIdx = 0;
ivResetStepIdx = 0;
// start the execution via action; _1, _2 are place holders for
// function arguments (see boost doc)
ivFootstepsExecution.sendGoal(
goal,
boost::bind(&FootstepNavigation::doneCallback, this, _1, _2),
boost::bind(&FootstepNavigation::activeCallback, this),
boost::bind(&FootstepNavigation::feedbackCallback, this, _1));
}
else
{
// free the lock
ivExecutingFootsteps = false;
replan();
}
}
void
FootstepNavigation::goalPoseCallback(
const geometry_msgs::PoseStampedConstPtr& goal_pose)
{
// check if the execution is locked
if (ivExecutingFootsteps)
{
ROS_INFO("Already performing a navigation task. Wait until it is "
"finished.");
return;
}
if (setGoal(goal_pose))
{
// this check enforces a planning from scratch if necessary (dependent on
// planning direction)
if (ivForwardSearch)
replan();
else
plan();
}
}
Status CachedPlanStage::pickBestPlan(PlanYieldPolicy* yieldPolicy) {
// Adds the amount of time taken by pickBestPlan() to executionTimeMillis. There's lots of
// execution work that happens here, so this is needed for the time accounting to
// make sense.
ScopedTimer timer(&_commonStats.executionTimeMillis);
// If we work this many times during the trial period, then we will replan the
// query from scratch.
size_t maxWorksBeforeReplan =
static_cast<size_t>(internalQueryCacheEvictionRatio * _decisionWorks);
// The trial period ends without replanning if the cached plan produces this many results.
size_t numResults = MultiPlanStage::getTrialPeriodNumToReturn(*_canonicalQuery);
for (size_t i = 0; i < maxWorksBeforeReplan; ++i) {
// Might need to yield between calls to work due to the timer elapsing.
Status yieldStatus = tryYield(yieldPolicy);
if (!yieldStatus.isOK()) {
return yieldStatus;
}
WorkingSetID id = WorkingSet::INVALID_ID;
PlanStage::StageState state = child()->work(&id);
if (PlanStage::ADVANCED == state) {
// Save result for later.
WorkingSetMember* member = _ws->get(id);
// Ensure that the BSONObj underlying the WorkingSetMember is owned in case we yield.
member->makeObjOwnedIfNeeded();
_results.push_back(id);
if (_results.size() >= numResults) {
// Once a plan returns enough results, stop working. Update cache with stats
// from this run and return.
updatePlanCache();
return Status::OK();
}
} else if (PlanStage::IS_EOF == state) {
// Cached plan hit EOF quickly enough. No need to replan. Update cache with stats
// from this run and return.
updatePlanCache();
return Status::OK();
} else if (PlanStage::NEED_YIELD == state) {
if (id == WorkingSet::INVALID_ID) {
if (!yieldPolicy->allowedToYield()) {
throw WriteConflictException();
}
} else {
WorkingSetMember* member = _ws->get(id);
invariant(member->hasFetcher());
// Transfer ownership of the fetcher and yield.
_fetcher.reset(member->releaseFetcher());
}
if (yieldPolicy->allowedToYield()) {
yieldPolicy->forceYield();
}
Status yieldStatus = tryYield(yieldPolicy);
if (!yieldStatus.isOK()) {
return yieldStatus;
}
} else if (PlanStage::FAILURE == state) {
// On failure, fall back to replanning the whole query. We neither evict the
// existing cache entry nor cache the result of replanning.
BSONObj statusObj;
WorkingSetCommon::getStatusMemberObject(*_ws, id, &statusObj);
LOG(1) << "Execution of cached plan failed, falling back to replan."
<< " query: " << _canonicalQuery->toStringShort()
<< " planSummary: " << Explain::getPlanSummary(child().get())
<< " status: " << statusObj;
const bool shouldCache = false;
return replan(yieldPolicy, shouldCache);
} else if (PlanStage::DEAD == state) {
BSONObj statusObj;
WorkingSetCommon::getStatusMemberObject(*_ws, id, &statusObj);
LOG(1) << "Execution of cached plan failed: PlanStage died"
<< ", query: " << _canonicalQuery->toStringShort()
<< " planSummary: " << Explain::getPlanSummary(child().get())
<< " status: " << statusObj;
return WorkingSetCommon::getMemberObjectStatus(statusObj);
} else {
invariant(PlanStage::NEED_TIME == state);
}
}
// If we're here, the trial period took more than 'maxWorksBeforeReplan' work cycles. This
// plan is taking too long, so we replan from scratch.
LOG(1) << "Execution of cached plan required " << maxWorksBeforeReplan
<< " works, but was originally cached with only " << _decisionWorks
<< " works. Evicting cache entry and replanning query: "
<< _canonicalQuery->toStringShort()
<< " plan summary before replan: " << Explain::getPlanSummary(child().get());
const bool shouldCache = true;
return replan(yieldPolicy, shouldCache);
}
Status CachedPlanStage::pickBestPlan(PlanYieldPolicy* yieldPolicy) {
// Adds the amount of time taken by pickBestPlan() to executionTimeMillis. There's lots of
// execution work that happens here, so this is needed for the time accounting to
// make sense.
ScopedTimer timer(getClock(), &_commonStats.executionTimeMillis);
// During plan selection, the list of indices we are using to plan must remain stable, so the
// query will die during yield recovery if any index has been dropped. However, once plan
// selection completes successfully, we no longer need all indices to stick around. The selected
// plan should safely die on yield recovery if it is using the dropped index.
//
// Dismiss the requirement that no indices can be dropped when this method returns.
ON_BLOCK_EXIT([this] { releaseAllIndicesRequirement(); });
// If we work this many times during the trial period, then we will replan the
// query from scratch.
size_t maxWorksBeforeReplan =
static_cast<size_t>(internalQueryCacheEvictionRatio * _decisionWorks);
// The trial period ends without replanning if the cached plan produces this many results.
size_t numResults = MultiPlanStage::getTrialPeriodNumToReturn(*_canonicalQuery);
for (size_t i = 0; i < maxWorksBeforeReplan; ++i) {
// Might need to yield between calls to work due to the timer elapsing.
Status yieldStatus = tryYield(yieldPolicy);
if (!yieldStatus.isOK()) {
return yieldStatus;
}
WorkingSetID id = WorkingSet::INVALID_ID;
PlanStage::StageState state = child()->work(&id);
if (PlanStage::ADVANCED == state) {
// Save result for later.
WorkingSetMember* member = _ws->get(id);
// Ensure that the BSONObj underlying the WorkingSetMember is owned in case we yield.
member->makeObjOwnedIfNeeded();
_results.push(id);
if (_results.size() >= numResults) {
// Once a plan returns enough results, stop working. Update cache with stats
// from this run and return.
updatePlanCache();
return Status::OK();
}
} else if (PlanStage::IS_EOF == state) {
// Cached plan hit EOF quickly enough. No need to replan. Update cache with stats
// from this run and return.
updatePlanCache();
return Status::OK();
} else if (PlanStage::NEED_YIELD == state) {
invariant(id == WorkingSet::INVALID_ID);
if (!yieldPolicy->canAutoYield()) {
throw WriteConflictException();
}
if (yieldPolicy->canAutoYield()) {
yieldPolicy->forceYield();
}
Status yieldStatus = tryYield(yieldPolicy);
if (!yieldStatus.isOK()) {
return yieldStatus;
}
} else if (PlanStage::FAILURE == state) {
// On failure, fall back to replanning the whole query. We neither evict the
// existing cache entry nor cache the result of replanning.
BSONObj statusObj;
WorkingSetCommon::getStatusMemberObject(*_ws, id, &statusObj);
LOG(1) << "Execution of cached plan failed, falling back to replan."
<< " query: " << redact(_canonicalQuery->toStringShort())
<< " planSummary: " << Explain::getPlanSummary(child().get())
<< " status: " << redact(statusObj);
const bool shouldCache = false;
return replan(yieldPolicy, shouldCache);
} else {
invariant(PlanStage::NEED_TIME == state);
}
}
// If we're here, the trial period took more than 'maxWorksBeforeReplan' work cycles. This
// plan is taking too long, so we replan from scratch.
LOG(1) << "Execution of cached plan required " << maxWorksBeforeReplan
<< " works, but was originally cached with only " << _decisionWorks
<< " works. Evicting cache entry and replanning query: "
<< redact(_canonicalQuery->toStringShort())
<< " plan summary before replan: " << Explain::getPlanSummary(child().get());
const bool shouldCache = true;
return replan(yieldPolicy, shouldCache);
}
void
FootstepNavigation::feedbackCallback(
const humanoid_nav_msgs::ExecFootstepsFeedbackConstPtr& fb)
{
int executed_steps_idx = fb->executed_footsteps.size() - ivExecutionShift;
// make sure at least one step has been performed
if (executed_steps_idx < 0)
return;
// if the currently executed footstep equals the currently observed one
// everything is ok
if (executed_steps_idx == ivControlStepIdx)
return;
// get planned foot placement
const State& planned = *(ivPlanner.getPathBegin() + ivControlStepIdx + 1 +
ivResetStepIdx);
// get executed foot placement
tf::Transform executed_tf;
std::string foot_id;
if (planned.getLeg() == RIGHT)
foot_id = ivIdFootRight;
else
foot_id = ivIdFootLeft;
if (!getFootTransform(foot_id, ivIdMapFrame, ros::Time::now(),
ros::Duration(0.5), &executed_tf))
{
State executed(executed_tf.getOrigin().x(), executed_tf.getOrigin().y(),
tf::getYaw(executed_tf.getRotation()), planned.getLeg());
ivFootstepsExecution.cancelGoal();
humanoid_nav_msgs::ExecFootstepsGoal goal;
// try to reach the calculated path
if (getFootstepsFromPath(executed, executed_steps_idx + ivResetStepIdx,
goal.footsteps))
{
goal.feedback_frequency = ivFeedbackFrequency;
// adjust the internal counters
ivResetStepIdx += ivControlStepIdx + 1;
ivControlStepIdx = 0;
// restart the footstep execution
ivFootstepsExecution.sendGoal(
goal,
boost::bind(&FootstepNavigation::doneCallback, this, _1, _2),
boost::bind(&FootstepNavigation::activeCallback, this),
boost::bind(&FootstepNavigation::feedbackCallback, this, _1));
}
// the previously calculated path cannot be reached so we have plan
// a new path
else
{
replan();
}
}
State executed(executed_tf.getOrigin().x(), executed_tf.getOrigin().y(),
tf::getYaw(executed_tf.getRotation()), planned.getLeg());
// check if the currently executed footstep is no longer observed (i.e.
// the robot no longer follows its calculated path)
if (executed_steps_idx >= ivControlStepIdx + 2)
{
ivFootstepsExecution.cancelGoal();
ROS_DEBUG("Footstep execution incorrect.");
humanoid_nav_msgs::ExecFootstepsGoal goal;
// try to reach the calculated path
if (getFootstepsFromPath(executed, executed_steps_idx + ivResetStepIdx,
goal.footsteps))
{
ROS_INFO("Try to reach calculated path.");
goal.feedback_frequency = ivFeedbackFrequency;
// adjust the internal counters
ivResetStepIdx += ivControlStepIdx + 1;
ivControlStepIdx = 0;
// restart the footstep execution
ivFootstepsExecution.sendGoal(
goal,
boost::bind(&FootstepNavigation::doneCallback, this, _1, _2),
boost::bind(&FootstepNavigation::activeCallback, this),
boost::bind(&FootstepNavigation::feedbackCallback, this, _1));
}
// the previously calculated path cannot be reached so we have plan
// a new path
else
{
replan();
}
return;
}
// check the currently observed footstep
else
{
ROS_DEBUG("planned (%f, %f, %f, %i) vs. executed (%f, %f, %f, %i)",
planned.getX(), planned.getY(), planned.getTheta(),
planned.getLeg(),
executed.getX(), executed.getY(), executed.getTheta(),
executed.getLeg());
// adjust the internal step counters if the footstep has been
// performed correctly; otherwise check in the next iteration if
// the step really has been incorrect
if (performanceValid(planned, executed))
ivControlStepIdx++;
else
ROS_DEBUG("Invalid step. Wait next step update before declaring"
" step incorrect.");
}
}
void
FootstepNavigation::executeFootsteps()
{
if (ivPlanner.getPathSize() <= 1)
return;
// lock this thread
ivExecutingFootsteps = true;
ROS_INFO("Start walking towards the goal.");
humanoid_nav_msgs::StepTarget step;
humanoid_nav_msgs::StepTargetService step_srv;
tf::Transform from;
std::string support_foot_id;
// calculate and perform relative footsteps until goal is reached
state_iter_t to_planned = ivPlanner.getPathBegin();
if (to_planned == ivPlanner.getPathEnd())
{
ROS_ERROR("No plan available. Return.");
return;
}
const State* from_planned = to_planned.base();
to_planned++;
while (to_planned != ivPlanner.getPathEnd())
{
try
{
boost::this_thread::interruption_point();
}
catch (const boost::thread_interrupted&)
{
// leave this thread
return;
}
if (from_planned->getLeg() == RIGHT)
support_foot_id = ivIdFootRight;
else // support_foot = LLEG
support_foot_id = ivIdFootLeft;
// try to get real placement of the support foot
if (getFootTransform(support_foot_id, ivIdMapFrame, ros::Time::now(),
ros::Duration(0.5), &from))
{
// calculate relative step and check if it can be performed
if (getFootstep(from, *from_planned, *to_planned, &step))
{
step_srv.request.step = step;
ivFootstepSrv.call(step_srv);
}
// ..if it cannot be performed initialize replanning
else
{
ROS_INFO("Footstep cannot be performed. Replanning necessary.");
replan();
// leave the thread
return;
}
}
else
{
// if the support foot could not be received wait and try again
ros::Duration(0.5).sleep();
continue;
}
from_planned = to_planned.base();
to_planned++;
}
ROS_INFO("Succeeded walking to the goal.\n");
// free the lock
ivExecutingFootsteps = false;
}
bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
ompl::geometric::PathGeometric &primaryPath)
{
// \todo: we should reuse our collision checking from the previous step to make this faster
OMPL_INFORM("LightningRetrieveRepair: Repairing path");
// Error check
if (primaryPath.getStateCount() < 2)
{
OMPL_ERROR("LightningRetrieveRepair: Cannot repair a path with less than 2 states");
return false;
}
// Loop through every pair of states and make sure path is valid.
// If not, replan between those states
for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
{
std::size_t fromID = toID - 1; // this is our last known valid state
ompl::base::State *fromState = primaryPath.getState(fromID);
ompl::base::State *toState = primaryPath.getState(toID);
// Check if our planner is out of time
if (ptc == true)
{
OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is true.");
return false;
}
// Check path between states
if (!si_->checkMotion(fromState, toState))
{
// Path between (from, to) states not valid, but perhaps to STATE is
// Search until next valid STATE is found in existing path
std::size_t subsearchID = toID;
ompl::base::State *new_to;
OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid out %d total states",
fromID,toID,primaryPath.getStateCount());
while (subsearchID < primaryPath.getStateCount())
{
new_to = primaryPath.getState(subsearchID);
if (si_->isValid(new_to))
{
OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states", subsearchID);
// This future state is valid, we can stop searching
toID = subsearchID;
toState = new_to;
break;
}
++subsearchID; // keep searching for a new state to plan to
}
// Check if we ever found a next state that is valid
if (subsearchID >= primaryPath.getStateCount())
{
// We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid. This is bad.
// I think this is a bug.
OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid goal state. This should not happen.");
return false;
}
// Plan between our two valid states
PathGeometric newPathSegment(si_);
// Not valid motion, replan
OMPL_DEBUG("LightningRetrieveRepair: Planning from %d to %d", fromID, toID);
if (!replan(fromState, toState, newPathSegment, ptc))
{
OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
return false;
}
// TODO make sure not approximate solution
// Reference to the path
std::vector<base::State*> &primaryPathStates = primaryPath.getStates();
// Remove all invalid states between (fromID, toID) - not including those states themselves
while (fromID != toID - 1)
{
OMPL_INFORM("LightningRetrieveRepair: Deleting state %d", fromID + 1);
primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
--toID; // because vector has shrunk
OMPL_INFORM("LightningRetrieveRepair: toID is now %d", toID);
}
// Insert new path segment into current path
OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
newPathSegment.getStateCount()-2, primaryPathStates.size());
// Note: skip first and last states because they should be same as our start and goal state, same as `fromID` and `toID`
for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
{
std::size_t insertLocation = toID + i - 1;
OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d",
i, insertLocation);
primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
si_->cloneState(newPathSegment.getStates()[i]) );
}
OMPL_DEBUG("LightningRetrieveRepair: Inserted new states into old path. New length: %d", primaryPathStates.size());
// Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we ignore start and goal
toID = toID + newPathSegment.getStateCount() - 2;
OMPL_DEBUG("LightningRetrieveRepair: Continuing searching at state %d", toID);
}
}
OMPL_INFORM("LightningRetrieveRepair: Done repairing");
return true;
}
unsigned int PortalPath::updateLocation( const Agents::BaseAgent * agent, const NavMeshPtr & navMesh, const NavMeshLocalizer * localizer, PathPlanner * planner ) {
// If off path, replan get a new route
// TODO: If off "approach" vector, recompute crossing
bool changed = false;
unsigned int currNodeID = getNode();
const NavMeshNode * currNode = &(navMesh->getNode( currNodeID ) );
// test current location
const Vector2 & p = agent->_pos;
const unsigned int PORTAL_COUNT = static_cast< unsigned int >( _route->getPortalCount() );
if ( ! currNode->containsPoint( p ) ) {
// test to see if I've progressed to the next
bool gotoNext = false;
const NavMeshNode * nextNode = 0x0;
if ( _currPortal + 1 < PORTAL_COUNT ) {
// there is another way portal to test
const WayPortal * nextPortal = _route->getPortal( _currPortal + 1 );
size_t nextID = nextPortal->_nodeID;
nextNode = &(navMesh->getNode( (unsigned int) nextID ) );
gotoNext = nextNode->containsPoint( p );
} else if ( _currPortal < PORTAL_COUNT ) {
// the next node is the goal polygon
nextNode = &(navMesh->getNode( (unsigned int) _route->getEndNode() ) );
gotoNext = nextNode->containsPoint( p );
}
if ( gotoNext ) {
// I've made progress, simply advance
++_currPortal;
assert( _currPortal <= PORTAL_COUNT && "Incremented current portal larger than goal" );
changed = true;
}
else {
const NavMeshNode * prevNode = 0x0;
// test to see if I've gone backwards
bool gotoPrev = false;
if ( _currPortal > 0 ) {
const WayPortal * prevPortal = _route->getPortal( _currPortal - 1 );
size_t prevID = prevPortal->_nodeID;
prevNode = &(navMesh->getNode( (unsigned int) prevID ) );
gotoPrev = prevNode->containsPoint( p );
}
if ( gotoPrev ) {
// back up to previous way portal in path
--_currPortal;
changed = true;
} else {
// Agent is not in current, previous or next polygons - agent got
// pushed off path - find a new path
// Path needs the nav mesh
// Assume that I must be in a neighboring node (the alternative is
// catstrophic)
// search current node's neighbors that aren't previous and aren't next
const size_t NBR_COUNT = currNode->getNeighborCount();
for ( size_t n = 0; n < NBR_COUNT; ++n ) {
const NavMeshNode * node = currNode->getNeighbor( n );
if ( node == nextNode || node == prevNode ) continue;
if ( node->containsPoint( p ) ) {
// find a new path from this node to the goal
replan( p, node->getID(), _route->getEndNode(), agent->_radius, planner );
changed = true;
}
}
// It is possible for the agent, in some cases, to advance several nodes in a single time step
// (e.g., when the navigation mesh has many long, skinny triangles and the agent steps across the
// narrow fan).
// In this case, the agent should search forwards along the path before blindly searching.
// TODO:
// If it gets "lost" at the beginning of a long path, I'm doing a bunch of wasted testing.
// Given how far the agent is from a particular portal, I know I should probably stop
// looking as the portals are only going to get farther. So, that means the inside
// query should CHEAPLY compute some sense of distance to the polygon so I can drop
// out.
if ( changed == false ) {
size_t testPortal = _currPortal + 2;
while ( testPortal < PORTAL_COUNT ) {
const WayPortal * nextPortal = _route->getPortal( testPortal );
size_t testID = nextPortal->_nodeID;
const NavMeshNode * testNode = &(navMesh->getNode( (unsigned int) testID ) );
if ( testNode->containsPoint( p ) ) {
_currPortal = testPortal;
changed = true;
break;
}
++testPortal;
}
}
if ( changed == false ) {
// I exited the loop without finding an intermediate node -- test the goal node
const NavMeshNode * testNode = &(navMesh->getNode( (unsigned int) _route->getEndNode() ) );
if ( testNode->containsPoint( p ) ) {
_currPortal = PORTAL_COUNT;
changed = true;
}
}
if ( !changed ) {
#ifdef _DEBUG
logger << Logger::WARN_MSG << "Agent " << agent->_id << " got pushed from its goal into a non-adjacent goal!!\n";
#endif
// do a full find path searching for the agent position
float lastElevation = currNode->getElevation( p );
unsigned int nodeID = localizer->findNodeBlind( p, lastElevation );
if ( nodeID != NavMeshLocation::NO_NODE ) {
try {
replan( p, nodeID, _route->getEndNode(), agent->_radius, planner );
} catch ( PathPlannerException ) {
std::stringstream ss;
ss << "Agent " << agent->_id << " trying to find a path from " << nodeID << " to " << _route->getEndNode() << ". A* finished without a route!";
throw PathPlannerException( ss.str() );
}
}
changed = true;
}
}
}
}
/*
// TODO: Implement the idea of replanning the path based on getting pushed off
// approach vector
if ( !changed && _currPortal < _route->getPortalCount() ) {
// vector from crossing point to current position.
// examine angle between original approach vector and current approach vector.
// If the angle > some threshold, replan.
}
*/
if ( _currPortal < _route->getPortalCount() ) {
return _route->getPortal( _currPortal )->_nodeID;
} else {
return _route->getEndNode();
}
}
