void AI::update(float dt, const std::list <CAR> & othercars)
{
updatePlan(); //TODO: THIS CALLS EMPTY FUNCTION. ANYONE CHECK IF THIS FUNCTION IS NEEDED.
for (std::vector<AI_Car>::iterator it = AI_Cars.begin (); it != AI_Cars.end (); it++ )
{
analyzeOthers(&(*it), dt, othercars);
updateGasBrake(&(*it));
updateSteer(&(*it));
}
}
bool MoveBaseSBPL::makePlan(){
std::cout << "Planning for new goal...\n";
// Apply map updates that were buffered from the cost map
applyMapUpdates();
unsigned int x, y;
const CostMap2D& cm = getCostMap();
// Set start state based on global pose.
cm.WC_MC(stateMsg.pos.x, stateMsg.pos.y, x, y);
envNav2D_.SetStart(x, y);
araPlanner_->set_start(envNav2D_.GetStateFromCoord(x, y));
// Set goal state
cm.WC_MC(stateMsg.goal.x, stateMsg.goal.y, x, y);
envNav2D_.SetGoal(x, y);
araPlanner_->set_goal(envNav2D_.GetStateFromCoord(x, y));
// Invoke the planner
std::vector<int> solutionStateIDs;
// Extract the solution, if available
if(araPlanner_->replan(PLANNER_TIMEOUT, &solutionStateIDs)){
std::list<std_msgs::Pose2DFloat32> plan;
for(std::vector<int>::const_iterator it = solutionStateIDs.begin(); it != solutionStateIDs.end(); ++it){
int state = *it;
int mx, my;
envNav2D_.GetCoordFromState(state, mx, my);
double wx, wy;
cm.MC_WC(mx, my, wx, wy);
std_msgs::Pose2DFloat32 waypoint;
waypoint.x = wx;
waypoint.y = wy;
plan.push_back(waypoint);
}
updatePlan(plan);
return true;
}
else{
std::cout << "No plan found\n";
return false;
}
}
bool MoveBaseSBPL::makePlan(){
ROS_DEBUG("Planning for new goal...\n");
ompl::SBPLPlannerStatistics::entry & statsEntry(pStat_.top());
try {
const CostMap2D& cm = getCostMap();
// Copy out start and goal states to minimize locking requirement. Lock was not previously required because the
// planner and controller were running on the same thread and the only contention was for cost map updates on call
// backs. Note that cost map queries here are const methods that merely do co-ordinate transformations, so we do not need
// to lock protect those.
stateMsg.lock();
statsEntry.start = stateMsg.pos;
statsEntry.goal = stateMsg.goal;
stateMsg.unlock();
// Set start state based on global pose, updating statistics in the process.
cm.WC_MC(statsEntry.start.x, statsEntry.start.y, statsEntry.startIx, statsEntry.startIy);
env_->SetStart(statsEntry.start);
statsEntry.startState = env_->GetStateFromPose(statsEntry.start);
if (0 > statsEntry.startState) {
ROS_ERROR("invalid start state ID %d from pose (%+8.3f, %+8.3f): outside of map?\n",
statsEntry.startState, statsEntry.start.x, statsEntry.start.y);
return false;
}
int status(pMgr_->set_start(statsEntry.startState));
if (1 != status) {
ROS_ERROR("failed to set start state ID %d from (%ud, %ud): pMgr_->set_start() returned %d\n",
statsEntry.startState, statsEntry.startIx, statsEntry.startIy, status);
return false;
}
// Set goal state, updating statistics in the process.
cm.WC_MC(statsEntry.goal.x, statsEntry.goal.y, statsEntry.goalIx, statsEntry.goalIy);
env_->SetGoal(statsEntry.goal);
statsEntry.goalState = env_->GetStateFromPose(statsEntry.goal);
if (0 > statsEntry.goalState) {
ROS_ERROR("invalid goal state ID %d from pose (%+8.3f, %+8.3f): outside of map?\n",
statsEntry.goalState, statsEntry.goal.x, statsEntry.goal.y);
return false;
}
status = pMgr_->set_goal(statsEntry.goalState);
if (1 != status) {
ROS_ERROR("failed to set goal state ID %d from (%ud, %ud): pMgr_->set_goal() returned %d\n",
statsEntry.goalState, statsEntry.goalIx, statsEntry.goalIy, status);
return false;
}
// Invoke the planner, updating the statistics in the process.
std::vector<int> solutionStateIDs;
statsEntry.allocated_time_sec = plannerTimeLimit_;
statsEntry.status = pMgr_->replan(statsEntry.allocated_time_sec,
&statsEntry.actual_time_wall_sec,
&statsEntry.actual_time_user_sec,
&statsEntry.actual_time_system_sec,
&solutionStateIDs);
// Extract the solution, if available, and update statistics (as usual).
statsEntry.plan_length_m = 0;
statsEntry.plan_angle_change_rad = 0;
if ((1 == statsEntry.status) && (1 < solutionStateIDs.size())) {
std::list<std_msgs::Pose2DFloat32> plan;
double prevx(0), prevy(0), prevth(0);
prevth = 42.17; // to detect when it has been initialized (see 42 below)
for(std::vector<int>::const_iterator it = solutionStateIDs.begin(); it != solutionStateIDs.end(); ++it){
std_msgs::Pose2DFloat32 const waypoint(env_->GetPoseFromState(*it));
// update stats:
// - first round, nothing to do
// - second round, update path length only
// - third round, update path length and angular change
if (plan.empty()) {
prevx = waypoint.x;
prevy = waypoint.y;
}
else {
double const dx(waypoint.x - prevx);
double const dy(waypoint.y - prevy);
statsEntry.plan_length_m += sqrt(pow(dx, 2) + pow(dy, 2));
double const th(atan2(dy, dx));
if (42 > prevth) // see 42.17 above
statsEntry.plan_angle_change_rad += fabs(mod2pi(th - prevth));
prevx = waypoint.x;
prevy = waypoint.y;
prevth = th;
#warning 'add the cumulation of delta(waypoint.th) now that we can have 3D plans'
}
plan.push_back(waypoint);
}
// probably we should add the delta from the last theta to
// the goal theta onto statsEntry.plan_angle_change_rad here, but
// that depends on whether our planner handles theta for us,
// and needs special handling if we have no plan...
{
ostringstream prefix_os;
prefix_os << "[" << goalCount_ << "] ";
static size_t lastPlannedGoal(171717);
char const * title("\nREPLAN SUCCESS");
if (lastPlannedGoal != goalCount_) {
lastPlannedGoal = goalCount_;
title = "\nPLAN SUCCESS";
}
statsEntry.logFile(planStatsFile_.c_str(), title, prefix_os.str().c_str());
}
//// statsEntry.logInfo("move_base_sbpl: ");
pStat_.pushBack(pMgr_->getName(), env_->getName());
updatePlan(plan);
return true;
}
}
catch (std::runtime_error const & ee) {
ROS_ERROR("runtime_error in makePlan(): %s\n", ee.what());
return false;
}
ROS_ERROR("No plan found\n");
{
static size_t lastFailedGoal(424242);
if (lastFailedGoal != goalCount_) {
lastFailedGoal = goalCount_;
ostringstream prefix_os;
prefix_os << "[" << goalCount_ << "] ";
statsEntry.logFile(planStatsFile_.c_str(), "\nPLAN FAILURE", prefix_os.str().c_str());
}
}
return false;
}
