FootstepPlanner.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/FootstepPlanner.h>
00022 #include <humanoid_nav_msgs/ClipFootstep.h>
00023 
00024 
00025 using gridmap_2d::GridMap2D;
00026 using gridmap_2d::GridMap2DPtr;
00027 
00028 
00029 namespace footstep_planner
00030 {
00031 FootstepPlanner::FootstepPlanner()
00032 : ivStartPoseSetUp(false),
00033   ivGoalPoseSetUp(false),
00034   ivLastMarkerMsgSize(0),
00035   ivPathCost(0),
00036   ivMarkerNamespace("")
00037 {
00038   // private NodeHandle for parameters and private messages (debug / info)
00039   ros::NodeHandle nh_private("~");
00040   ros::NodeHandle nh_public;
00041 
00042   // ..publishers
00043   ivExpandedStatesVisPub = nh_private.advertise<
00044       sensor_msgs::PointCloud>("expanded_states", 1);
00045   ivRandomStatesVisPub = nh_private.advertise<
00046       sensor_msgs::PointCloud>("random_states", 1);
00047   ivFootstepPathVisPub = nh_private.advertise<
00048       visualization_msgs::MarkerArray>("footsteps_array", 1);
00049   ivHeuristicPathVisPub = nh_private.advertise<
00050       nav_msgs::Path>("heuristic_path", 1);
00051   ivPathVisPub = nh_private.advertise<nav_msgs::Path>("path", 1);
00052   ivStartPoseVisPub = nh_private.advertise<
00053       geometry_msgs::PoseStamped>("start", 1);
00054 
00055   std::string heuristic_type;
00056   double diff_angle_cost;
00057 
00058   // read parameters from config file:
00059   // planner environment settings
00060   nh_private.param("heuristic_type", heuristic_type,
00061                    std::string("EuclideanHeuristic"));
00062   nh_private.param("heuristic_scale", ivEnvironmentParams.heuristic_scale, 1.0);
00063   nh_private.param("max_hash_size", ivEnvironmentParams.hash_table_size, 65536);
00064   nh_private.param("accuracy/collision_check",
00065                    ivEnvironmentParams.collision_check_accuracy,
00066                    2);
00067   nh_private.param("accuracy/cell_size", ivEnvironmentParams.cell_size, 0.01);
00068   nh_private.param("accuracy/num_angle_bins",
00069                    ivEnvironmentParams.num_angle_bins,
00070                    64);
00071   nh_private.param("step_cost", ivEnvironmentParams.step_cost, 0.05);
00072   nh_private.param("diff_angle_cost", diff_angle_cost, 0.0);
00073 
00074   nh_private.param("planner_type", ivPlannerType, std::string("ARAPlanner"));
00075   nh_private.param("search_until_first_solution", ivSearchUntilFirstSolution,
00076                    false);
00077   nh_private.param("allocated_time", ivMaxSearchTime, 7.0);
00078   nh_private.param("forward_search", ivEnvironmentParams.forward_search, false);
00079   nh_private.param("initial_epsilon", ivInitialEpsilon, 3.0);
00080   nh_private.param("changed_cells_limit", ivChangedCellsLimit, 20000);
00081   nh_private.param("num_random_nodes", ivEnvironmentParams.num_random_nodes,
00082                    20);
00083   nh_private.param("random_node_dist", ivEnvironmentParams.random_node_distance,
00084                    1.0);
00085 
00086   // footstep settings
00087   nh_private.param("foot/size/x", ivEnvironmentParams.footsize_x, 0.16);
00088   nh_private.param("foot/size/y", ivEnvironmentParams.footsize_y, 0.06);
00089   nh_private.param("foot/size/z", ivEnvironmentParams.footsize_z, 0.015);
00090   nh_private.param("foot/separation", ivFootSeparation, 0.1);
00091   nh_private.param("foot/origin_shift/x",
00092                    ivEnvironmentParams.foot_origin_shift_x,
00093                    0.02);
00094   nh_private.param("foot/origin_shift/y",
00095                    ivEnvironmentParams.foot_origin_shift_y,
00096                    0.0);
00097   nh_private.param("foot/max/step/x", ivEnvironmentParams.max_footstep_x, 0.08);
00098   nh_private.param("foot/max/step/y", ivEnvironmentParams.max_footstep_y, 0.16);
00099   nh_private.param("foot/max/step/theta",
00100                    ivEnvironmentParams.max_footstep_theta,
00101                    0.3);
00102   nh_private.param("foot/max/inverse/step/x",
00103                    ivEnvironmentParams.max_inverse_footstep_x,
00104                    -0.04);
00105   nh_private.param("foot/max/inverse/step/y",
00106                    ivEnvironmentParams.max_inverse_footstep_y,
00107                    0.09);
00108   nh_private.param("foot/max/inverse/step/theta",
00109                    ivEnvironmentParams.max_inverse_footstep_theta,
00110                    -0.3);
00111 
00112   // footstep discretization
00113   XmlRpc::XmlRpcValue footsteps_x;
00114   XmlRpc::XmlRpcValue footsteps_y;
00115   XmlRpc::XmlRpcValue footsteps_theta;
00116   nh_private.getParam("footsteps/x", footsteps_x);
00117   nh_private.getParam("footsteps/y", footsteps_y);
00118   nh_private.getParam("footsteps/theta", footsteps_theta);
00119   if (footsteps_x.getType() != XmlRpc::XmlRpcValue::TypeArray)
00120     ROS_ERROR("Error reading footsteps/x from config file.");
00121   if (footsteps_y.getType() != XmlRpc::XmlRpcValue::TypeArray)
00122     ROS_ERROR("Error reading footsteps/y from config file.");
00123   if (footsteps_theta.getType() != XmlRpc::XmlRpcValue::TypeArray)
00124     ROS_ERROR("Error reading footsteps/theta from config file.");
00125   int size_x = footsteps_x.size();
00126   int size_y = footsteps_y.size();
00127   int size_t = footsteps_theta.size();
00128   if (size_x != size_y || size_x != size_t)
00129   {
00130     ROS_ERROR("Footstep parameterization has different sizes for x/y/theta. "
00131               "Exit!");
00132     exit(2);
00133   }
00134   // create footstep set
00135   ivEnvironmentParams.footstep_set.clear();
00136   double max_step_width = 0;
00137   for(int i=0; i < footsteps_x.size(); ++i)
00138   {
00139     double x = (double)footsteps_x[i];
00140     double y = (double)footsteps_y[i];
00141     double theta = (double)footsteps_theta[i];
00142 
00143     Footstep f(x, y, theta,
00144                ivEnvironmentParams.cell_size,
00145                ivEnvironmentParams.num_angle_bins,
00146                ivEnvironmentParams.hash_table_size);
00147     ivEnvironmentParams.footstep_set.push_back(f);
00148 
00149     double cur_step_width = sqrt(x*x + y*y);
00150 
00151     if (cur_step_width > max_step_width)
00152       max_step_width = cur_step_width;
00153   }
00154 
00155   // step range
00156   XmlRpc::XmlRpcValue step_range_x;
00157   XmlRpc::XmlRpcValue step_range_y;
00158   nh_private.getParam("step_range/x", step_range_x);
00159   nh_private.getParam("step_range/y", step_range_y);
00160   if (step_range_x.getType() != XmlRpc::XmlRpcValue::TypeArray)
00161     ROS_ERROR("Error reading footsteps/x from config file.");
00162   if (step_range_y.getType() != XmlRpc::XmlRpcValue::TypeArray)
00163     ROS_ERROR("Error reading footsteps/y from config file.");
00164   if (step_range_x.size() != step_range_y.size())
00165   {
00166     ROS_ERROR("Step range points have different size. Exit!");
00167     exit(2);
00168   }
00169   // create step range
00170   ivEnvironmentParams.step_range.clear();
00171   ivEnvironmentParams.step_range.reserve(step_range_x.size());
00172   double x, y;
00173   double max_x = 0.0;
00174   double max_y = 0.0;
00175   double cell_size = ivEnvironmentParams.cell_size;
00176   for (int i=0; i < step_range_x.size(); ++i)
00177   {
00178     x = (double)step_range_x[i];
00179     y = (double)step_range_y[i];
00180     if (fabs(x) > max_x)
00181       max_x = fabs(x);
00182     if (fabs(y) > max_y)
00183       max_y = fabs(y);
00184     ivEnvironmentParams.step_range.push_back(
00185       std::pair<int, int>(disc_val(x, cell_size), disc_val(y, cell_size)));
00186   }
00187   // insert first point again at the end!
00188   ivEnvironmentParams.step_range.push_back(ivEnvironmentParams.step_range[0]);
00189   ivEnvironmentParams.max_step_width = sqrt(max_x*max_x + max_y*max_y) * 1.5;
00190 
00191   // initialize the heuristic
00192   boost::shared_ptr<Heuristic> h;
00193   if (heuristic_type == "EuclideanHeuristic")
00194   {
00195     h.reset(
00196         new EuclideanHeuristic(ivEnvironmentParams.cell_size,
00197                                ivEnvironmentParams.num_angle_bins));
00198     ROS_INFO("FootstepPlanner heuristic: euclidean distance");
00199   }
00200   else if(heuristic_type == "EuclStepCostHeuristic")
00201   {
00202     h.reset(
00203         new EuclStepCostHeuristic(ivEnvironmentParams.cell_size,
00204                                   ivEnvironmentParams.num_angle_bins,
00205                                   ivEnvironmentParams.step_cost,
00206                                   diff_angle_cost,
00207                                   max_step_width));
00208     ROS_INFO("FootstepPlanner heuristic: euclidean distance with step costs");
00209   }
00210   else if (heuristic_type == "PathCostHeuristic")
00211   {
00212     // for heuristic inflation
00213     double foot_incircle =
00214       std::min((ivEnvironmentParams.footsize_x / 2.0 -
00215                 std::abs(ivEnvironmentParams.foot_origin_shift_x)),
00216                (ivEnvironmentParams.footsize_y / 2.0 -
00217                 std::abs(ivEnvironmentParams.foot_origin_shift_y)));
00218     assert(foot_incircle > 0.0);
00219 
00220     h.reset(
00221         new PathCostHeuristic(ivEnvironmentParams.cell_size,
00222                               ivEnvironmentParams.num_angle_bins,
00223                               ivEnvironmentParams.step_cost,
00224                               diff_angle_cost,
00225                               max_step_width,
00226                               foot_incircle));
00227     ROS_INFO("FootstepPlanner heuristic: 2D path euclidean distance with step "
00228              "costs");
00229 
00230     // keep a local ptr for visualization
00231     ivPathCostHeuristicPtr = boost::dynamic_pointer_cast<PathCostHeuristic>(h);
00232   }
00233   else
00234   {
00235     ROS_ERROR_STREAM("Heuristic " << heuristic_type << " not available, "
00236                      "exiting.");
00237     exit(1);
00238   }
00239   ivEnvironmentParams.heuristic = h;
00240 
00241   // initialize the planner environment
00242   ivPlannerEnvironmentPtr.reset(
00243     new FootstepPlannerEnvironment(ivEnvironmentParams));
00244 
00245   // set up planner
00246   if (ivPlannerType == "ARAPlanner" ||
00247       ivPlannerType == "ADPlanner"  ||
00248       ivPlannerType == "RSTARPlanner" )
00249   {
00250     ROS_INFO_STREAM("Planning with " << ivPlannerType);
00251   }
00252   else
00253   {
00254     ROS_ERROR_STREAM("Planner "<< ivPlannerType <<" not available / "
00255                      "untested.");
00256     exit(1);
00257   }
00258   if (ivEnvironmentParams.forward_search)
00259   {
00260     ROS_INFO_STREAM("Search direction: forward planning");
00261   }
00262   else
00263   {
00264     ROS_INFO_STREAM("Search direction: backward planning");
00265   }
00266   setPlanner();
00267 }
00268 
00269 
00270 FootstepPlanner::~FootstepPlanner()
00271 {}
00272 
00273 
00274 void
00275 FootstepPlanner::setPlanner()
00276 {
00277   if (ivPlannerType == "ARAPlanner")
00278   {
00279     ivPlannerPtr.reset(
00280         new ARAPlanner(ivPlannerEnvironmentPtr.get(),
00281                        ivEnvironmentParams.forward_search));
00282   }
00283   else if (ivPlannerType == "ADPlanner")
00284   {
00285     ivPlannerPtr.reset(
00286         new ADPlanner(ivPlannerEnvironmentPtr.get(),
00287                       ivEnvironmentParams.forward_search));
00288   }
00289   else if (ivPlannerType == "RSTARPlanner")
00290   {
00291     RSTARPlanner* p =
00292         new RSTARPlanner(ivPlannerEnvironmentPtr.get(),
00293                          ivEnvironmentParams.forward_search);
00294     // new options, require patched SBPL
00295     //          p->set_local_expand_thres(500);
00296     //          p->set_eps_step(1.0);
00297     ivPlannerPtr.reset(p);
00298   }
00299   //        else if (ivPlannerType == "ANAPlanner")
00300   //            ivPlannerPtr.reset(new anaPlanner(ivPlannerEnvironmentPtr.get(),
00301   //                                              ivForwardSearch));
00302 }
00303 
00304 
00305 bool
00306 FootstepPlanner::run()
00307 {
00308   bool path_existed = (bool)ivPath.size();
00309   int ret = 0;
00310   MDPConfig mdp_config;
00311   std::vector<int> solution_state_ids;
00312 
00313   // commit start/goal poses to the environment
00314   ivPlannerEnvironmentPtr->updateStart(ivStartFootLeft, ivStartFootRight);
00315   ivPlannerEnvironmentPtr->updateGoal(ivGoalFootLeft, ivGoalFootRight);
00316   ivPlannerEnvironmentPtr->updateHeuristicValues();
00317   ivPlannerEnvironmentPtr->InitializeEnv(NULL);
00318   ivPlannerEnvironmentPtr->InitializeMDPCfg(&mdp_config);
00319 
00320   // inform AD planner about changed (start) states for replanning
00321   if (path_existed &&
00322       !ivEnvironmentParams.forward_search &&
00323       ivPlannerType == "ADPlanner")
00324   {
00325     std::vector<int> changed_edges;
00326     changed_edges.push_back(mdp_config.startstateid);
00327     // update the AD planner
00328     boost::shared_ptr<ADPlanner> ad_planner =
00329       boost::dynamic_pointer_cast<ADPlanner>(ivPlannerPtr);
00330     ad_planner->update_preds_of_changededges(&changed_edges);
00331   }
00332 
00333   // set up SBPL
00334   if (ivPlannerPtr->set_start(mdp_config.startstateid) == 0)
00335   {
00336     ROS_ERROR("Failed to set start state.");
00337     return false;
00338   }
00339   if (ivPlannerPtr->set_goal(mdp_config.goalstateid) == 0)
00340   {
00341     ROS_ERROR("Failed to set goal state\n");
00342     return false;
00343   }
00344 
00345   ivPlannerPtr->set_initialsolution_eps(ivInitialEpsilon);
00346   ivPlannerPtr->set_search_mode(ivSearchUntilFirstSolution);
00347 
00348   ROS_INFO("Start planning (max time: %f, initial eps: %f (%f))\n",
00349            ivMaxSearchTime, ivInitialEpsilon,
00350            ivPlannerPtr->get_initial_eps());
00351   int path_cost;
00352   ros::WallTime startTime = ros::WallTime::now();
00353   try
00354   {
00355     ret = ivPlannerPtr->replan(ivMaxSearchTime, &solution_state_ids,
00356                                &path_cost);
00357   }
00358   catch (const SBPL_Exception& e)
00359   {
00360     // ROS_ERROR("SBPL planning failed (%s)", e.what());
00361     return false;
00362   }
00363   ivPathCost = double(path_cost) / FootstepPlannerEnvironment::cvMmScale;
00364 
00365   bool path_is_new = pathIsNew(solution_state_ids);
00366   if (ret && solution_state_ids.size() > 0)
00367   {
00368     if (!path_is_new)
00369       ROS_WARN("Solution found by SBPL is the same as the old solution. This could indicate that replanning failed.");
00370 
00371     ROS_INFO("Solution of size %zu found after %f s",
00372              solution_state_ids.size(),
00373              (ros::WallTime::now()-startTime).toSec());
00374 
00375     if (extractPath(solution_state_ids))
00376     {
00377       ROS_INFO("Expanded states: %i total / %i new",
00378                ivPlannerEnvironmentPtr->getNumExpandedStates(),
00379                ivPlannerPtr->get_n_expands());
00380       ROS_INFO("Final eps: %f", ivPlannerPtr->get_final_epsilon());
00381       ROS_INFO("Path cost: %f (%i)\n", ivPathCost, path_cost);
00382 
00383       ivPlanningStatesIds = solution_state_ids;
00384 
00385       broadcastExpandedNodesVis();
00386       broadcastRandomNodesVis();
00387       broadcastFootstepPathVis();
00388       broadcastPathVis();
00389 
00390       return true;
00391     }
00392     else
00393     {
00394       ROS_ERROR("extracting path failed\n\n");
00395       return false;
00396     }
00397   }
00398   else
00399   {
00400     broadcastExpandedNodesVis();
00401     broadcastRandomNodesVis();
00402 
00403     ROS_ERROR("No solution found");
00404     return false;
00405   }
00406 }
00407 
00408 
00409 bool
00410 FootstepPlanner::extractPath(const std::vector<int>& state_ids)
00411 {
00412   ivPath.clear();
00413 
00414   State s;
00415   State start_left;
00416   std::vector<int>::const_iterator state_ids_iter = state_ids.begin();
00417 
00418   // first state is always the robot's left foot
00419   if (!ivPlannerEnvironmentPtr->getState(*state_ids_iter, &start_left))
00420   {
00421     ivPath.clear();
00422     return false;
00423   }
00424   ++state_ids_iter;
00425   if (!ivPlannerEnvironmentPtr->getState(*state_ids_iter, &s))
00426   {
00427     ivPath.clear();
00428     return false;
00429   }
00430   ++state_ids_iter;
00431 
00432   // check if the robot's left foot can be ommited as first state in the path,
00433   // i.e. the robot's right foot is appended first to the path
00434   if (s.getLeg() == LEFT)
00435     ivPath.push_back(ivStartFootRight);
00436   else
00437     ivPath.push_back(start_left);
00438   ivPath.push_back(s);
00439 
00440   for(; state_ids_iter < state_ids.end(); ++state_ids_iter)
00441   {
00442     if (!ivPlannerEnvironmentPtr->getState(*state_ids_iter, &s))
00443     {
00444       ivPath.clear();
00445       return false;
00446     }
00447     ivPath.push_back(s);
00448   }
00449 
00450   // add last neutral step
00451   if (ivPath.back().getLeg() == RIGHT)
00452     ivPath.push_back(ivGoalFootLeft);
00453   else // last_leg == LEFT
00454     ivPath.push_back(ivGoalFootRight);
00455 
00456   return true;
00457 }
00458 
00459 
00460 void
00461 FootstepPlanner::reset()
00462 {
00463   ROS_INFO("Resetting planner");
00464   // reset the previously calculated paths
00465   ivPath.clear();
00466   ivPlanningStatesIds.clear();
00467   // reset the planner
00468   // INFO: force_planning_from_scratch was not working properly the last time
00469   // checked; therefore instead of using this function the planner is manually
00470   // reset
00471   //ivPlannerPtr->force_planning_from_scratch();
00472   ivPlannerEnvironmentPtr->reset();
00473   setPlanner();
00474 }
00475 
00476 
00477 void
00478 FootstepPlanner::resetTotally()
00479 {
00480   ROS_INFO("Resetting planner and environment");
00481   // reset the previously calculated paths
00482   ivPath.clear();
00483   ivPlanningStatesIds.clear();
00484   // reinitialize the planner environment
00485   ivPlannerEnvironmentPtr.reset(
00486       new FootstepPlannerEnvironment(ivEnvironmentParams));
00487   setPlanner();
00488 }
00489 
00490 
00491 bool
00492 FootstepPlanner::plan(bool force_new_plan)
00493 {
00494   if (!ivMapPtr)
00495   {
00496     ROS_ERROR("FootstepPlanner has no map for planning yet.");
00497     return false;
00498   }
00499   if (!ivGoalPoseSetUp || !ivStartPoseSetUp)
00500   {
00501     ROS_ERROR("FootstepPlanner has not set the start and/or goal pose "
00502               "yet.");
00503     return false;
00504   }
00505 
00506   if (force_new_plan
00507       || ivPlannerType == "RSTARPlanner" || ivPlannerType == "ARAPlanner" )
00508   {
00509     reset();
00510   }
00511   // start the planning and return success
00512   return run();
00513 }
00514 
00515 
00516 bool
00517 FootstepPlanner::replan()
00518 {
00519   return plan(false);
00520 }
00521 
00522 
00523 bool
00524 FootstepPlanner::plan(const geometry_msgs::PoseStampedConstPtr start,
00525                       const geometry_msgs::PoseStampedConstPtr goal)
00526 {
00527   return plan(start->pose.position.x, start->pose.position.y,
00528               tf::getYaw(start->pose.orientation),
00529               goal->pose.position.x, goal->pose.position.y,
00530               tf::getYaw(goal->pose.orientation));
00531 }
00532 
00533 
00534 bool
00535 FootstepPlanner::plan(float start_x, float start_y, float start_theta,
00536                       float goal_x, float goal_y, float goal_theta)
00537 {
00538   if (!(setStart(start_x, start_y, start_theta) &&
00539       setGoal(goal_x, goal_y, goal_theta)))
00540   {
00541     return false;
00542   }
00543 
00544   return plan(false);
00545 }
00546 
00547 
00548 bool
00549 FootstepPlanner::planService(humanoid_nav_msgs::PlanFootsteps::Request &req,
00550                              humanoid_nav_msgs::PlanFootsteps::Response &resp)
00551 {
00552   bool result = plan(req.start.x, req.start.y, req.start.theta,
00553                      req.goal.x, req.goal.y, req.goal.theta);
00554 
00555   resp.costs = getPathCosts();
00556   resp.footsteps.reserve(getPathSize());
00557   resp.final_eps = ivPlannerPtr->get_final_epsilon();
00558   resp.expanded_states = ivPlannerEnvironmentPtr->getNumExpandedStates();
00559   extractFootstepsSrv(resp.footsteps);
00560 
00561   resp.result = result;
00562 
00563   // return true since service call was successful (independent from the
00564   // success of the planning call)
00565   return true;
00566 }
00567 
00568 
00569 bool
00570 FootstepPlanner::planFeetService(humanoid_nav_msgs::PlanFootstepsBetweenFeet::Request &req,
00571                              humanoid_nav_msgs::PlanFootstepsBetweenFeet::Response &resp)
00572 {
00573   // TODO check direction and change of states, force planning from scratch if does not fit
00574   setStart(State(req.start_left.pose.x, req.start_left.pose.y, req.start_left.pose.theta, LEFT),
00575            State(req.start_right.pose.x, req.start_right.pose.y, req.start_right.pose.theta, RIGHT));
00576   setGoal(State(req.goal_left.pose.x, req.goal_left.pose.y, req.goal_left.pose.theta, LEFT),
00577            State(req.goal_right.pose.x, req.goal_right.pose.y, req.goal_right.pose.theta, RIGHT));
00578 
00579   bool result = plan(false);
00580 
00581   resp.costs = getPathCosts();
00582   resp.footsteps.reserve(getPathSize());
00583   resp.final_eps = ivPlannerPtr->get_final_epsilon();
00584   resp.expanded_states = ivPlannerEnvironmentPtr->getNumExpandedStates();
00585   extractFootstepsSrv(resp.footsteps);
00586 
00587   resp.result = result;
00588 
00589   // return true since service call was successful (independent from the
00590   // success of the planning call)
00591   return true;
00592 }
00593 
00594 void
00595 FootstepPlanner::extractFootstepsSrv(std::vector<humanoid_nav_msgs::StepTarget> & footsteps) const{
00596   humanoid_nav_msgs::StepTarget foot;
00597   state_iter_t path_iter;
00598   for (path_iter = getPathBegin(); path_iter != getPathEnd(); ++path_iter)
00599   {
00600     foot.pose.x = path_iter->getX();
00601     foot.pose.y = path_iter->getY();
00602     foot.pose.theta = path_iter->getTheta();
00603     if (path_iter->getLeg() == LEFT)
00604       foot.leg = humanoid_nav_msgs::StepTarget::left;
00605     else if (path_iter->getLeg() == RIGHT)
00606       foot.leg = humanoid_nav_msgs::StepTarget::right;
00607     else
00608     {
00609       ROS_ERROR("Footstep pose at (%f, %f, %f) is set to NOLEG!",
00610                 path_iter->getX(), path_iter->getY(),
00611                 path_iter->getTheta());
00612       continue;
00613     }
00614 
00615     footsteps.push_back(foot);
00616   }
00617 
00618 }
00619 
00620 
00621 void
00622 FootstepPlanner::goalPoseCallback(
00623     const geometry_msgs::PoseStampedConstPtr& goal_pose)
00624 {
00625   // update the goal states in the environment
00626   if (setGoal(goal_pose))
00627   {
00628     if (ivStartPoseSetUp)
00629     {
00630       // force planning from scratch when backwards direction
00631       plan(!ivEnvironmentParams.forward_search);
00632     }
00633   }
00634 }
00635 
00636 
00637 void
00638 FootstepPlanner::startPoseCallback(
00639     const geometry_msgs::PoseWithCovarianceStampedConstPtr& start_pose)
00640 {
00641   if (setStart(start_pose->pose.pose.position.x,
00642                start_pose->pose.pose.position.y,
00643                tf::getYaw(start_pose->pose.pose.orientation)))
00644   {
00645     if (ivGoalPoseSetUp)
00646     {
00647       // force planning from scratch when forward direction
00648       plan(ivEnvironmentParams.forward_search);
00649     }
00650   }
00651 }
00652 
00653 
00654 void
00655 FootstepPlanner::mapCallback(
00656     const nav_msgs::OccupancyGridConstPtr& occupancy_map)
00657 {
00658   GridMap2DPtr map(new GridMap2D(occupancy_map));
00659 
00660   // new map: update the map information
00661   if (updateMap(map))
00662   {
00663     // NOTE: update map currently simply resets the planner, i.e. replanning
00664     // here is in fact a planning from the scratch
00665     plan(false);
00666   }
00667 }
00668 
00669 
00670 bool
00671 FootstepPlanner::setGoal(const geometry_msgs::PoseStampedConstPtr goal_pose)
00672 {
00673   return setGoal(goal_pose->pose.position.x,
00674                  goal_pose->pose.position.y,
00675                  tf::getYaw(goal_pose->pose.orientation));
00676 }
00677 
00678 
00679 bool
00680 FootstepPlanner::setGoal(float x, float y, float theta)
00681 {
00682   if (!ivMapPtr)
00683   {
00684     ROS_ERROR("Distance map hasn't been initialized yet.");
00685     return false;
00686   }
00687 
00688   State goal(x, y, theta, NOLEG);
00689   State foot_left = getFootPose(goal, LEFT);
00690   State foot_right = getFootPose(goal, RIGHT);
00691 
00692   if (ivPlannerEnvironmentPtr->occupied(foot_left) ||
00693       ivPlannerEnvironmentPtr->occupied(foot_right))
00694   {
00695     ROS_ERROR("Goal pose at (%f %f %f) not accessible.", x, y, theta);
00696     ivGoalPoseSetUp = false;
00697     return false;
00698   }
00699   ivGoalFootLeft = foot_left;
00700   ivGoalFootRight = foot_right;
00701 
00702   ivGoalPoseSetUp = true;
00703   ROS_INFO("Goal pose set to (%f %f %f)", x, y, theta);
00704 
00705   return true;
00706 }
00707 
00708 bool
00709 FootstepPlanner::setGoal(const State& left_foot, const State& right_foot)
00710 {
00711   if (ivPlannerEnvironmentPtr->occupied(left_foot) ||
00712       ivPlannerEnvironmentPtr->occupied(right_foot))
00713   {
00714     ivGoalPoseSetUp = false;
00715     return false;
00716   }
00717   ivGoalFootLeft = left_foot;
00718   ivGoalFootRight = right_foot;
00719 
00720   ivGoalPoseSetUp = true;
00721 
00722   return true;
00723 }
00724 
00725 
00726 bool
00727 FootstepPlanner::setStart(const geometry_msgs::PoseStampedConstPtr start_pose)
00728 {
00729   return setStart(start_pose->pose.position.x,
00730                   start_pose->pose.position.y,
00731                   tf::getYaw(start_pose->pose.orientation));
00732 }
00733 
00734 
00735 bool
00736 FootstepPlanner::setStart(const State& left_foot, const State& right_foot)
00737 {
00738   if (ivPlannerEnvironmentPtr->occupied(left_foot) ||
00739       ivPlannerEnvironmentPtr->occupied(right_foot))
00740   {
00741     ivStartPoseSetUp = false;
00742     return false;
00743   }
00744   ivStartFootLeft = left_foot;
00745   ivStartFootRight = right_foot;
00746 
00747   ivStartPoseSetUp = true;
00748 
00749   return true;
00750 }
00751 
00752 
00753 bool
00754 FootstepPlanner::setStart(float x, float y, float theta)
00755 {
00756   if (!ivMapPtr)
00757   {
00758     ROS_ERROR("Distance map hasn't been initialized yet.");
00759     return false;
00760   }
00761 
00762   State start(x, y, theta, NOLEG);
00763   State foot_left = getFootPose(start, LEFT);
00764   State foot_right = getFootPose(start, RIGHT);
00765 
00766   bool success = setStart(foot_left, foot_right);
00767   if (success)
00768     ROS_INFO("Start pose set to (%f %f %f)", x, y, theta);
00769   else
00770     ROS_ERROR("Start pose (%f %f %f) not accessible.", x, y, theta);
00771 
00772   // publish visualization:
00773   geometry_msgs::PoseStamped start_pose;
00774   start_pose.pose.position.x = x;
00775   start_pose.pose.position.y = y;
00776   start_pose.pose.position.z = 0.025;
00777   start_pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
00778   start_pose.header.frame_id = ivMapPtr->getFrameID();
00779   start_pose.header.stamp = ros::Time::now();
00780   ivStartPoseVisPub.publish(start_pose);
00781 
00782   return success;
00783 }
00784 
00785 
00786 bool
00787 FootstepPlanner::updateMap(const GridMap2DPtr map)
00788 {
00789   // store old map pointer locally
00790   GridMap2DPtr old_map = ivMapPtr;
00791   // store new map
00792   ivMapPtr.reset();
00793   ivMapPtr = map;
00794 
00795   // check if a previous map and a path existed
00796   if (old_map && (bool)ivPath.size())
00797   {
00798     updateEnvironment(old_map);
00799     return true;
00800   }
00801 
00802   // ..otherwise the environment's map can simply be updated
00803   ivPlannerEnvironmentPtr->updateMap(map);
00804   return false;
00805 }
00806 
00807 
00808 void
00809 FootstepPlanner::updateEnvironment(const GridMap2DPtr old_map)
00810 {
00811   ROS_INFO("Reseting the planning environment.");
00812   // reset environment
00813   resetTotally();
00814   // set the new map
00815   ivPlannerEnvironmentPtr->updateMap(ivMapPtr);
00816 
00817 
00818   // The following is not used any more
00819 
00820   // Replanning based on old planning info currently disabled
00821   //        // TODO: handle size changes of the map; currently the planning
00822   //        // information is reseted
00823   //
00824   //        if (ivPlannerType == "ADPlanner" &&
00825   //            ivMapPtr->getResolution() == old_map->getResolution() &&
00826   //            ivMapPtr->size().height == old_map->size().height &&
00827   //            ivMapPtr->size().width == old_map->size().width)
00828   //        {
00829   //            ROS_INFO("Received an updated map => change detection");
00830   //
00831   //            std::vector<State2> changed_states;
00832   //            cv::Mat changed_cells;
00833   //
00834   //            // get new occupied cells only (0: occupied in binary map)
00835   //            // changedCells(x,y) = old(x,y) AND NOT(new(x,y))
00838   //
00839   //            // to get all changed cells (new free and occupied) use XOR:
00840   //            cv::bitwise_xor(old_map->binaryMap(), ivMapPtr->binaryMap(),
00841   //                            changed_cells);
00842   //
00843   //            //inflate by outer foot radius:
00844   //            cv::bitwise_not(changed_cells, changed_cells); // invert for distanceTransform
00845   //            cv::Mat changedDistMap = cv::Mat(changed_cells.size(), CV_32FC1);
00846   //            cv::distanceTransform(changed_cells, changedDistMap,
00847   //                                  CV_DIST_L2, CV_DIST_MASK_PRECISE);
00848   //            double max_foot_radius = sqrt(
00849   //                    pow(std::abs(ivOriginFootShiftX) + ivFootsizeX / 2.0, 2.0) +
00850   //                    pow(std::abs(ivOriginFootShiftY) + ivFootsizeY / 2.0, 2.0))
00851   //                    / ivMapPtr->getResolution();
00852   //            changed_cells = (changedDistMap <= max_foot_radius); // threshold, also invert back
00853   //
00854   //            // loop over changed cells (now marked with 255 in the mask):
00855   //            unsigned int num_changed_cells = 0;
00856   //            double wx, wy;
00857   //            State2 s;
00858   //            for (int y = 0; y < changed_cells.rows; ++y)
00859   //            {
00860   //                for (int x = 0; x < changed_cells.cols; ++x)
00861   //                {
00862   //                    if (changed_cells.at<uchar>(x,y) == 255)
00863   //                    {
00864   //                        ++num_changed_cells;
00865   //                        ivMapPtr->mapToWorld(x, y, wx, wy);
00866   //                        s.setX(wx);
00867   //                        s.setY(wy);
00868   //                        // on each grid cell ivNumAngleBins-many planning states
00869   //                        // can be placed (if the resolution of the grid cells is
00870   //                        // the same as of the planning state grid)
00871   //                        for (int theta = 0; theta < ivNumAngleBins; ++theta)
00872   //                        {
00873   //                            s.setTheta(angle_cell_2_state(theta, ivNumAngleBins));
00874   //                            changed_states.push_back(s);
00875   //                        }
00876   //                    }
00877   //                }
00878   //            }
00879   //
00880   //            if (num_changed_cells == 0)
00881   //            {
00882   //                ROS_INFO("old map equals new map; no replanning necessary");
00883   //                return;
00884   //            }
00885   //
00886   //            ROS_INFO("%d changed map cells found", num_changed_cells);
00887   //            if (num_changed_cells <= ivChangedCellsLimit)
00888   //            {
00889   //                // update planer
00890   //                ROS_INFO("Use old information in new planning taks");
00891   //
00892   //                std::vector<int> neighbour_ids;
00893   //                if (ivForwardSearch)
00894   //                    ivPlannerEnvironmentPtr->getSuccsOfGridCells(
00895   //                            changed_states, &neighbour_ids);
00896   //                else
00897   //                    ivPlannerEnvironmentPtr->getPredsOfGridCells(
00898   //                            changed_states, &neighbour_ids);
00899   //
00900   //                boost::shared_ptr<ADPlanner> h =
00901   //                        boost::dynamic_pointer_cast<ADPlanner>(ivPlannerPtr);
00902   //                h->costs_changed(PlanningStateChangeQuery(neighbour_ids));
00903   //            }
00904   //            else
00905   //            {
00906   //                ROS_INFO("Reset old information in new planning task");
00907   //                // reset planner
00908   //                ivPlannerEnvironmentPtr->reset();
00909   //                setPlanner();
00910   //                //ivPlannerPtr->force_planning_from_scratch();
00911   //            }
00912   //        }
00913   //        else
00914   //        {
00915   //            ROS_INFO("Reset old information in new planning task");
00916   //            // reset planner
00917   //            ivPlannerEnvironmentPtr->reset();
00918   //            setPlanner();
00919   //            //ivPlannerPtr->force_planning_from_scratch();
00920   //        }
00921 }
00922 
00923 
00924 State
00925 FootstepPlanner::getFootPose(const State& robot, Leg leg)
00926 {
00927   double shift_x = -sin(robot.getTheta()) * ivFootSeparation / 2.0;
00928   double shift_y =  cos(robot.getTheta()) * ivFootSeparation / 2.0;
00929 
00930   double sign = -1.0;
00931   if (leg == LEFT)
00932     sign = 1.0;
00933 
00934   return State(robot.getX() + sign * shift_x,
00935                robot.getY() + sign * shift_y,
00936                robot.getTheta(),
00937                leg);
00938 }
00939 
00940 
00941 bool
00942 FootstepPlanner::pathIsNew(const std::vector<int>& new_path)
00943 {
00944   if (new_path.size() != ivPlanningStatesIds.size())
00945     return true;
00946 
00947   bool unequal = true;
00948   for (unsigned i = 0; i < new_path.size(); ++i)
00949     unequal = new_path[i] != ivPlanningStatesIds[i] && unequal;
00950 
00951   return unequal;
00952 }
00953 
00954 
00955 void
00956 FootstepPlanner::clearFootstepPathVis(unsigned num_footsteps)
00957 {
00958   visualization_msgs::Marker marker;
00959   visualization_msgs::MarkerArray marker_msg;
00960 
00961   marker.header.stamp = ros::Time::now();
00962   marker.header.frame_id = ivMapPtr->getFrameID();
00963 
00964 
00965   if (num_footsteps < 1)
00966     num_footsteps = ivLastMarkerMsgSize;
00967 
00968   for (unsigned i = 0; i < num_footsteps; ++i)
00969   {
00970     marker.ns = ivMarkerNamespace;
00971     marker.id = i;
00972     marker.action = visualization_msgs::Marker::DELETE;
00973 
00974     marker_msg.markers.push_back(marker);
00975   }
00976 
00977   ivFootstepPathVisPub.publish(marker_msg);
00978 }
00979 
00980 
00981 void
00982 FootstepPlanner::broadcastExpandedNodesVis()
00983 {
00984   if (ivExpandedStatesVisPub.getNumSubscribers() > 0)
00985   {
00986     sensor_msgs::PointCloud cloud_msg;
00987     geometry_msgs::Point32 point;
00988     std::vector<geometry_msgs::Point32> points;
00989 
00990     State s;
00991     FootstepPlannerEnvironment::exp_states_2d_iter_t state_id_it;
00992     for(state_id_it = ivPlannerEnvironmentPtr->getExpandedStatesStart();
00993         state_id_it != ivPlannerEnvironmentPtr->getExpandedStatesEnd();
00994         ++state_id_it)
00995     {
00996       point.x = cell_2_state(state_id_it->first,
00997                              ivEnvironmentParams.cell_size);
00998       point.y = cell_2_state(state_id_it->second,
00999                              ivEnvironmentParams.cell_size);
01000       point.z = 0.01;
01001       points.push_back(point);
01002     }
01003     cloud_msg.header.stamp = ros::Time::now();
01004     cloud_msg.header.frame_id = ivMapPtr->getFrameID();
01005 
01006     cloud_msg.points = points;
01007 
01008     ivExpandedStatesVisPub.publish(cloud_msg);
01009   }
01010 }
01011 
01012 
01013 void
01014 FootstepPlanner::broadcastFootstepPathVis()
01015 {
01016   if (getPathSize() == 0)
01017   {
01018     ROS_INFO("no path has been extracted yet");
01019     return;
01020   }
01021 
01022   clearFootstepPathVis(0);
01023 
01024   visualization_msgs::Marker marker;
01025   visualization_msgs::MarkerArray broadcast_msg;
01026   std::vector<visualization_msgs::Marker> markers;
01027 
01028   int markers_counter = 0;
01029 
01030   marker.header.stamp = ros::Time::now();
01031   marker.header.frame_id = ivMapPtr->getFrameID();
01032 
01033   // add the missing start foot to the publish vector for visualization:
01034   if (ivPath.front().getLeg() == LEFT)
01035     footPoseToMarker(ivStartFootRight, &marker);
01036   else
01037     footPoseToMarker(ivStartFootLeft, &marker);
01038   marker.id = markers_counter++;
01039   markers.push_back(marker);
01040 
01041   // add the footsteps of the path to the publish vector
01042   for(state_iter_t path_iter = getPathBegin(); path_iter != getPathEnd();
01043       ++path_iter)
01044   {
01045     footPoseToMarker(*path_iter, &marker);
01046     marker.id = markers_counter++;
01047     markers.push_back(marker);
01048   }
01049 
01050   broadcast_msg.markers = markers;
01051   ivLastMarkerMsgSize = markers.size();
01052 
01053   ivFootstepPathVisPub.publish(broadcast_msg);
01054 }
01055 
01056 
01057 void
01058 FootstepPlanner::broadcastRandomNodesVis()
01059 {
01060   if (ivRandomStatesVisPub.getNumSubscribers() > 0){
01061     sensor_msgs::PointCloud cloud_msg;
01062     geometry_msgs::Point32 point;
01063     std::vector<geometry_msgs::Point32> points;
01064     visualization_msgs::Marker marker;
01065     visualization_msgs::MarkerArray broadcast_msg;
01066     std::vector<visualization_msgs::Marker> markers;
01067 
01068     marker.header.stamp = ros::Time::now();
01069     marker.header.frame_id = ivMapPtr->getFrameID();
01070 
01071     State s;
01072     FootstepPlannerEnvironment::exp_states_iter_t state_id_iter;
01073     for(state_id_iter = ivPlannerEnvironmentPtr->getRandomStatesStart();
01074         state_id_iter != ivPlannerEnvironmentPtr->getRandomStatesEnd();
01075         ++state_id_iter)
01076     {
01077       if (!ivPlannerEnvironmentPtr->getState(*state_id_iter, &s))
01078       {
01079         ROS_WARN("Could not get random state %d", *state_id_iter);
01080       }
01081       else
01082       {
01083         point.x = s.getX();
01084         point.y = s.getY();
01085         point.z = 0.01;
01086         points.push_back(point);
01087       }
01088     }
01089     cloud_msg.header.stamp = ros::Time::now();
01090     cloud_msg.header.frame_id = ivMapPtr->getFrameID();
01091 
01092     cloud_msg.points = points;
01093 
01094     ivRandomStatesVisPub.publish(cloud_msg);
01095   }
01096 }
01097 
01098 
01099 void
01100 FootstepPlanner::broadcastPathVis()
01101 {
01102   if (getPathSize() == 0)
01103   {
01104     ROS_INFO("no path has been extracted yet");
01105     return;
01106   }
01107 
01108   nav_msgs::Path path_msg;
01109   geometry_msgs::PoseStamped state;
01110 
01111   state.header.stamp = ros::Time::now();
01112   state.header.frame_id = ivMapPtr->getFrameID();
01113 
01114   state_iter_t path_iter;
01115   for(path_iter = getPathBegin(); path_iter != getPathEnd(); ++path_iter)
01116   {
01117     state.pose.position.x = path_iter->getX();
01118     state.pose.position.y = path_iter->getY();
01119     path_msg.poses.push_back(state);
01120   }
01121 
01122   path_msg.header = state.header;
01123   ivPathVisPub.publish(path_msg);
01124 }
01125 
01126 
01127 void
01128 FootstepPlanner::footPoseToMarker(const State& foot_pose,
01129                                   visualization_msgs::Marker* marker)
01130 {
01131   marker->header.stamp = ros::Time::now();
01132   marker->header.frame_id = ivMapPtr->getFrameID();
01133   marker->ns = ivMarkerNamespace;
01134   marker->type = visualization_msgs::Marker::CUBE;
01135   marker->action = visualization_msgs::Marker::ADD;
01136 
01137   float cos_theta = cos(foot_pose.getTheta());
01138   float sin_theta = sin(foot_pose.getTheta());
01139   float x_shift = cos_theta * ivEnvironmentParams.foot_origin_shift_x -
01140                   sin_theta * ivEnvironmentParams.foot_origin_shift_y;
01141   float y_shift;
01142   if (foot_pose.getLeg() == LEFT)
01143     y_shift = sin_theta * ivEnvironmentParams.foot_origin_shift_x +
01144               cos_theta * ivEnvironmentParams.foot_origin_shift_y;
01145   else // leg == RLEG
01146     y_shift = sin_theta * ivEnvironmentParams.foot_origin_shift_x -
01147               cos_theta * ivEnvironmentParams.foot_origin_shift_y;
01148   marker->pose.position.x = foot_pose.getX() + x_shift;
01149   marker->pose.position.y = foot_pose.getY() + y_shift;
01150   marker->pose.position.z = ivEnvironmentParams.footsize_z / 2.0;
01151   tf::quaternionTFToMsg(tf::createQuaternionFromYaw(foot_pose.getTheta()),
01152                         marker->pose.orientation);
01153 
01154   marker->scale.x = ivEnvironmentParams.footsize_x; // - 0.01;
01155   marker->scale.y = ivEnvironmentParams.footsize_y; // - 0.01;
01156   marker->scale.z = ivEnvironmentParams.footsize_z;
01157 
01158   // TODO: make color configurable?
01159   if (foot_pose.getLeg() == RIGHT)
01160   {
01161     marker->color.r = 0.0f;
01162     marker->color.g = 1.0f;
01163   }
01164   else // leg == LEFT
01165       {
01166     marker->color.r = 1.0f;
01167     marker->color.g = 0.0f;
01168       }
01169   marker->color.b = 0.0;
01170   marker->color.a = 0.6;
01171 
01172   marker->lifetime = ros::Duration();
01173 }
01174 }


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:41