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