FootstepPlannerEnvironment.cpp
Go to the documentation of this file.
00001 /*
00002  * A footstep planner for humanoid robots
00003  *
00004  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00005  * http://www.ros.org/wiki/footstep_planner
00006  *
00007  *
00008  * This program is free software: you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation, version 3.
00011  *
00012  * This program is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  * GNU General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU General Public License
00018  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00021 #include <footstep_planner/FootstepPlannerEnvironment.h>
00022 
00023 
00024 namespace footstep_planner
00025 {
00026 FootstepPlannerEnvironment::FootstepPlannerEnvironment(
00027     const environment_params& params)
00028 : DiscreteSpaceInformation(),
00029   ivIdPlanningGoal(-1),
00030   ivIdStartFootLeft(-1),
00031   ivIdStartFootRight(-1),
00032   ivIdGoalFootLeft(-1),
00033   ivIdGoalFootRight(-1),
00034   ivpStateHash2State(
00035     new std::vector<const PlanningState*>[params.hash_table_size]),
00036   ivFootstepSet(params.footstep_set),
00037   ivHeuristicConstPtr(params.heuristic),
00038   ivFootsizeX(params.footsize_x),
00039   ivFootsizeY(params.footsize_y),
00040   ivOriginFootShiftX(params.foot_origin_shift_x),
00041   ivOriginFootShiftY(params.foot_origin_shift_y),
00042   ivMaxFootstepX(disc_val(params.max_footstep_x, params.cell_size)),
00043   ivMaxFootstepY(disc_val(params.max_footstep_y, params.cell_size)),
00044   ivMaxFootstepTheta(
00045     angle_state_2_cell(params.max_footstep_theta, params.num_angle_bins)),
00046   ivMaxInvFootstepX(disc_val(params.max_inverse_footstep_x, params.cell_size)),
00047   ivMaxInvFootstepY(disc_val(params.max_inverse_footstep_y, params.cell_size)),
00048   ivMaxInvFootstepTheta(
00049     angle_state_2_cell(params.max_inverse_footstep_theta,
00050                        params.num_angle_bins)),
00051   ivStepCost(cvMmScale * params.step_cost),
00052   ivCollisionCheckAccuracy(params.collision_check_accuracy),
00053   ivHashTableSize(params.hash_table_size),
00054   ivCellSize(params.cell_size),
00055   ivNumAngleBins(params.num_angle_bins),
00056   ivForwardSearch(params.forward_search),
00057   ivMaxStepWidth(double(disc_val(params.max_step_width, params.cell_size))),
00058   ivNumRandomNodes(params.num_random_nodes),
00059   ivRandomNodeDist(params.random_node_distance / ivCellSize),
00060   ivHeuristicScale(params.heuristic_scale),
00061   ivHeuristicExpired(true),
00062   ivNumExpandedStates(0)
00063 {
00064   int num_angle_bins_half = ivNumAngleBins / 2;
00065   if (ivMaxFootstepTheta >= num_angle_bins_half)
00066     ivMaxFootstepTheta -= ivNumAngleBins;
00067   if (ivMaxInvFootstepTheta >= num_angle_bins_half)
00068     ivMaxInvFootstepTheta -= ivNumAngleBins;
00069 
00070   int num_x = ivMaxFootstepX - ivMaxInvFootstepX + 1;
00071   ivpStepRange = new bool[num_x * (ivMaxFootstepY - ivMaxInvFootstepY + 1)];
00072 
00073   // determine whether a (x,y) translation can be performed by the robot by
00074   // checking if it is within a certain area of performable steps
00075   for (int j = ivMaxInvFootstepY; j <= ivMaxFootstepY; ++j)
00076   {
00077     for (int i = ivMaxInvFootstepX; i <= ivMaxFootstepX; ++i)
00078     {
00079       ivpStepRange[(j - ivMaxInvFootstepY) * num_x + (i - ivMaxInvFootstepX)] =
00080         pointWithinPolygon(i, j, params.step_range);
00081     }
00082   }
00083 }
00084 
00085 
00086 FootstepPlannerEnvironment::~FootstepPlannerEnvironment()
00087 {
00088   reset();
00089   if (ivpStateHash2State)
00090   {
00091     delete[] ivpStateHash2State;
00092     ivpStateHash2State = NULL;
00093   }
00094   if (ivpStepRange)
00095   {
00096     delete[] ivpStepRange;
00097     ivpStepRange = NULL;
00098   }
00099 }
00100 
00101 
00102 std::pair<int, int>
00103 FootstepPlannerEnvironment::updateGoal(const State& foot_left,
00104                                        const State& foot_right)
00105 {
00106   // keep the old IDs
00107   int goal_foot_id_left = ivIdGoalFootLeft;
00108   int goal_foot_id_right = ivIdGoalFootRight;
00109 
00110   // update the states for both feet (if necessary)
00111   const PlanningState* p_foot_left = getHashEntry(foot_left);
00112   if (p_foot_left == NULL)
00113     p_foot_left = createNewHashEntry(foot_left);
00114   const PlanningState* p_foot_right = getHashEntry(foot_right);
00115   if (p_foot_right == NULL)
00116     p_foot_right = createNewHashEntry(foot_right);
00117   ivIdGoalFootLeft = p_foot_left->getId();
00118   ivIdGoalFootRight = p_foot_right->getId();
00119   // check if everything has been set correctly
00120   assert(ivIdGoalFootLeft != -1);
00121   assert(ivIdGoalFootRight != -1);
00122 
00123   // if using the forward search a change of the goal states involves an
00124   // update of the heuristic
00125   if (ivForwardSearch)
00126   {
00127     // check if the goal states have been changed
00128     if (goal_foot_id_left != ivIdGoalFootLeft &&
00129         goal_foot_id_right != ivIdGoalFootRight)
00130     {
00131       ivHeuristicExpired = true;
00132       setStateArea(*p_foot_left, *p_foot_right);
00133     }
00134   }
00135 
00136   return std::pair<int, int>(ivIdGoalFootLeft, ivIdGoalFootRight);
00137 }
00138 
00139 
00140 std::pair<int, int>
00141 FootstepPlannerEnvironment::updateStart(const State& foot_left,
00142                                         const State& foot_right)
00143 {
00144   // keep the old IDs
00145   int start_foot_id_left = ivIdStartFootLeft;
00146   int start_foot_id_right = ivIdStartFootRight;
00147 
00148   // update the states for both feet (if necessary)
00149   const PlanningState* p_foot_left = getHashEntry(foot_left);
00150   if (p_foot_left == NULL)
00151     p_foot_left = createNewHashEntry(foot_left);
00152   const PlanningState* p_foot_right = getHashEntry(foot_right);
00153   if (p_foot_right == NULL)
00154     p_foot_right = createNewHashEntry(foot_right);
00155   ivIdStartFootLeft = p_foot_left->getId();
00156   ivIdStartFootRight = p_foot_right->getId();
00157   // check if everything has been set correctly
00158   assert(ivIdStartFootLeft != -1);
00159   assert(ivIdStartFootRight != -1);
00160 
00161   // if using the backward search a change of the start states involves an
00162   // update of the heuristic
00163   if (!ivForwardSearch)
00164   {
00165     // check if the start states have been changed
00166     if (start_foot_id_left != ivIdStartFootLeft ||
00167         start_foot_id_right != ivIdStartFootRight)
00168     {
00169       ivHeuristicExpired = true;
00170       setStateArea(*p_foot_left, *p_foot_right);
00171     }
00172   }
00173 
00174   return std::pair<int, int>(ivIdStartFootLeft, ivIdStartFootRight);
00175 }
00176 
00177 
00178 const PlanningState*
00179 FootstepPlannerEnvironment::createNewHashEntry(const State& s)
00180 {
00181   PlanningState tmp(s, ivCellSize, ivNumAngleBins, ivHashTableSize);
00182   return createNewHashEntry(tmp);
00183 }
00184 
00185 
00186 const PlanningState*
00187 FootstepPlannerEnvironment::createNewHashEntry(const PlanningState& s)
00188 {
00189   unsigned int state_hash = s.getHashTag();
00190   PlanningState* new_state = new PlanningState(s);
00191 
00192   size_t state_id = ivStateId2State.size();
00193   assert(state_id < (size_t)std::numeric_limits<int>::max());
00194 
00195   // insert the ID of the new state into the corresponding map
00196   new_state->setId(state_id);
00197   ivStateId2State.push_back(new_state);
00198 
00199   // insert the new state into the hash map at the corresponding position
00200   ivpStateHash2State[state_hash].push_back(new_state);
00201 
00202   int* entry = new int[NUMOFINDICES_STATEID2IND];
00203   StateID2IndexMapping.push_back(entry);
00204   for(int i = 0; i < NUMOFINDICES_STATEID2IND; ++i)
00205   {
00206     StateID2IndexMapping[state_id][i] = -1;
00207   }
00208 
00209   assert(StateID2IndexMapping.size() - 1 == state_id);
00210 
00211   return new_state;
00212 }
00213 
00214 
00215 const PlanningState*
00216 FootstepPlannerEnvironment::getHashEntry(const State& s)
00217 {
00218   PlanningState tmp(s, ivCellSize, ivNumAngleBins, ivHashTableSize);
00219   return getHashEntry(tmp);
00220 }
00221 
00222 
00223 const PlanningState*
00224 FootstepPlannerEnvironment::getHashEntry(const PlanningState& s)
00225 {
00226   unsigned int state_hash = s.getHashTag();
00227   std::vector<const PlanningState*>::const_iterator state_iter;
00228   for (state_iter = ivpStateHash2State[state_hash].begin();
00229        state_iter != ivpStateHash2State[state_hash].end();
00230        ++state_iter)
00231   {
00232     if (*(*state_iter) == s)
00233       return *state_iter;
00234   }
00235 
00236   return NULL;
00237 }
00238 
00239 const PlanningState*
00240 FootstepPlannerEnvironment::createHashEntryIfNotExists(
00241     const PlanningState& s)
00242 {
00243   const PlanningState* hash_entry = getHashEntry(s);
00244   if (hash_entry == NULL)
00245     hash_entry = createNewHashEntry(s);
00246 
00247   return hash_entry;
00248 
00249 }
00250 
00251 
00252 int
00253 FootstepPlannerEnvironment::stepCost(const PlanningState& a,
00254                                      const PlanningState& b)
00255 {
00256   if (a == b)
00257     return 0;
00258 
00259   // NOTE: instead of using cont_val() the calculation is done directly
00260       // here because cont_val() truncates the input length to int
00261   double dist = euclidean_distance(
00262       a.getX(), a.getY(), b.getX(), b.getY()) * ivCellSize;
00263 
00264   return int(cvMmScale * dist) + ivStepCost;
00265 }
00266 
00267 
00268 bool
00269 FootstepPlannerEnvironment::occupied(const State& s)
00270 {
00271   return occupied(PlanningState(s, ivCellSize, ivNumAngleBins,
00272                                 ivHashTableSize));
00273 }
00274 
00275 
00276 bool
00277 FootstepPlannerEnvironment::occupied(const PlanningState& s)
00278 {
00279   double x = cell_2_state(s.getX(), ivCellSize);
00280   double y = cell_2_state(s.getY(), ivCellSize);
00281   // collision check for the planning state
00282   if (ivMapPtr->isOccupiedAt(x,y))
00283     return true;
00284   double theta = angle_cell_2_state(s.getTheta(), ivNumAngleBins);
00285   double theta_cos = cos(theta);
00286   double theta_sin = sin(theta);
00287 
00288   // transform the planning state to the foot center
00289   x += theta_cos*ivOriginFootShiftX - theta_sin*ivOriginFootShiftY;
00290   if (s.getLeg() == LEFT)
00291     y += theta_sin*ivOriginFootShiftX + theta_cos*ivOriginFootShiftY;
00292   else // leg == RLEG
00293     y += theta_sin*ivOriginFootShiftX - theta_cos*ivOriginFootShiftY;
00294 
00295   // collision check for the foot center
00296   return collision_check(x, y, theta, ivFootsizeX, ivFootsizeY,
00297                          ivCollisionCheckAccuracy, *ivMapPtr);
00298 }
00299 
00300 
00301 bool
00302 FootstepPlannerEnvironment::getState(unsigned int id, State* s)
00303 {
00304   if (id >= ivStateId2State.size())
00305     return false;
00306 
00307   const PlanningState* planning_state = ivStateId2State[id];
00308   s->setX(cell_2_state(planning_state->getX(), ivCellSize));
00309   s->setY(cell_2_state(planning_state->getY(), ivCellSize));
00310   s->setTheta(angles::normalize_angle(angle_cell_2_state(
00311       planning_state->getTheta(), ivNumAngleBins)));
00312   s->setLeg(planning_state->getLeg());
00313 
00314   return true;
00315 }
00316 
00317 
00318 void
00319 FootstepPlannerEnvironment::updateMap(gridmap_2d::GridMap2DPtr map)
00320 {
00321   ivMapPtr.reset();
00322   ivMapPtr = map;
00323 
00324   if (ivHeuristicConstPtr->getHeuristicType() == Heuristic::PATH_COST)
00325   {
00326     boost::shared_ptr<PathCostHeuristic> h =
00327         boost::dynamic_pointer_cast<PathCostHeuristic>(
00328             ivHeuristicConstPtr);
00329     h->updateMap(map);
00330 
00331     ivHeuristicExpired = true;
00332   }
00333 }
00334 
00335 
00336 void
00337 FootstepPlannerEnvironment::updateHeuristicValues()
00338 {
00339   // check if start and goal have been set
00340   assert(ivIdGoalFootLeft != -1 && ivIdGoalFootRight != -1);
00341   assert(ivIdStartFootLeft != -1 && ivIdStartFootRight != -1);
00342 
00343   if (!ivHeuristicExpired)
00344     return;
00345 
00346   ROS_INFO("Updating the heuristic values.");
00347 
00348   if (ivHeuristicConstPtr->getHeuristicType() == Heuristic::PATH_COST)
00349   {
00350     boost::shared_ptr<PathCostHeuristic> h =
00351         boost::dynamic_pointer_cast<PathCostHeuristic>(
00352             ivHeuristicConstPtr);
00353     MDPConfig MDPCfg;
00354     InitializeMDPCfg(&MDPCfg);
00355     const PlanningState* start = ivStateId2State[MDPCfg.startstateid];
00356     const PlanningState* goal = ivStateId2State[MDPCfg.goalstateid];
00357 
00358     // NOTE: start/goal state are set to left leg
00359     bool success;
00360     if (ivForwardSearch)
00361       success = h->calculateDistances(*start, *goal);
00362     else
00363       success = h->calculateDistances(*goal, *start);
00364     if (!success)
00365     {
00366       ROS_ERROR("Failed to calculate path cost heuristic.");
00367       exit(1);
00368     }
00369   }
00370 
00371   ROS_DEBUG("Finished updating the heuristic values.");
00372   ivHeuristicExpired = false;
00373 }
00374 
00375 
00376 void
00377 FootstepPlannerEnvironment::reset()
00378 {
00379   for(unsigned int i = 0; i < ivStateId2State.size(); ++i)
00380   {
00381     if (ivStateId2State[i])
00382     {
00383       delete ivStateId2State[i];
00384     }
00385   }
00386   ivStateId2State.clear();
00387 
00388   if (ivpStateHash2State)
00389   {
00390     for(int i = 0; i < ivHashTableSize; ++i)
00391       ivpStateHash2State[i].clear();
00392   }
00393 
00394   StateID2IndexMapping.clear();
00395 
00396   ivExpandedStates.clear();
00397   ivNumExpandedStates = 0;
00398   ivRandomStates.clear();
00399 
00400   ivIdPlanningGoal = -1;
00401 
00402   ivIdGoalFootLeft = -1;
00403   ivIdGoalFootRight = -1;
00404   ivIdStartFootLeft = -1;
00405   ivIdStartFootRight = -1;
00406 
00407   ivHeuristicExpired = true;
00408 }
00409 
00410 
00411 bool
00412 FootstepPlannerEnvironment::closeToStart(const PlanningState& from)
00413 {
00414   // NOTE: "goal check" for backward planning
00415   const PlanningState* start;
00416   if (from.getLeg() == RIGHT)
00417     start = ivStateId2State[ivIdStartFootLeft];
00418   else
00419     start = ivStateId2State[ivIdStartFootRight];
00420 
00421   return reachable(*start, from);
00422 }
00423 
00424 
00425 bool
00426 FootstepPlannerEnvironment::closeToGoal(const PlanningState& from)
00427 {
00428   // NOTE: "goal check" for forward planning
00429   const PlanningState* goal;
00430   if (from.getLeg() == RIGHT)
00431     goal = ivStateId2State[ivIdGoalFootLeft];
00432   else
00433     goal = ivStateId2State[ivIdGoalFootRight];
00434 
00435   // TODO: check step if reachable == True
00436   return reachable(from, *goal);
00437 }
00438 
00439 
00440 bool
00441 FootstepPlannerEnvironment::reachable(const PlanningState& from,
00442                                       const PlanningState& to)
00443 {
00444   if (euclidean_distance(from.getX(), from.getY(), to.getX(), to.getY()) >
00445       ivMaxStepWidth)
00446   {
00447       return false;
00448   }
00449 
00450   tf::Transform step =
00451     tf::Pose(
00452       tf::createQuaternionFromYaw(
00453         angle_cell_2_state(from.getTheta(), ivNumAngleBins)),
00454       tf::Point(cell_2_state(from.getX(), ivCellSize),
00455                 cell_2_state(from.getY(), ivCellSize),
00456                 0.0)).inverse() *
00457     tf::Pose(
00458       tf::createQuaternionFromYaw(
00459         angle_cell_2_state(from.getTheta(), ivNumAngleBins)),
00460       tf::Point(cell_2_state(to.getX(), ivCellSize),
00461                 cell_2_state(to.getY(), ivCellSize),
00462                 0.0));
00463   int footstep_x = disc_val(step.getOrigin().x(), ivCellSize);
00464   int footstep_y = disc_val(step.getOrigin().y(), ivCellSize);
00465 
00466   // calculate the footstep rotation
00467   int footstep_theta = to.getTheta() - from.getTheta();
00468   // transform the value into [-ivNumAngleBins/2..ivNumAngleBins/2)
00469   int num_angle_bins_half = ivNumAngleBins / 2;
00470   if (footstep_theta >= num_angle_bins_half)
00471     footstep_theta -= ivNumAngleBins;
00472   else if (footstep_theta < -num_angle_bins_half)
00473     footstep_theta += ivNumAngleBins;
00474 
00475   // adjust for the left foot
00476   if (from.getLeg() == LEFT)
00477   {
00478     footstep_y = -footstep_y;
00479     footstep_theta = -footstep_theta;
00480   }
00481 
00482   // check if footstep_x is not within the executable range
00483   if (footstep_x > ivMaxFootstepX || footstep_x < ivMaxInvFootstepX)
00484       return false;
00485   // check if footstep_y is not within the executable range
00486   if (footstep_y > ivMaxFootstepY || footstep_y < ivMaxInvFootstepY)
00487       return false;
00488   // check if footstep_theta is not within the executable range
00489   if (footstep_theta > ivMaxFootstepTheta ||
00490       footstep_theta < ivMaxInvFootstepTheta)
00491       return false;
00492   return ivpStepRange[(footstep_y - ivMaxInvFootstepY) *
00493                       (ivMaxFootstepX - ivMaxInvFootstepX + 1) +
00494                       (footstep_x - ivMaxInvFootstepX)];
00495 
00496 //  // get the (continuous) orientation of state 'from'
00497 //  double orient = -(angle_cell_2_state(from.getTheta(), ivNumAngleBins));
00498 //  double orient_cos = cos(orient);
00499 //  double orient_sin = sin(orient);
00500 //
00501 //  // calculate the footstep shift and rotate it into the 'from'-view
00502 //  int footstep_x = to.getX() - from.getX();
00503 //  int footstep_y = to.getY() - from.getY();
00504 //  double shift_x = footstep_x * orient_cos - footstep_y * orient_sin;
00505 //  double shift_y = footstep_x * orient_sin + footstep_y * orient_cos;
00506 //  footstep_x = round(shift_x);
00507 //  footstep_y = round(shift_y);
00508 //
00509 //  // calculate the footstep rotation
00510 //  int footstep_theta = to.getTheta() - from.getTheta();
00511 //
00512 //  // transform the value into [-ivNumAngleBins/2..ivNumAngleBins/2)
00513 //  int num_angle_bins_half = ivNumAngleBins / 2;
00514 //  if (footstep_theta >= num_angle_bins_half)
00515 //    footstep_theta -= ivNumAngleBins;
00516 //  else if (footstep_theta < -num_angle_bins_half)
00517 //    footstep_theta += ivNumAngleBins;
00518 //
00519 //  // adjust for the left foot
00520 //  if (from.getLeg() == LEFT)
00521 //  {
00522 //    footstep_y = -footstep_y;
00523 //    footstep_theta = -footstep_theta;
00524 //  }
00525 //
00526 //  return (footstep_x <= ivMaxFootstepX &&
00527 //          footstep_x >= ivMaxInvFootstepX &&
00528 //          footstep_y <= ivMaxFootstepY &&
00529 //          footstep_y >= ivMaxInvFootstepY &&
00530 //          footstep_theta <= ivMaxFootstepTheta &&
00531 //          footstep_theta >= ivMaxInvFootstepTheta);
00532 }
00533 
00534 
00535 void
00536 FootstepPlannerEnvironment::getPredsOfGridCells(
00537     const std::vector<State>& changed_states,
00538     std::vector<int>* pred_ids)
00539 {
00540   pred_ids->clear();
00541 
00542   std::vector<State>::const_iterator state_iter;
00543   for (state_iter = changed_states.begin();
00544       state_iter != changed_states.end();
00545       ++state_iter)
00546   {
00547     PlanningState s(*state_iter, ivCellSize, ivNumAngleBins,
00548                     ivHashTableSize);
00549     // generate predecessor planning states
00550     std::vector<Footstep>::const_iterator footstep_set_iter;
00551     for(footstep_set_iter = ivFootstepSet.begin();
00552         footstep_set_iter != ivFootstepSet.end();
00553         ++footstep_set_iter)
00554     {
00555       PlanningState pred = footstep_set_iter->reverseMeOnThisState(s);
00556       // check if predecessor exists
00557       const PlanningState* pred_hash_entry = getHashEntry(pred);
00558       if (pred_hash_entry == NULL)
00559         continue;
00560       pred_ids->push_back(pred_hash_entry->getId());
00561     }
00562   }
00563 }
00564 
00565 
00566 void
00567 FootstepPlannerEnvironment::getSuccsOfGridCells(
00568     const std::vector<State>& changed_states,
00569     std::vector<int>* succ_ids)
00570 {
00571   succ_ids->clear();
00572 
00573   std::vector<State>::const_iterator state_iter;
00574   for (state_iter = changed_states.begin();
00575       state_iter != changed_states.end();
00576       ++state_iter)
00577   {
00578     PlanningState s(*state_iter, ivCellSize, ivNumAngleBins,
00579                     ivHashTableSize);
00580     // generate successors
00581     std::vector<Footstep>::const_iterator footstep_set_iter;
00582     for(footstep_set_iter = ivFootstepSet.begin();
00583         footstep_set_iter != ivFootstepSet.end();
00584         ++footstep_set_iter)
00585     {
00586       PlanningState succ = footstep_set_iter->performMeOnThisState(s);
00587       // check if successor exists
00588       const PlanningState* succ_hash_entry = getHashEntry(succ);
00589       if (succ_hash_entry == NULL)
00590         continue;
00591       succ_ids->push_back(succ_hash_entry->getId());
00592     }
00593   }
00594 }
00595 
00596 
00597 int
00598 FootstepPlannerEnvironment::GetFromToHeuristic(int FromStateID,
00599                                                int ToStateID)
00600 {
00601   assert(FromStateID >= 0 && (unsigned int) FromStateID < ivStateId2State.size());
00602   assert(ToStateID >= 0 && (unsigned int) ToStateID < ivStateId2State.size());
00603 
00604   if ((FromStateID == ivIdGoalFootLeft && ToStateID == ivIdGoalFootRight)
00605       || (FromStateID == ivIdGoalFootRight && ToStateID == ivIdGoalFootLeft)){
00606     return 0;
00607   }
00608 
00609   const PlanningState* from = ivStateId2State[FromStateID];
00610   const PlanningState* to = ivStateId2State[ToStateID];
00611   //            if (ivHeuristicConstPtr->getHeuristicType() == Heuristic::PATH_COST){
00612   //                    boost::shared_ptr<PathCostHeuristic> pathCostHeuristic = boost::dynamic_pointer_cast<PathCostHeuristic>(ivHeuristicConstPtr);
00613   //                    pathCostHeuristic->calculateDistances(*from, *to);
00614   //            }
00615   return GetFromToHeuristic(*from, *to);
00616 }
00617 
00618 int
00619 FootstepPlannerEnvironment::GetFromToHeuristic(const PlanningState& from,
00620                                                const PlanningState& to)
00621 {
00622   return cvMmScale * ivHeuristicScale *
00623     ivHeuristicConstPtr->getHValue(from, to);
00624 }
00625 
00626 
00627 int
00628 FootstepPlannerEnvironment::GetGoalHeuristic(int stateID)
00629 {
00630   return GetFromToHeuristic(stateID, ivIdGoalFootLeft);
00631 }
00632 
00633 
00634 void
00635 FootstepPlannerEnvironment::GetPreds(int TargetStateID,
00636                                      std::vector<int> *PredIDV,
00637                                      std::vector<int> *CostV)
00638 {
00639   PredIDV->clear();
00640   CostV->clear();
00641 
00642   assert(TargetStateID >= 0 &&
00643          (unsigned int) TargetStateID < ivStateId2State.size());
00644 
00645   // make goal state absorbing (only left!)
00646   if (TargetStateID == ivIdStartFootLeft)
00647     return;
00648   // add cheap transition from right to left, so right becomes an equivalent
00649   // goal
00650   if (TargetStateID == ivIdStartFootRight)
00651   {
00652     PredIDV->push_back(ivIdStartFootLeft);
00653     CostV->push_back(0.0);
00654     return;
00655   }
00656 
00657   const PlanningState* current = ivStateId2State[TargetStateID];
00658 
00659   // make sure goal state transitions are consistent with
00660   // GetSuccs(some_state, goal_state) where goal_state is reachable by an
00661   // arbitrary step from some_state
00662   if (ivForwardSearch)
00663   {
00664     if (TargetStateID == ivIdGoalFootLeft || TargetStateID == ivIdGoalFootRight)
00665     {
00666       const PlanningState* s;
00667       int cost;
00668       std::vector<int>::const_iterator state_id_iter;
00669       for(state_id_iter = ivStateArea.begin();
00670           state_id_iter != ivStateArea.end();
00671           ++state_id_iter)
00672       {
00673         s = ivStateId2State[*state_id_iter];
00674         cost = stepCost(*current, *s);
00675         PredIDV->push_back(s->getId());
00676         CostV->push_back(cost);
00677       }
00678       return;
00679     }
00680   }
00681 
00682   ivExpandedStates.insert(std::pair<int,int>(current->getX(), current->getY()));
00683   ++ivNumExpandedStates;
00684 
00685   if (closeToStart(*current))
00686   {
00687     // map to the start state id
00688     PredIDV->push_back(ivIdStartFootLeft);
00689     // get actual costs (dependent on whether the start foot is left or right)
00690     int start_id;
00691     if (current->getLeg() == RIGHT)
00692       start_id = ivIdStartFootLeft;
00693     else
00694       start_id = ivIdStartFootRight;
00695     CostV->push_back(stepCost(*current, *ivStateId2State[start_id]));
00696 
00697     return;
00698   }
00699 
00700   PredIDV->reserve(ivFootstepSet.size());
00701   CostV->reserve(ivFootstepSet.size());
00702   std::vector<Footstep>::const_iterator footstep_set_iter;
00703   for(footstep_set_iter = ivFootstepSet.begin();
00704       footstep_set_iter != ivFootstepSet.end();
00705       ++footstep_set_iter)
00706   {
00707     const PlanningState predecessor =
00708         footstep_set_iter->reverseMeOnThisState(*current);
00709     if (occupied(predecessor))
00710       continue;
00711 
00712     const PlanningState* predecessor_hash = createHashEntryIfNotExists(
00713         predecessor);
00714 
00715     int cost = stepCost(*current, *predecessor_hash);
00716     PredIDV->push_back(predecessor_hash->getId());
00717     CostV->push_back(cost);
00718   }
00719 }
00720 
00721 
00722 int
00723 FootstepPlannerEnvironment::GetStartHeuristic(int stateID)
00724 {
00725   return GetFromToHeuristic(stateID, ivIdStartFootLeft);
00726 }
00727 
00728 
00729 void
00730 FootstepPlannerEnvironment::GetSuccs(int SourceStateID,
00731                                      std::vector<int> *SuccIDV,
00732                                      std::vector<int> *CostV)
00733 {
00734   SuccIDV->clear();
00735   CostV->clear();
00736 
00737   assert(SourceStateID >= 0 &&
00738          unsigned(SourceStateID) < ivStateId2State.size());
00739 
00740   // make goal state absorbing (only left!)
00741   if (SourceStateID == ivIdGoalFootLeft)
00742   {
00743     return;
00744   }
00745   // add cheap transition from right to left, so right becomes an
00746   // equivalent goal
00747   if (SourceStateID == ivIdGoalFootRight)
00748   {
00749     SuccIDV->push_back(ivIdGoalFootLeft);
00750     CostV->push_back(0.0);
00751     return;
00752   }
00753 
00754   const PlanningState* current = ivStateId2State[SourceStateID];
00755 
00756   // make sure start state transitions are consistent with
00757   // GetPreds(some_state, start_state) where some_state is reachable by an
00758   // arbitrary step from start_state
00759   if (!ivForwardSearch)
00760   {
00761     if (SourceStateID == ivIdStartFootLeft ||
00762         SourceStateID == ivIdStartFootRight)
00763     {
00764       const PlanningState* s;
00765       int cost;
00766       std::vector<int>::const_iterator state_id_iter;
00767       for(state_id_iter = ivStateArea.begin();
00768           state_id_iter != ivStateArea.end();
00769           ++state_id_iter)
00770       {
00771         s = ivStateId2State[*state_id_iter];
00772         cost = stepCost(*current, *s);
00773         SuccIDV->push_back(s->getId());
00774         CostV->push_back(cost);
00775       }
00776       return;
00777     }
00778   }
00779 
00780   ivExpandedStates.insert(std::pair<int,int>(current->getX(), current->getY()));
00781   ++ivNumExpandedStates;
00782 
00783   if (closeToGoal(*current))
00784   {
00785     int goal_id;
00786     assert(current->getLeg() != NOLEG);
00787     if (current->getLeg() == RIGHT)
00788       goal_id = ivIdGoalFootLeft;
00789     else
00790       goal_id = ivIdGoalFootRight;
00791 
00792     const PlanningState* goal = ivStateId2State[goal_id];
00793     SuccIDV->push_back(goal_id);
00794     CostV->push_back(stepCost(*current, *goal));
00795 
00796     return;
00797   }
00798 
00799   SuccIDV->reserve(ivFootstepSet.size());
00800   CostV->reserve(ivFootstepSet.size());
00801   std::vector<Footstep>::const_iterator footstep_set_iter;
00802   for(footstep_set_iter = ivFootstepSet.begin();
00803       footstep_set_iter != ivFootstepSet.end();
00804       ++footstep_set_iter)
00805   {
00806     PlanningState successor =
00807         footstep_set_iter->performMeOnThisState(*current);
00808     if (occupied(successor))
00809       continue;
00810 
00811     const PlanningState* successor_hash_entry =
00812         createHashEntryIfNotExists(successor);
00813 
00814     int cost = stepCost(*current, *successor_hash_entry);
00815     SuccIDV->push_back(successor_hash_entry->getId());
00816     CostV->push_back(cost);
00817   }
00818 }
00819 
00820 void
00821 FootstepPlannerEnvironment::GetSuccsTo(int SourceStateID, int goalStateId,
00822                                        std::vector<int> *SuccIDV,
00823                                        std::vector<int> *CostV)
00824 {
00825   //return GetSuccs(SourceStateID, SuccIDV, CostV);
00826 
00827   SuccIDV->clear();
00828   CostV->clear();
00829 
00830   assert(SourceStateID >= 0 &&
00831          unsigned(SourceStateID) < ivStateId2State.size());
00832 
00833   // make goal state absorbing
00834   if (SourceStateID == ivIdGoalFootLeft ){
00835     return;
00836   }
00837 
00838   const PlanningState* current = ivStateId2State[SourceStateID];
00839   ivExpandedStates.insert(std::pair<int,int>(current->getX(), current->getY()));
00840   ++ivNumExpandedStates;
00841 
00842   //ROS_INFO("GetSuccsTo %d -> %d: %f", SourceStateID, goalStateId, euclidean_distance(current->getX(), current->getY(), ivStateId2State[goalStateId]->getX(), ivStateId2State[goalStateId]->getY()));
00843 
00844   // add cheap transition from right to left, so right becomes an equivalent goal
00845   if (goalStateId== ivIdGoalFootLeft && SourceStateID == ivIdGoalFootRight && current->getLeg() == RIGHT){
00846     SuccIDV->push_back(ivIdGoalFootLeft);
00847     CostV->push_back(ivStepCost);
00848     return;
00849   }
00850 
00851   if (closeToGoal(*current))
00852   {
00853     int goal_id;
00854     assert(current->getLeg() != NOLEG);
00855     if (current->getLeg() == RIGHT){
00856       goal_id = ivIdGoalFootLeft;
00857     } else {
00858       goal_id = ivIdGoalFootRight;
00859     }
00860 
00861     const PlanningState* goal = ivStateId2State[goal_id];
00862     int cost = stepCost(*current, *goal);
00863     SuccIDV->push_back(goal_id);
00864     CostV->push_back(cost);
00865 
00866     return;
00867   }
00868 
00869   // intermediate goal reachable (R*)?
00870   assert(goalStateId >= 0 && unsigned(goalStateId) < ivStateId2State.size());
00871   const PlanningState* randomGoal = ivStateId2State[goalStateId];
00872   if (randomGoal->getLeg() != current->getLeg() && reachable(*current, *randomGoal)){
00873     int cost = stepCost(*current, *randomGoal);
00874     SuccIDV->push_back(goalStateId);
00875     CostV->push_back(cost);
00876     //                  ROS_INFO("%d %d", goalStateId, cost);
00877 
00878     //                  return;
00879   }
00880 
00881 
00882   SuccIDV->reserve(ivFootstepSet.size());
00883   CostV->reserve(ivFootstepSet.size());
00884   std::vector<Footstep>::const_iterator footstep_set_iter;
00885   for(footstep_set_iter = ivFootstepSet.begin();
00886       footstep_set_iter != ivFootstepSet.end();
00887       ++footstep_set_iter)
00888   {
00889     PlanningState successor =
00890         footstep_set_iter->performMeOnThisState(*current);
00891     if (occupied(successor))
00892       continue;
00893 
00894     const PlanningState* successor_hash = createHashEntryIfNotExists(successor);
00895 
00896     int cost = stepCost(*current, *successor_hash);
00897     SuccIDV->push_back(successor_hash->getId());
00898     CostV->push_back(cost);
00899   }
00900 }
00901 
00902 
00903 void
00904 FootstepPlannerEnvironment::GetRandomSuccsatDistance(int SourceStateID,
00905                                                      std::vector<int>* SuccIDV, std::vector<int>* CLowV)
00906 {
00907 
00908   assert(SourceStateID >= 0 && unsigned(SourceStateID) < ivStateId2State.size());
00909   //goal state should be absorbing
00910   if (SourceStateID == ivIdGoalFootLeft || SourceStateID == ivIdGoalFootRight )
00911     return;
00912 
00913 
00914   const PlanningState* currentState = ivStateId2State[SourceStateID];
00915   // TODO: closeToGoal?
00916   //
00917   //            if (closeToGoal(*currentState))
00918   //                    return;
00919 
00920   //get the successors
00921   GetRandomNeighs(currentState, SuccIDV, CLowV, ivNumRandomNodes,
00922                   ivRandomNodeDist, true);
00923 }
00924 
00925 void
00926 FootstepPlannerEnvironment::GetRandomPredsatDistance(int TargetStateID,
00927                                                      std::vector<int>* PredIDV, std::vector<int>* CLowV)
00928 {
00929 
00930   assert(TargetStateID >= 0 &&
00931                  unsigned(TargetStateID) < ivStateId2State.size());
00932 
00933   // start state should be absorbing
00934   if (TargetStateID == ivIdStartFootLeft || TargetStateID == ivIdStartFootRight)
00935     return;
00936 
00937   const PlanningState* currentState = ivStateId2State[TargetStateID];
00938 
00939   // TODO: ???
00940   //            if(closeToStart(*currentState))
00941   //                    return;
00942 
00943   //get the predecessors
00944   GetRandomNeighs(currentState, PredIDV, CLowV, ivNumRandomNodes,
00945                   ivRandomNodeDist, false);
00946 
00947 }
00948 
00949 //generates nNumofNeighs random neighbors of cell <X,Y> at distance nDist_c (measured in cells)
00950 //it will also generate goal if within this distance as an additional neighbor
00951 //bSuccs is set to true if we are computing successor states, otherwise it is Preds
00952 // (see fct. implemented in environment_nav2D)
00953 void FootstepPlannerEnvironment::GetRandomNeighs(const PlanningState* currentState, std::vector<int>* NeighIDV, std::vector<int>* CLowV, int nNumofNeighs, int nDist_c, bool bSuccs)
00954 {
00955 
00956   //clear the successor array
00957   NeighIDV->clear();
00958   CLowV->clear();
00959 
00960 
00961   //get X, Y for the states
00962   int X = currentState->getX();
00963   int Y = currentState->getY();
00964   //int theta = currentState->getTheta();
00965 
00966   //see if the goal/start belongs to the inside area and if yes then add it to Neighs as well
00967   // NOTE: "goal check" for backward planning
00968   const PlanningState* goal_left = NULL;
00969   const PlanningState* goal_right = NULL;
00970   if (bSuccs){
00971     goal_left = ivStateId2State[ivIdGoalFootLeft];
00972     goal_right = ivStateId2State[ivIdGoalFootRight];
00973   } else {
00974     goal_left = ivStateId2State[ivIdStartFootLeft];
00975     goal_right = ivStateId2State[ivIdStartFootRight];
00976   }
00977 
00978   int nDist_sq = nDist_c*nDist_c;
00979 
00980   //add left if within the distance
00981   if (euclidean_distance_sq(X, Y, goal_left->getX(), goal_left->getY()) <= nDist_sq)
00982   {
00983     //compute clow
00984     int clow;
00985     if(bSuccs)
00986       clow = GetFromToHeuristic(*currentState, *goal_left);
00987     else
00988       clow = GetFromToHeuristic(*goal_left, *currentState);
00989 
00990     NeighIDV->push_back(goal_left->getId());
00991     CLowV->push_back(clow);
00992     ivRandomStates.push_back(goal_left->getId());
00993   }
00994 
00995   //add right if within the distance
00996   if(euclidean_distance_sq(X, Y, goal_right->getX(), goal_right->getY()) <= nDist_sq)
00997   {
00998     //compute clow
00999     int clow;
01000     if(bSuccs)
01001       clow = GetFromToHeuristic(*currentState, *goal_right);
01002     else
01003       clow = GetFromToHeuristic(*goal_right, *currentState);
01004 
01005     NeighIDV->push_back(goal_right->getId());
01006     CLowV->push_back(clow);
01007     ivRandomStates.push_back(goal_right->getId());
01008   }
01009 
01010   //iterate through random actions
01011   int nAttempts = 0;
01012   for (int i = 0; i < nNumofNeighs && nAttempts < 5*nNumofNeighs; ++i, ++nAttempts)
01013   {
01014 
01015     // pick goal in random direction
01016     float fDir = (float)(TWO_PI*(((double)rand())/RAND_MAX));
01017 
01018     int dX = (int)(nDist_c*cos(fDir));
01019     int dY = (int)(nDist_c*sin(fDir));
01020 
01021     //get the coords of the state
01022     int newX = X + dX;
01023     int newY = Y + dY;
01024 
01025     // TODO / FIXME x,y, can be negative! need offset
01026     // check if outside of map:
01027     //                  if (newX < 0 || newY < 0 || unsigned(newX) >= ivMapPtr->getInfo().width || unsigned(newY) >= ivMapPtr->getInfo().height){
01028     //                          i--;
01029     //                          ROS_INFO("Outside of map: %d %d", newX, newY);
01030     //                          continue;
01031     //                  }
01032 
01033     // direction of random exploration (facing forward):
01034     int newTheta = angle_state_2_cell(fDir, ivNumAngleBins);
01035 
01036     // random left/right
01037     Leg newLeg = Leg(rand() % 2);
01038 
01039     PlanningState randomState(newX, newY, newTheta, newLeg, ivHashTableSize);
01040 
01041     // add both left and right if available:
01042     //                  int sep = disc_val(0.07, ivCellSize);
01043     //                  int ddX = int(-sin(fDir) * sep);
01044     //                  int ddY = int(cos(fDir) * sep);
01045     //                  PlanningState randomState(newX+ddX, newY+ddY, newTheta, LEFT, ivHashTableSize);
01046     //
01047     //                  PlanningState randomStateR(newX-ddX, newY-ddY, newTheta, RIGHT, ivHashTableSize);
01048 
01049     if(!occupied(randomState))
01050     {
01051       const PlanningState* random_hash_entry = getHashEntry(randomState);
01052       if (random_hash_entry == NULL){
01053         random_hash_entry = createNewHashEntry(randomState);
01054         ivRandomStates.push_back(random_hash_entry->getId());
01055       }
01056 
01057       //compute clow
01058       int clow;
01059       if(bSuccs)
01060         clow = GetFromToHeuristic(currentState->getId(), random_hash_entry->getId());
01061 
01062       else
01063         clow = GetFromToHeuristic(random_hash_entry->getId(), currentState->getId());
01064 
01065       NeighIDV->push_back(random_hash_entry->getId());
01066       CLowV->push_back(clow);
01067 
01068     }else{
01069       i--;
01070     }
01071 
01072     //                  if(!occupied(randomStateR))
01073     //                  {
01074     //                          const PlanningState* random_hash_entry = getHashEntry(randomStateR);
01075     //                          if (random_hash_entry == NULL){
01076     //                                  random_hash_entry = createNewHashEntry(randomStateR);
01077     //                                  ivRandomStates.push_back(random_hash_entry->getId());
01078     //                          }
01079     //
01080     //                          //compute clow
01081     //                          int clow;
01082     //                          if(bSuccs)
01083     //                                  clow = GetFromToHeuristic(currentState->getId(), random_hash_entry->getId());
01084     //                          else
01085     //                                  clow = GetFromToHeuristic(random_hash_entry->getId(), currentState->getId());
01086     //
01087     //                          NeighIDV->push_back(random_hash_entry->getId());
01088     //                          CLowV->push_back(clow);
01089     //
01090     //                  }else{
01091     //                          i--;
01092     //                  }
01093 
01094 
01095   }
01096 
01097   if (NeighIDV->size() == 0){
01098     ROS_WARN("Could not create any random neighbor nodes (%d attempts) from id %d (%d %d)",
01099              nAttempts, currentState->getId(), X, Y);
01100   } else
01101 
01102     ROS_DEBUG("Created %zu random neighbors (%d attempts) from id %d "
01103         "(%d %d)", NeighIDV->size(), nAttempts, currentState->getId(),
01104         X, Y);
01105 }
01106 
01107 bool
01108 FootstepPlannerEnvironment::AreEquivalent(int StateID1, int StateID2)
01109 {
01110   assert(StateID1 >= 0 && StateID2 >= 0
01111          && unsigned(StateID1) < ivStateId2State.size() && unsigned(StateID2) < ivStateId2State.size());
01112 
01113 
01114   if (StateID1 == StateID2)
01115     return true;
01116 
01117   const PlanningState* s1 = ivStateId2State[StateID1];
01118   const PlanningState* s2 = ivStateId2State[StateID2];
01119 
01120   //            // approximately compare, ignore theta:
01121   return (std::abs(s1->getX() - s2->getX()) < 1
01122       && std::abs(s1->getY() - s2->getY()) < 1
01123       //                                        && std::abs(s1->getTheta() - s2->getTheta()) < 3
01124       && s1->getLeg() == s2->getLeg()
01125   );
01126 
01127 
01128 //  compare the actual values (exact comparison)
01129 //  return (*s1 == *s2);
01130 }
01131 
01132 
01133 
01134 bool
01135 FootstepPlannerEnvironment::InitializeEnv(const char *sEnvFile)
01136 {
01137 //  ROS_ERROR("FootstepPlanerEnvironment::InitializeEnv: Hit unimplemented "
01138 //            "function. Check this!");
01139   return true;
01140 }
01141 
01142 
01143 bool
01144 FootstepPlannerEnvironment::InitializeMDPCfg(MDPConfig *MDPCfg)
01145 {
01146   // NOTE: The internal start and goal ids are set here to the left foot
01147   // (this affects the calculation of the heuristic values)
01148   MDPCfg->goalstateid = ivIdGoalFootLeft;
01149   MDPCfg->startstateid = ivIdStartFootLeft;
01150 
01151   assert(ivIdGoalFootLeft != -1);
01152   assert(ivIdStartFootLeft != -1);
01153 
01154   return true;
01155 }
01156 
01157 
01158 void
01159 FootstepPlannerEnvironment::PrintEnv_Config(FILE *fOut)
01160 {
01161   // NOTE: implement this if the planner needs to print out configurations
01162   ROS_ERROR("FootstepPlanerEnvironment::PrintEnv_Config: Hit "
01163       "unimplemented function. Check this!");
01164 }
01165 
01166 
01167 void
01168 FootstepPlannerEnvironment::PrintState(int stateID, bool bVerbose,
01169                                        FILE *fOut)
01170 {
01171   if(fOut == NULL)
01172   {
01173     fOut = stdout;
01174   }
01175 
01176   if(stateID == ivIdGoalFootLeft && bVerbose)
01177   {
01178     SBPL_FPRINTF(fOut, "the state is a goal state\n");
01179   }
01180 
01181   const PlanningState* s = ivStateId2State[stateID];
01182 
01183   if(bVerbose)
01184   {
01185     SBPL_FPRINTF(fOut, "X=%i Y=%i THETA=%i FOOT=%i\n",
01186                  s->getX(), s->getY(), s->getTheta(), s->getLeg());
01187   }
01188   else
01189   {
01190     SBPL_FPRINTF(fOut, "%i %i %i %i\n",
01191                  s->getX(), s->getY(), s->getTheta(), s->getLeg());
01192   }
01193 }
01194 
01195 
01196 void
01197 FootstepPlannerEnvironment::SetAllActionsandAllOutcomes(CMDPSTATE *state)
01198 {
01199   // NOTE: not implemented so far
01200   // Description: Some searches may also use SetAllActionsandAllOutcomes
01201   // or SetAllPreds functions if they keep the pointers to successors
01202   // (predecessors) but most searches do not require this, so it is not
01203   // necessary to support this
01204 
01205   ROS_ERROR("FootstepPlannerEnvironment::SetAllActionsandAllOutcomes: Hit"
01206       " unimplemented function. Check this!");
01207 }
01208 
01209 
01210 void
01211 FootstepPlannerEnvironment::SetAllPreds(CMDPSTATE *state)
01212 {
01213   // NOTE: not implemented so far
01214   // Description: Some searches may also use SetAllActionsandAllOutcomes
01215   // or SetAllPreds functions if they keep the pointers to successors
01216   // (predecessors) but most searches do not require this, so it is not
01217   // necessary to support this
01218 
01219   ROS_ERROR("FootstepPlannerEnvironment::SetAllPreds: Hit unimplemented "
01220       "function. Check this!");
01221 }
01222 
01223 
01224 int
01225 FootstepPlannerEnvironment::SizeofCreatedEnv()
01226 {
01227   return ivStateId2State.size();
01228 }
01229 
01230 
01231 void
01232 FootstepPlannerEnvironment::setStateArea(const PlanningState& left,
01233                                          const PlanningState& right)
01234 {
01235   ivStateArea.clear();
01236 
01237   const PlanningState* p_state = getHashEntry(right);
01238   ivStateArea.push_back(p_state->getId());
01239 
01240   double cont_step_x, cont_step_y, cont_step_theta;
01241   for (int step_y = ivMaxInvFootstepY; step_y <= ivMaxFootstepY; ++step_y)
01242   {
01243     for (int step_x = ivMaxInvFootstepX; step_x <= ivMaxFootstepX; ++step_x)
01244     {
01245       for (int step_theta = ivMaxInvFootstepTheta;
01246            step_theta <= ivMaxFootstepTheta;
01247            ++step_theta)
01248       {
01249         cont_step_x = cont_val(step_x, ivCellSize);
01250         cont_step_y = cont_val(step_y, ivCellSize);
01251         cont_step_theta = angle_cell_2_state(step_theta, ivNumAngleBins);
01252         Footstep step(cont_step_x, cont_step_y, cont_step_theta,
01253                       ivCellSize, ivNumAngleBins, ivHashTableSize);
01254         if (ivForwardSearch)
01255         {
01256           PlanningState pred = step.reverseMeOnThisState(left);
01257           if (occupied(pred) || !reachable(pred, left))
01258             continue;
01259           p_state = createHashEntryIfNotExists(pred);
01260           ivStateArea.push_back(p_state->getId());
01261 
01262           pred = step.reverseMeOnThisState(right);
01263           if (occupied(pred) || !reachable(pred, right))
01264             continue;
01265           p_state = createHashEntryIfNotExists(pred);
01266           ivStateArea.push_back(p_state->getId());
01267         }
01268         else
01269         {
01270           PlanningState succ = step.performMeOnThisState(left);
01271           if (occupied(succ) || !reachable(left, succ))
01272             continue;
01273           p_state = createHashEntryIfNotExists(succ);
01274           ivStateArea.push_back(p_state->getId());
01275 
01276           succ = step.performMeOnThisState(right);
01277           if (occupied(succ) || !reachable(right, succ))
01278             continue;
01279           p_state = createHashEntryIfNotExists(succ);
01280           ivStateArea.push_back(p_state->getId());
01281         }
01282       }
01283     }
01284   }
01285 }
01286 
01287 
01288 bool
01289 FootstepPlannerEnvironment::less::operator ()(const PlanningState* a,
01290                                               const PlanningState* b)
01291 const
01292 {
01293   if (a->getX() < b->getX())
01294     return true;
01295   else if (a->getY() < b->getY())
01296     return true;
01297   else
01298     return false;
01299 }
01300 }
01301 


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Sat Jun 8 2019 20:21:05