$search
00001 //================================================================================================= 00002 // Copyright (c) 2012, Mark Sollweck, Stefan Kohlbrecher, Florian Berz TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Simulation, Systems Optimization and Robotics 00013 // group, TU Darmstadt nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #include "hector_exploration_planner/hector_exploration_planner.h" 00030 00031 #include <visualization_msgs/Marker.h> 00032 #include <visualization_msgs/MarkerArray.h> 00033 #include <hector_nav_msgs/GetRobotTrajectory.h> 00034 00035 #define STRAIGHT_COST 3 00036 #define DIAGONAL_COST 4 00037 00038 using namespace hector_exploration_planner; 00039 00040 HectorExplorationPlanner::HectorExplorationPlanner() 00041 : costmap_ros_(0) 00042 , occupancy_grid_array_(0) 00043 , exploration_trans_array_(0) 00044 , obstacle_trans_array_(0) 00045 , frontier_map_array_(0) 00046 , is_goal_array_(0) 00047 , initialized_(false) 00048 , map_width_(0) 00049 , map_height_(0) 00050 , num_map_cells_(0) 00051 {} 00052 00053 HectorExplorationPlanner::~HectorExplorationPlanner(){ 00054 this->deleteMapData(); 00055 } 00056 00057 HectorExplorationPlanner::HectorExplorationPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros_in) : 00058 costmap_ros_(NULL), initialized_(false) { 00059 HectorExplorationPlanner::initialize(name, costmap_ros_in); 00060 } 00061 00062 void HectorExplorationPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros_in){ 00063 00064 // unknown: 255, obstacle 254, inflated: 253, free: 0 00065 00066 if(initialized_){ 00067 ROS_ERROR("[hector_exploration_planner] HectorExplorationPlanner is already initialized_! Please check why initialize() got called twice."); 00068 return; 00069 } 00070 00071 ROS_INFO("[hector_exploration_planner] Initializing HectorExplorationPlanner"); 00072 00073 // initialize costmaps 00074 this->costmap_ros_ = costmap_ros_in; 00075 this->setupMapData(); 00076 00077 // initialize parameters 00078 ros::NodeHandle private_nh_("~/" + name); 00079 ros::NodeHandle nh; 00080 visualization_pub_ = private_nh_.advertise<visualization_msgs::Marker>("visualization_marker", 0); 00081 private_nh_.param("security_constant", p_alpha_, 0.5); 00082 private_nh_.param("min_obstacle_dist", p_min_obstacle_dist_, 10); 00083 private_nh_.param("plan_in_unknown", p_plan_in_unknown_, false); 00084 private_nh_.param("use_inflated_obstacle", p_use_inflated_obs_, true); 00085 private_nh_.param("goal_angle_penalty", p_goal_angle_penalty_, 50); 00086 private_nh_.param("min_frontier_size", p_min_frontier_size_, 5); 00087 private_nh_.param("dist_for_goal_reached", p_dist_for_goal_reached_, 0.25); 00088 private_nh_.param("same_frontier_distance", p_same_frontier_dist_, 0.25); 00089 00090 path_service_client_ = nh.serviceClient<hector_nav_msgs::GetRobotTrajectory>("trajectory"); 00091 00092 ROS_DEBUG("[hector_exploration_planner] Parameter set. security_const: %f, min_obstacle_dist: %d, plan_in_unknown: %d, use_inflated_obstacle: %d, p_goal_angle_penalty_:%d , min_frontier_size: %d, p_dist_for_goal_reached_: %f, same_frontier: %f", p_alpha_, p_min_obstacle_dist_, p_plan_in_unknown_, p_use_inflated_obs_, p_goal_angle_penalty_, p_min_frontier_size_,p_dist_for_goal_reached_,p_same_frontier_dist_); 00093 p_min_obstacle_dist_ = p_min_obstacle_dist_ * STRAIGHT_COST; 00094 00095 this->name = name; 00096 this->initialized_ = true; 00097 this->previous_goal_ = -1; 00098 00099 } 00100 00101 bool HectorExplorationPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan){ 00102 00103 this->setupMapData(); 00104 00105 // do exploration? (not used anymore? -> call doExploration()) 00106 if ((goal.pose.orientation.w == 0.0) && (goal.pose.orientation.x == 0.0) && 00107 (goal.pose.orientation.y == 0.0) && (goal.pose.orientation.z == 0.0)){ 00108 return doExploration(start,plan); 00109 } 00110 00111 // planning 00112 ROS_INFO("[hector_exploration_planner] planning: starting to make a plan to given goal point"); 00113 00114 // setup maps and goals 00115 resetMaps(); 00116 plan.clear(); 00117 00118 std::vector<geometry_msgs::PoseStamped> goals; 00119 00120 // create obstacle tranform 00121 buildobstacle_trans_array_(); 00122 00123 // plan to given goal 00124 goals.push_back(goal); 00125 00126 // make plan 00127 if(!buildexploration_trans_array_(start,goals,true)){ 00128 return false; 00129 } 00130 if(!getTrajectory(start,goals,plan)){ 00131 return false; 00132 } 00133 00134 // save and add last point 00135 plan.push_back(goal); 00136 unsigned int mx,my; 00137 costmap_.worldToMap(goal.pose.position.x,goal.pose.position.y,mx,my); 00138 previous_goal_ = costmap_.getIndex(mx,my); 00139 00140 ROS_INFO("[hector_exploration_planner] planning: plan has been found! plansize: %u ", (unsigned int)plan.size()); 00141 return true; 00142 } 00143 00144 bool HectorExplorationPlanner::doExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan){ 00145 00146 ROS_INFO("[hector_exploration_planner] exploration: starting exploration"); 00147 00148 this->setupMapData(); 00149 00150 // setup maps and goals 00151 00152 resetMaps(); 00153 clearFrontiers(); 00154 plan.clear(); 00155 00156 std::vector<geometry_msgs::PoseStamped> goals; 00157 00158 00159 // create obstacle tranform 00160 buildobstacle_trans_array_(); 00161 00162 // search for frontiers 00163 if(findFrontiers(goals)){ 00164 ROS_INFO("[hector_exploration_planner] exploration: found %u frontiers!", (unsigned int)goals.size()); 00165 } else { 00166 ROS_INFO("[hector_exploration_planner] exploration: no frontiers have been found! starting inner-exploration"); 00167 return doInnerExploration(start,plan); 00168 } 00169 00170 // make plan 00171 if(!buildexploration_trans_array_(start,goals,true)){ 00172 return false; 00173 } 00174 if(!getTrajectory(start,goals,plan)){ 00175 ROS_INFO("[hector_exploration_planner] exploration: could not plan to frontier, starting inner-exploration"); 00176 return doInnerExploration(start,plan); 00177 } 00178 00179 // update previous goal 00180 if(!plan.empty()){ 00181 geometry_msgs::PoseStamped thisgoal = plan.back(); 00182 unsigned int mx,my; 00183 costmap_.worldToMap(thisgoal.pose.position.x,thisgoal.pose.position.y,mx,my); 00184 previous_goal_ = costmap_.getIndex(mx,my); 00185 } 00186 00187 00188 ROS_INFO("[hector_exploration_planner] exploration: plan to a frontier has been found! plansize: %u", (unsigned int)plan.size()); 00189 return true; 00190 } 00191 00192 bool HectorExplorationPlanner::doInnerExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan){ 00193 ROS_INFO("[hector_exploration_planner] inner-exploration: starting exploration"); 00194 00195 // setup maps and goals 00196 00197 resetMaps(); 00198 clearFrontiers(); 00199 plan.clear(); 00200 00201 std::vector<geometry_msgs::PoseStamped> goals; 00202 00203 // create obstacle tranform 00204 buildobstacle_trans_array_(); 00205 00206 // search for frontiers 00207 if(findInnerFrontier(goals)){ 00208 ROS_INFO("[hector_exploration_planner] inner-exploration: found %u inner-frontiers!", (unsigned int)goals.size()); 00209 } else { 00210 ROS_WARN("[hector_exploration_planner] inner-exploration: no inner-frontiers have been found! exploration failed!"); 00211 return false; 00212 } 00213 00214 // make plan 00215 if(!buildexploration_trans_array_(start,goals,true)){ 00216 return false; 00217 } 00218 if(!getTrajectory(start,goals,plan)){ 00219 ROS_WARN("[hector_exploration_planner] inner-exploration: could not plan to inner-frontier. exploration failed!"); 00220 return false; 00221 } 00222 00223 // cutoff last points of plan due to sbpl error when planning close to walls 00224 00225 int plansize = plan.size() - 5; 00226 if(plansize > 0 ){ 00227 plan.resize(plansize); 00228 } 00229 00230 // update previous goal 00231 if(!plan.empty()){ 00232 geometry_msgs::PoseStamped thisgoal = plan.back(); 00233 unsigned int mx,my; 00234 costmap_.worldToMap(thisgoal.pose.position.x,thisgoal.pose.position.y,mx,my); 00235 previous_goal_ = costmap_.getIndex(mx,my); 00236 } 00237 00238 ROS_INFO("[hector_exploration_planner] inner-exploration: plan to an inner-frontier has been found! plansize: %u", (unsigned int)plan.size()); 00239 return true; 00240 } 00241 00242 bool HectorExplorationPlanner::doAlternativeExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan, std::vector<geometry_msgs::PoseStamped> &oldplan){ 00243 ROS_INFO("[hector_exploration_planner] alternative exploration: starting alternative exploration"); 00244 00245 // setup maps and goals 00246 resetMaps(); 00247 plan.clear(); 00248 00249 std::vector<geometry_msgs::PoseStamped> goals; 00250 00251 std::vector<geometry_msgs::PoseStamped> old_frontier; 00252 old_frontier.push_back(oldplan.back()); 00253 00254 // create obstacle tranform 00255 buildobstacle_trans_array_(); 00256 00257 // search for frontiers 00258 if(findFrontiers(goals,old_frontier)){ 00259 ROS_INFO("[hector_exploration_planner] alternative exploration: found %u frontiers!", (unsigned int) goals.size()); 00260 } else { 00261 ROS_WARN("[hector_exploration_planner] alternative exploration: no frontiers have been found!"); 00262 return false; 00263 } 00264 00265 // make plan 00266 if(!buildexploration_trans_array_(start,goals,true)){ 00267 return false; 00268 } 00269 if(!getTrajectory(start,goals,plan)){ 00270 return false; 00271 } 00272 00273 const geometry_msgs::PoseStamped& this_goal = plan.back(); 00274 unsigned int mx,my; 00275 costmap_.worldToMap(this_goal.pose.position.x,this_goal.pose.position.y,mx,my); 00276 previous_goal_ = costmap_.getIndex(mx,my); 00277 00278 ROS_INFO("[hector_exploration_planner] alternative exploration: plan to a frontier has been found! plansize: %u ", (unsigned int)plan.size()); 00279 return true; 00280 } 00281 00282 float HectorExplorationPlanner::angleDifferenceWall(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal){ 00283 // setup start positions 00284 unsigned int mxs,mys; 00285 costmap_.worldToMap(start.pose.position.x,start.pose.position.y,mxs,mys); 00286 00287 unsigned int gx,gy; 00288 costmap_.worldToMap(goal.pose.position.x,goal.pose.position.y,gx,gy); 00289 00290 int goal_proj_x = gx-mxs; 00291 int goal_proj_y = gy-mys; 00292 00293 float start_angle = tf::getYaw(start.pose.orientation); 00294 float goal_angle = std::atan2(goal_proj_y,goal_proj_x); 00295 00296 float both_angle = 0; 00297 if(start_angle > goal_angle){ 00298 both_angle = start_angle - goal_angle; 00299 } else { 00300 both_angle = goal_angle - start_angle; 00301 } 00302 00303 return both_angle; 00304 } 00305 00306 bool HectorExplorationPlanner::exploreWalls(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan){ 00307 00308 //@TODO: Properly set this parameters 00309 int startExploreWall = 1; 00310 00311 ROS_DEBUG("[hector_exploration_planner] wall-follow: exploreWalls"); 00312 unsigned int mx,my; 00313 if(!costmap_.worldToMap(start.pose.position.x, start.pose.position.y, mx, my)){ 00314 ROS_WARN("[hector_exploration_planner] wall-follow: The start coordinates are outside the costmap!"); 00315 return false; 00316 } 00317 int currentPoint=costmap_.getIndex(mx, my); 00318 int nextPoint; 00319 int oldDirection = -1; 00320 int k=0; 00321 int loop=0; 00322 00323 while(k<50){ 00324 int adjacentPoints [8]; 00325 getAdjacentPoints(currentPoint, adjacentPoints); 00326 int dirPoints [3]; 00327 00328 int minDelta=INT_MAX; 00329 int maxDelta=0; 00330 int thisDelta; 00331 float minAngle=3.1415; //Rad -> 180° 00332 00333 geometry_msgs::PoseStamped trajPoint; 00334 unsigned int gx,gy; 00335 00336 if(oldDirection==-1){ 00337 // find robot orientation 00338 for ( int i=0; i<8; i++){ 00339 costmap_.indexToCells((unsigned int)adjacentPoints[i],gx,gy); 00340 double wx,wy; 00341 costmap_.mapToWorld(gx,gy,wx,wy); 00342 std::string global_frame = costmap_ros_->getGlobalFrameID(); 00343 trajPoint.header.frame_id = global_frame; 00344 trajPoint.pose.position.x = wx; 00345 trajPoint.pose.position.y = wy; 00346 trajPoint.pose.position.z = 0.0; 00347 float yaw = angleDifferenceWall(start, trajPoint); 00348 if(yaw < minAngle){ 00349 minAngle=yaw; 00350 oldDirection=i; 00351 } 00352 } 00353 } 00354 00355 //search possible orientation 00356 00357 if (oldDirection == 0){ 00358 dirPoints[0]=oldDirection+4; //right-forward 00359 dirPoints[1]=oldDirection; //forward 00360 dirPoints[2]=oldDirection+7; //left-forward 00361 } 00362 else if (oldDirection < 3){ 00363 dirPoints[0]=oldDirection+4; 00364 dirPoints[1]=oldDirection; 00365 dirPoints[2]=oldDirection+3; 00366 } 00367 else if (oldDirection == 3){ 00368 dirPoints[0]=oldDirection+4; 00369 dirPoints[1]=oldDirection; 00370 dirPoints[2]=oldDirection+3; 00371 } 00372 else if (oldDirection == 4){ 00373 dirPoints[0]=oldDirection-3; 00374 dirPoints[1]=oldDirection; 00375 dirPoints[2]=oldDirection-4; 00376 } 00377 else if (oldDirection < 7){ 00378 dirPoints[0]=oldDirection-3; 00379 dirPoints[1]=oldDirection; 00380 dirPoints[2]=oldDirection-4; 00381 } 00382 else if (oldDirection == 7){ 00383 dirPoints[0]=oldDirection-7; 00384 dirPoints[1]=oldDirection; 00385 dirPoints[2]=oldDirection-4; 00386 } 00387 00388 // decide LHR or RHR 00389 if(startExploreWall == -1){ 00390 if(obstacle_trans_array_[adjacentPoints[dirPoints[0]]] <= obstacle_trans_array_[adjacentPoints[dirPoints[2]]]){ 00391 startExploreWall = 0; 00392 ROS_INFO("[hector_exploration_planner] wall-follow: RHR");//mirror inverted?? 00393 } 00394 else { 00395 startExploreWall = 1; 00396 ROS_INFO("[hector_exploration_planner] wall-follow: LHR");//mirror inverted?? 00397 } 00398 } 00399 00400 00401 00402 //switch left and right, LHR 00403 if(startExploreWall == 1){ 00404 int temp=dirPoints[0]; 00405 dirPoints[0]=dirPoints[2]; 00406 dirPoints[2]=temp; 00407 } 00408 00409 00410 // find next point 00411 int t=0; 00412 for(int i=0; i<3; i++){ 00413 thisDelta= obstacle_trans_array_[adjacentPoints[dirPoints[i]]]; 00414 00415 if (thisDelta > 3000 || loop > 7) // point is unknown or robot drive loop 00416 { 00417 int plansize = plan.size() - 4; 00418 if(plansize > 0 ){ 00419 plan.resize(plansize); 00420 } 00421 ROS_DEBUG("[hector_exploration_planner] wall-follow: END: exploreWalls. Plansize %d", plan.size()); 00422 return !plan.empty(); 00423 } 00424 00425 if(thisDelta >= p_min_obstacle_dist_){ 00426 if(obstacle_trans_array_[currentPoint] >= p_min_obstacle_dist_){ 00427 if(abs(thisDelta - p_min_obstacle_dist_) < minDelta){ 00428 minDelta = abs(thisDelta - p_min_obstacle_dist_); 00429 nextPoint = adjacentPoints[dirPoints[i]]; 00430 oldDirection = dirPoints[i]; 00431 } 00432 } 00433 if(obstacle_trans_array_[currentPoint] < p_min_obstacle_dist_){ 00434 if(thisDelta > maxDelta){ 00435 maxDelta = thisDelta; 00436 nextPoint = adjacentPoints[dirPoints[i]]; 00437 oldDirection = dirPoints[i]; 00438 } 00439 } 00440 } 00441 else { 00442 if(thisDelta < obstacle_trans_array_[currentPoint]){ 00443 t++; 00444 } 00445 if(thisDelta > maxDelta){ 00446 maxDelta = thisDelta; 00447 nextPoint = adjacentPoints[dirPoints[i]]; 00448 oldDirection = dirPoints[i]; 00449 00450 } 00451 } 00452 } 00453 00454 if(t==3 && abs(obstacle_trans_array_[adjacentPoints[dirPoints[0]]] - obstacle_trans_array_[adjacentPoints[dirPoints[1]]]) < STRAIGHT_COST 00455 && abs(obstacle_trans_array_[adjacentPoints[dirPoints[0]]] - obstacle_trans_array_[adjacentPoints[dirPoints[2]]]) < STRAIGHT_COST 00456 && abs(obstacle_trans_array_[adjacentPoints[dirPoints[1]]] - obstacle_trans_array_[adjacentPoints[dirPoints[2]]]) < STRAIGHT_COST){ 00457 nextPoint=adjacentPoints[dirPoints[2]]; 00458 oldDirection=dirPoints[2]; 00459 } 00460 00461 if(oldDirection==dirPoints[2]) 00462 loop++; 00463 else 00464 loop=0; 00465 00466 // add point 00467 unsigned int sx,sy; 00468 costmap_.indexToCells((unsigned int)currentPoint,sx,sy); 00469 costmap_.indexToCells((unsigned int)nextPoint,gx,gy); 00470 double wx,wy; 00471 costmap_.mapToWorld(sx,sy,wx,wy); 00472 std::string global_frame = costmap_ros_->getGlobalFrameID(); 00473 trajPoint.header.frame_id = global_frame; 00474 trajPoint.pose.position.x = wx; 00475 trajPoint.pose.position.y = wy; 00476 trajPoint.pose.position.z = 0.0; 00477 // assign orientation 00478 int dx = gx-sx; 00479 int dy = gy-sy; 00480 double yaw_path = std::atan2(dy,dx); 00481 trajPoint.pose.orientation.x = 0.0; 00482 trajPoint.pose.orientation.y = 0.0; 00483 trajPoint.pose.orientation.z = sin(yaw_path*0.5f); 00484 trajPoint.pose.orientation.w = cos(yaw_path*0.5f); 00485 plan.push_back(trajPoint); 00486 00487 currentPoint=nextPoint; 00488 k++; 00489 } 00490 ROS_DEBUG("[hector_exploration_planner] wall-follow: END: exploreWalls. Plansize %d", plan.size()); 00491 return !plan.empty(); 00492 } 00493 00494 void HectorExplorationPlanner::setupMapData() 00495 { 00496 costmap_ros_->getCostmapCopy(costmap_); 00497 00498 if ((this->map_width_ != costmap_ros_->getSizeInCellsX()) || (this->map_height_ != costmap_ros_->getSizeInCellsY())){ 00499 this->deleteMapData(); 00500 00501 map_width_ = costmap_.getSizeInCellsX(); 00502 map_height_ = costmap_.getSizeInCellsY(); 00503 num_map_cells_ = map_width_ * map_height_; 00504 00505 // initialize exploration_trans_array_, obstacle_trans_array_, goalMap and frontier_map_array_ 00506 exploration_trans_array_ = new unsigned int[num_map_cells_]; 00507 obstacle_trans_array_ = new unsigned int[num_map_cells_]; 00508 is_goal_array_ = new bool[num_map_cells_]; 00509 frontier_map_array_ = new int[num_map_cells_]; 00510 clearFrontiers(); 00511 resetMaps(); 00512 } 00513 00514 00515 occupancy_grid_array_ = costmap_.getCharMap(); 00516 } 00517 00518 void HectorExplorationPlanner::deleteMapData() 00519 { 00520 if(exploration_trans_array_){ 00521 delete[] exploration_trans_array_; 00522 exploration_trans_array_ = 0; 00523 } 00524 if(obstacle_trans_array_){ 00525 delete[] obstacle_trans_array_; 00526 obstacle_trans_array_ = 0; 00527 } 00528 if(is_goal_array_){ 00529 delete[] is_goal_array_; 00530 is_goal_array_ = 0; 00531 } 00532 if(frontier_map_array_){ 00533 delete[] frontier_map_array_; 00534 frontier_map_array_=0; 00535 } 00536 } 00537 00538 00539 bool HectorExplorationPlanner::buildexploration_trans_array_(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals, bool useAnglePenalty){ 00540 00541 ROS_DEBUG("[hector_exploration_planner] buildexploration_trans_array_"); 00542 00543 std::queue<int> myqueue; 00544 00545 // initialize goals 00546 for(unsigned int i = 0; i < goals.size(); ++i){ 00547 // setup goal positions 00548 unsigned int mx,my; 00549 00550 if(!costmap_.worldToMap(goals[i].pose.position.x,goals[i].pose.position.y,mx,my)){ 00551 ROS_WARN("[hector_exploration_planner] The goal coordinates are outside the costmap!"); 00552 return false; 00553 } 00554 00555 int goal_point = costmap_.getIndex(mx,my); 00556 00557 if(!isFree(goal_point)){ 00558 ROS_WARN("[hector_exploration_planner] The goal coordinates are occupied! (obstacle, inflated obstacle or unknown)"); 00559 return false; 00560 } 00561 00562 unsigned int init_cost = 0; 00563 if(useAnglePenalty){ 00564 init_cost = angleDanger(angleDifference(start,goals[i])) * getDistanceWeight(start,goals[i]); 00565 } 00566 00567 exploration_trans_array_[goal_point] = init_cost; 00568 00569 00570 // do not punish previous frontiers (oscillation) 00571 if(isValid(previous_goal_)){ 00572 if(isSameFrontier(goal_point, previous_goal_)){ 00573 ROS_DEBUG("[hector_exploration_planner] same frontier: init with 0"); 00574 exploration_trans_array_[goal_point] = 0; 00575 } 00576 } 00577 ROS_DEBUG("[hector_exploration_planner] Goal init cost: %d, point: %d", exploration_trans_array_[goal_point], goal_point); 00578 is_goal_array_[goal_point] = true; 00579 myqueue.push(goal_point); 00580 00581 00582 } 00583 00584 // exploration transform algorithm 00585 while(myqueue.size()){ 00586 int point = myqueue.front(); 00587 myqueue.pop(); 00588 00589 unsigned int point_newvalue = exploration_trans_array_[point]; 00590 unsigned int minimum = exploration_trans_array_[point]; 00591 00592 int straightPoints[4]; 00593 getStraightPoints(point,straightPoints); 00594 int diagonalPoints[4]; 00595 getDiagonalPoints(point,diagonalPoints); 00596 00597 00598 // calculate the minimum exploration value of all adjacent cells 00599 if(!is_goal_array_[point]){ 00600 00601 for(int i = 0; i < 4; ++i){ 00602 if(isFree(straightPoints[i]) && (exploration_trans_array_[straightPoints[i]] + STRAIGHT_COST + cellDanger(point)) < minimum){ 00603 minimum = exploration_trans_array_[straightPoints[i]] + STRAIGHT_COST + cellDanger(point); 00604 } 00605 if(isFree(diagonalPoints[i]) && (exploration_trans_array_[diagonalPoints[i]] + DIAGONAL_COST + cellDanger(point)) < minimum){ 00606 minimum = exploration_trans_array_[diagonalPoints[i]] + DIAGONAL_COST + cellDanger(point); 00607 } 00608 } 00609 point_newvalue = minimum; 00610 } 00611 00612 // if exploration_trans_array_ of the point changes, add all adjacent cells (theirs might change too) 00613 if((point_newvalue < exploration_trans_array_[point]) || (is_goal_array_[point])){ 00614 exploration_trans_array_[point] = point_newvalue; 00615 00616 for(int i = 0; i < 4; ++i){ 00617 if(isFree(straightPoints[i]) && !is_goal_array_[straightPoints[i]]){ 00618 myqueue.push(straightPoints[i]); 00619 } 00620 if(isFree(diagonalPoints[i]) && !is_goal_array_[diagonalPoints[i]]){ 00621 myqueue.push(diagonalPoints[i]); 00622 } 00623 } 00624 } 00625 } 00626 00627 ROS_DEBUG("[hector_exploration_planner] END: buildexploration_trans_array_"); 00628 return true; 00629 00630 } 00631 00632 bool HectorExplorationPlanner::buildobstacle_trans_array_(){ 00633 ROS_DEBUG("[hector_exploration_planner] buildobstacle_trans_array_"); 00634 std::queue<int> myqueue; 00635 00636 // init obstacles 00637 for(unsigned int i=0; i < num_map_cells_; ++i){ 00638 if(occupancy_grid_array_[i] == costmap_2d::LETHAL_OBSTACLE){ 00639 myqueue.push(i); 00640 obstacle_trans_array_[i] = 0; 00641 } else if(p_use_inflated_obs_){ 00642 if(occupancy_grid_array_[i] == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ 00643 myqueue.push(i); 00644 obstacle_trans_array_[i] = 0; 00645 } 00646 } 00647 } 00648 00649 // obstacle transform algorithm 00650 while(myqueue.size()){ 00651 int point = myqueue.front(); 00652 myqueue.pop(); 00653 00654 unsigned int minimum = obstacle_trans_array_[point]; 00655 00656 int straightPoints[4]; 00657 getStraightPoints(point,straightPoints); 00658 int diagonalPoints[4]; 00659 getDiagonalPoints(point,diagonalPoints); 00660 00661 if(obstacle_trans_array_[point] != 0){ 00662 00663 // check all 8 directions 00664 for(int i = 0; i < 4; ++i){ 00665 if(isValid(straightPoints[i]) && ((obstacle_trans_array_[straightPoints[i]] + STRAIGHT_COST) < minimum)){ 00666 minimum = obstacle_trans_array_[straightPoints[i]] + STRAIGHT_COST; 00667 } 00668 if(isValid(diagonalPoints[i]) && ((obstacle_trans_array_[diagonalPoints[i]] + DIAGONAL_COST) < minimum)){ 00669 minimum = obstacle_trans_array_[diagonalPoints[i]] + DIAGONAL_COST; 00670 } 00671 } 00672 } 00673 00674 // if obstacle_trans_array_ of the point changes, add all adjacent cells (theirs might change too) 00675 if((minimum < obstacle_trans_array_[point]) || (obstacle_trans_array_[point] == 0)){ 00676 obstacle_trans_array_[point] = minimum; 00677 00678 for(int i = 0; i < 4; ++i){ 00679 if(isFree(straightPoints[i])){ 00680 myqueue.push(straightPoints[i]); 00681 } 00682 if(isFree(diagonalPoints[i])){ 00683 myqueue.push(diagonalPoints[i]); 00684 } 00685 } 00686 } 00687 } 00688 ROS_DEBUG("[hector_exploration_planner] END: buildobstacle_trans_array_"); 00689 return true; 00690 } 00691 00692 bool HectorExplorationPlanner::getTrajectory(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals, std::vector<geometry_msgs::PoseStamped> &plan){ 00693 00694 ROS_DEBUG("[hector_exploration_planner] getTrajectory"); 00695 00696 // setup start positions 00697 unsigned int mx,my; 00698 00699 if(!costmap_.worldToMap(start.pose.position.x,start.pose.position.y,mx,my)){ 00700 ROS_WARN("[hector_exploration_planner] The start coordinates are outside the costmap!"); 00701 return false; 00702 } 00703 00704 int currentPoint = costmap_.getIndex(mx,my); 00705 int nextPoint = currentPoint; 00706 int maxDelta = 0; 00707 00708 00709 geometry_msgs::PoseStamped trajPoint; 00710 std::string global_frame = costmap_ros_->getGlobalFrameID(); 00711 trajPoint.header.frame_id = global_frame; 00712 00713 while(!is_goal_array_[currentPoint]){ 00714 int thisDelta; 00715 int adjacentPoints[8]; 00716 getAdjacentPoints(currentPoint,adjacentPoints); 00717 00718 for(int i = 0; i < 8; ++i){ 00719 if(isFree(adjacentPoints[i])){ 00720 thisDelta = exploration_trans_array_[currentPoint] - exploration_trans_array_[adjacentPoints[i]]; 00721 if(thisDelta > maxDelta){ 00722 maxDelta = thisDelta; 00723 nextPoint = adjacentPoints[i]; 00724 } 00725 } 00726 } 00727 00728 if(maxDelta == 0){ 00729 ROS_DEBUG("[hector_exploration_planner] No path to the goal could be found by following gradient!"); 00730 return false; 00731 } 00732 00733 00734 // make trajectory point 00735 unsigned int sx,sy,gx,gy; 00736 costmap_.indexToCells((unsigned int)currentPoint,sx,sy); 00737 costmap_.indexToCells((unsigned int)nextPoint,gx,gy); 00738 double wx,wy; 00739 costmap_.mapToWorld(sx,sy,wx,wy); 00740 00741 trajPoint.pose.position.x = wx; 00742 trajPoint.pose.position.y = wy; 00743 trajPoint.pose.position.z = 0.0; 00744 00745 // assign orientation 00746 int dx = gx-sx; 00747 int dy = gy-sy; 00748 double yaw_path = std::atan2(dy,dx); 00749 trajPoint.pose.orientation.x = 0.0; 00750 trajPoint.pose.orientation.y = 0.0; 00751 trajPoint.pose.orientation.z = sin(yaw_path*0.5f); 00752 trajPoint.pose.orientation.w = cos(yaw_path*0.5f); 00753 00754 plan.push_back(trajPoint); 00755 00756 currentPoint = nextPoint; 00757 maxDelta = 0; 00758 } 00759 00760 00761 00762 ROS_DEBUG("[hector_exploration_planner] END: getTrajectory. Plansize %u", (unsigned int)plan.size()); 00763 return !plan.empty(); 00764 } 00765 00766 bool HectorExplorationPlanner::findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers){ 00767 std::vector<geometry_msgs::PoseStamped> empty_vec; 00768 return findFrontiers(frontiers,empty_vec); 00769 } 00770 00771 /* 00772 * searches the occupancy grid for frontier cells and merges them into one target point per frontier. 00773 * The returned frontiers are in world coordinates. 00774 */ 00775 bool HectorExplorationPlanner::findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers, std::vector<geometry_msgs::PoseStamped> &noFrontiers){ 00776 00777 // get latest costmap 00778 clearFrontiers(); 00779 00780 // list of all frontiers in the occupancy grid 00781 std::vector<int> allFrontiers; 00782 00783 // check for all cells in the occupancy grid whether or not they are frontier cells 00784 for(unsigned int i = 0; i < num_map_cells_; ++i){ 00785 if(isFrontier(i)){ 00786 allFrontiers.push_back(i); 00787 } 00788 } 00789 00790 // value of the next blob 00791 int nextBlobValue = 1; 00792 std::list<int> usedBlobs; 00793 00794 for(unsigned int i = 0; i < allFrontiers.size(); ++i){ 00795 00796 // get all adjacent blobs to the current frontier point 00797 int currentPoint = allFrontiers[i]; 00798 int adjacentPoints[8]; 00799 getAdjacentPoints(currentPoint,adjacentPoints); 00800 00801 std::list<int> blobs; 00802 00803 for(int j = 0; j < 8; j++){ 00804 if(isValid(adjacentPoints[j]) && (frontier_map_array_[adjacentPoints[j]] > 0)){ 00805 blobs.push_back(frontier_map_array_[adjacentPoints[j]]); 00806 } 00807 } 00808 blobs.unique(); 00809 00810 if(blobs.empty()){ 00811 // create new blob 00812 frontier_map_array_[currentPoint] = nextBlobValue; 00813 usedBlobs.push_back(nextBlobValue); 00814 nextBlobValue++; 00815 } else { 00816 // merge all found blobs 00817 int blobMergeVal = 0; 00818 00819 for(std::list<int>::iterator adjBlob = blobs.begin(); adjBlob != blobs.end(); ++adjBlob){ 00820 if(adjBlob == blobs.begin()){ 00821 blobMergeVal = *adjBlob; 00822 frontier_map_array_[currentPoint] = blobMergeVal; 00823 00824 } else { 00825 00826 for(unsigned int k = 0; k < allFrontiers.size(); k++){ 00827 if(frontier_map_array_[allFrontiers[k]] == *adjBlob){ 00828 usedBlobs.remove(*adjBlob); 00829 frontier_map_array_[allFrontiers[k]] = blobMergeVal; 00830 } 00831 } 00832 } 00833 } 00834 } 00835 } 00836 00837 int id = 1; 00838 00839 bool visualization_requested = (visualization_pub_.getNumSubscribers() > 0); 00840 00841 // summarize every blob into a single point (maximum obstacle_trans_array_ value) 00842 for(std::list<int>::iterator currentBlob = usedBlobs.begin(); currentBlob != usedBlobs.end(); ++currentBlob){ 00843 int current_frontier_size = 0; 00844 int max_obs_idx = 0; 00845 00846 for(unsigned int i = 0; i < allFrontiers.size(); ++i){ 00847 int point = allFrontiers[i]; 00848 00849 if(frontier_map_array_[point] == *currentBlob){ 00850 current_frontier_size++; 00851 if(obstacle_trans_array_[point] > obstacle_trans_array_[allFrontiers[max_obs_idx]]){ 00852 max_obs_idx = i; 00853 } 00854 } 00855 00856 } 00857 00858 if(current_frontier_size < p_min_frontier_size_){ 00859 continue; 00860 } 00861 00862 int frontier_point = allFrontiers[max_obs_idx]; 00863 unsigned int x,y; 00864 costmap_.indexToCells(frontier_point,x,y); 00865 00866 // check if frontier is valid (not to close to robot and not in noFrontiers vector 00867 bool frontier_is_valid = true; 00868 00869 if(isFrontierReached(frontier_point)){ 00870 frontier_is_valid = false; 00871 } 00872 for(size_t i = 0; i < noFrontiers.size(); ++i){ 00873 const geometry_msgs::PoseStamped& noFrontier = noFrontiers[i]; 00874 unsigned int mx,my; 00875 costmap_.worldToMap(noFrontier.pose.position.x,noFrontier.pose.position.y,mx,my); 00876 int no_frontier_point = costmap_.getIndex(x,y); 00877 if(isSameFrontier(frontier_point,no_frontier_point)){ 00878 frontier_is_valid = false; 00879 } 00880 } 00881 00882 if(frontier_is_valid){ 00883 geometry_msgs::PoseStamped finalFrontier; 00884 double wx,wy; 00885 costmap_.mapToWorld(x,y,wx,wy); 00886 std::string global_frame = costmap_ros_->getGlobalFrameID(); 00887 finalFrontier.header.frame_id = global_frame; 00888 finalFrontier.pose.position.x = wx; 00889 finalFrontier.pose.position.y = wy; 00890 finalFrontier.pose.position.z = 0.0; 00891 00892 double yaw = getYawToUnknown(costmap_.getIndex(x,y)); 00893 finalFrontier.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); 00894 frontiers.push_back(finalFrontier); 00895 00896 // visualization (export to method?) 00897 if(visualization_requested){ 00898 visualization_msgs::Marker marker; 00899 marker.header.frame_id = "map"; 00900 marker.header.stamp = ros::Time(); 00901 marker.ns = "hector_global_planner"; 00902 marker.id = id++; 00903 marker.type = visualization_msgs::Marker::ARROW; 00904 marker.action = visualization_msgs::Marker::ADD; 00905 marker.pose.position.x = wx; 00906 marker.pose.position.y = wy; 00907 marker.pose.position.z = 0.0; 00908 marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); 00909 marker.scale.x = 0.5; 00910 marker.scale.y = 0.5; 00911 marker.scale.z = 0.5; 00912 marker.color.a = 1.0; 00913 marker.color.r = 1.0; 00914 marker.color.g = 0.0; 00915 marker.color.b = 0.0; 00916 marker.lifetime = ros::Duration(5,0); 00917 visualization_pub_.publish(marker); 00918 } 00919 } 00920 } 00921 return !frontiers.empty(); 00922 } 00923 00924 bool HectorExplorationPlanner::findInnerFrontier(std::vector<geometry_msgs::PoseStamped> &innerFrontier){ 00925 clearFrontiers(); 00926 00927 // get the trajectory as seeds for the exploration transform 00928 hector_nav_msgs::GetRobotTrajectory srv_path; 00929 if (path_service_client_.call(srv_path)){ 00930 00931 std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses); 00932 std::vector<geometry_msgs::PoseStamped> goals; 00933 size_t size = traj_vector.size(); 00934 ROS_DEBUG("[hector_exploration_planner] size of trajectory vector %u", (unsigned int)size); 00935 00936 if(size > 0){ 00937 geometry_msgs::PoseStamped lastPose = traj_vector[0]; 00938 goals.push_back(lastPose); 00939 for(size_t i = 1; i < size; ++i){ 00940 const geometry_msgs::PoseStamped& pose = traj_vector[i]; 00941 unsigned int x,y; 00942 costmap_.worldToMap(pose.pose.position.x,pose.pose.position.y,x,y); 00943 unsigned m_point = costmap_.getIndex(x,y); 00944 00945 double dx = lastPose.pose.position.x - pose.pose.position.x; 00946 double dy = lastPose.pose.position.y - pose.pose.position.y; 00947 00948 // extract points with 0.5m distance (if free) 00949 if(isFree(m_point)){ 00950 if((dx*dx) + (dy*dy) > (0.5*0.5)){ 00951 goals.push_back(pose); 00952 lastPose = pose; 00953 } 00954 } 00955 } 00956 00957 00958 ROS_DEBUG("[hector_exploration_planner] pushed %u goals (trajectory) for inner frontier-search", (unsigned int)goals.size()); 00959 00960 // make exploration transform 00961 tf::Stamped<tf::Pose> robotPose; 00962 if(!costmap_ros_->getRobotPose(robotPose)){ 00963 ROS_WARN("[hector_global_planner]: Failed to get RobotPose"); 00964 } 00965 geometry_msgs::PoseStamped robotPoseMsg; 00966 tf::poseStampedTFToMsg(robotPose, robotPoseMsg); 00967 00968 buildexploration_trans_array_(robotPoseMsg, goals, false); 00969 00970 unsigned int x,y; 00971 costmap_.worldToMap(robotPoseMsg.pose.position.x,robotPoseMsg.pose.position.y,x,y); 00972 00973 00974 // get point with maximal distance to trajectory 00975 int max_expl_point = costmap_.getIndex(x,y); 00976 for(unsigned int i = 0; i < num_map_cells_; ++i){ 00977 if(isFree(i)){ 00978 if(exploration_trans_array_[i] < INT_MAX){ 00979 if(exploration_trans_array_[i] > exploration_trans_array_[max_expl_point]){ 00980 if(!isFrontierReached(i)){ 00981 max_expl_point = i; 00982 } 00983 } 00984 } 00985 } 00986 } 00987 00988 // reset exploration transform 00989 for(unsigned int i = 0; i < num_map_cells_; ++i){ 00990 exploration_trans_array_[i] = INT_MAX; 00991 is_goal_array_[i] = false; 00992 } 00993 00994 geometry_msgs::PoseStamped finalFrontier; 00995 unsigned int fx,fy; 00996 double wfx,wfy; 00997 costmap_.indexToCells(max_expl_point,fx,fy); 00998 costmap_.mapToWorld(fx,fy,wfx,wfy); 00999 std::string global_frame = costmap_ros_->getGlobalFrameID(); 01000 finalFrontier.header.frame_id = global_frame; 01001 finalFrontier.pose.position.x = wfx; 01002 finalFrontier.pose.position.y = wfy; 01003 finalFrontier.pose.position.z = 0.0; 01004 01005 // assign orientation 01006 int dx = fx-x; 01007 int dy = fy-y; 01008 double yaw_path = std::atan2(dy,dx); 01009 finalFrontier.pose.orientation.x = 0.0; 01010 finalFrontier.pose.orientation.y = 0.0; 01011 finalFrontier.pose.orientation.z = sin(yaw_path*0.5f); 01012 finalFrontier.pose.orientation.w = cos(yaw_path*0.5f); 01013 01014 innerFrontier.push_back(finalFrontier); 01015 01016 if(visualization_pub_.getNumSubscribers() > 0){ 01017 visualization_msgs::Marker marker; 01018 marker.header.frame_id = "map"; 01019 marker.header.stamp = ros::Time(); 01020 marker.ns = "hector_global_planner"; 01021 marker.id = 100; 01022 marker.type = visualization_msgs::Marker::ARROW; 01023 marker.action = visualization_msgs::Marker::ADD; 01024 marker.pose.position.x = wfx; 01025 marker.pose.position.y = wfy; 01026 marker.pose.position.z = 0.0; 01027 marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_path); 01028 marker.scale.x = 0.5; 01029 marker.scale.y = 0.5; 01030 marker.scale.z = 0.5; 01031 marker.color.a = 1.0; 01032 marker.color.r = 0.0; 01033 marker.color.g = 0.0; 01034 marker.color.b = 1.0; 01035 marker.lifetime = ros::Duration(5,0); 01036 visualization_pub_.publish(marker); 01037 } 01038 return !innerFrontier.empty(); 01039 } 01040 } 01041 return false; 01042 } 01043 01044 /* 01045 * checks if a given point is a frontier cell. a frontier cell is a cell in the occupancy grid 01046 * that seperates known from unknown space. Therefore the cell has to be free but at least three 01047 * of its neighbours need to be unknown 01048 */ 01049 bool HectorExplorationPlanner::isFrontier(int point){ 01050 if(isFree(point)){ 01051 01052 int adjacentPoints[8]; 01053 getAdjacentPoints(point,adjacentPoints); 01054 01055 for(int i = 0; i < 8; ++i){ 01056 if(isValid(adjacentPoints[i])){ 01057 if(occupancy_grid_array_[adjacentPoints[i]] == costmap_2d::NO_INFORMATION){ 01058 01059 int no_inf_count = 0; 01060 int noInfPoints[8]; 01061 getAdjacentPoints(adjacentPoints[i],noInfPoints); 01062 for(int j = 0; j < 8; j++){ 01063 if(occupancy_grid_array_[noInfPoints[j]] == costmap_2d::NO_INFORMATION){ 01064 no_inf_count++; 01065 } 01066 } 01067 if(no_inf_count > 2){ 01068 return true; 01069 } 01070 01071 } 01072 } 01073 } 01074 } 01075 01076 return false; 01077 } 01078 01079 01080 void HectorExplorationPlanner::resetMaps(){ 01081 std::fill_n(exploration_trans_array_, num_map_cells_, INT_MAX); 01082 std::fill_n(obstacle_trans_array_, num_map_cells_, INT_MAX); 01083 std::fill_n(is_goal_array_, num_map_cells_, false); 01084 } 01085 01086 void HectorExplorationPlanner::clearFrontiers(){ 01087 std::fill_n(frontier_map_array_, num_map_cells_, 0); 01088 } 01089 01090 inline bool HectorExplorationPlanner::isValid(int point){ 01091 return (point>=0); 01092 } 01093 01094 bool HectorExplorationPlanner::isFree(int point){ 01095 01096 if(isValid(point)){ 01097 // if a point is not inscribed_inflated_obstacle, leathal_obstacle or no_information, its free 01098 if(p_use_inflated_obs_){ 01099 if(occupancy_grid_array_[point] < costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ 01100 return true; 01101 } 01102 } else { 01103 if(occupancy_grid_array_[point] <= costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ 01104 return true; 01105 } 01106 } 01107 01108 if(p_plan_in_unknown_){ 01109 if(occupancy_grid_array_[point] == costmap_2d::NO_INFORMATION){ 01110 return true; 01111 } 01112 } 01113 } 01114 return false; 01115 } 01116 01117 bool HectorExplorationPlanner::isFrontierReached(int point){ 01118 01119 tf::Stamped<tf::Pose> robotPose; 01120 if(!costmap_ros_->getRobotPose(robotPose)){ 01121 ROS_WARN("[hector_global_planner]: Failed to get RobotPose"); 01122 } 01123 geometry_msgs::PoseStamped robotPoseMsg; 01124 tf::poseStampedTFToMsg(robotPose, robotPoseMsg); 01125 01126 unsigned int fx,fy; 01127 double wfx,wfy; 01128 costmap_.indexToCells(point,fx,fy); 01129 costmap_.mapToWorld(fx,fy,wfx,wfy); 01130 01131 01132 double dx = robotPoseMsg.pose.position.x - wfx; 01133 double dy = robotPoseMsg.pose.position.y - wfy; 01134 01135 if ( (dx*dx) + (dy*dy) < (p_dist_for_goal_reached_*p_dist_for_goal_reached_)) { 01136 ROS_DEBUG("[hector_global_planner]: frontier is within the squared range of: %f", p_dist_for_goal_reached_); 01137 return true; 01138 } 01139 return false; 01140 01141 } 01142 01143 bool HectorExplorationPlanner::isSameFrontier(int frontier_point1, int frontier_point2){ 01144 unsigned int fx1,fy1; 01145 unsigned int fx2,fy2; 01146 double wfx1,wfy1; 01147 double wfx2,wfy2; 01148 costmap_.indexToCells(frontier_point1,fx1,fy1); 01149 costmap_.indexToCells(frontier_point2,fx2,fy2); 01150 costmap_.mapToWorld(fx1,fy1,wfx1,wfy1); 01151 costmap_.mapToWorld(fx2,fy2,wfx2,wfy2); 01152 01153 double dx = wfx1 - wfx2; 01154 double dy = wfy1 - wfy2; 01155 01156 if((dx*dx) + (dy*dy) < (p_same_frontier_dist_*p_same_frontier_dist_)){ 01157 return true; 01158 } 01159 return false; 01160 } 01161 01162 inline unsigned int HectorExplorationPlanner::cellDanger(int point){ 01163 unsigned int danger = 0; 01164 if((int)obstacle_trans_array_[point] <= p_min_obstacle_dist_){ 01165 danger = p_alpha_ * std::pow(p_min_obstacle_dist_ - obstacle_trans_array_[point],2); 01166 } 01167 return danger; 01168 } 01169 01170 float HectorExplorationPlanner::angleDifference(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal){ 01171 // setup start positions 01172 unsigned int mxs,mys; 01173 costmap_.worldToMap(start.pose.position.x,start.pose.position.y,mxs,mys); 01174 01175 unsigned int gx,gy; 01176 costmap_.worldToMap(goal.pose.position.x,goal.pose.position.y,gx,gy); 01177 01178 int goal_proj_x = gx-mxs; 01179 int goal_proj_y = gy-mys; 01180 01181 float start_angle = tf::getYaw(start.pose.orientation); 01182 float goal_angle = std::atan2(goal_proj_y,goal_proj_x); 01183 01184 float both_angle = 0; 01185 if(start_angle > goal_angle){ 01186 both_angle = start_angle - goal_angle; 01187 } else { 01188 both_angle = goal_angle - start_angle; 01189 } 01190 01191 if(both_angle > M_PI){ 01192 both_angle = (M_PI - std::abs(start_angle)) + (M_PI - std::abs(goal_angle)); 01193 } 01194 01195 return both_angle; 01196 } 01197 01198 double HectorExplorationPlanner::getYawToUnknown(int point){ 01199 int adjacentPoints[8]; 01200 getAdjacentPoints(point,adjacentPoints); 01201 01202 int max_obs_idx = 0; 01203 01204 for(int i = 0; i < 8; ++i){ 01205 if(isValid(adjacentPoints[i])){ 01206 if(occupancy_grid_array_[adjacentPoints[i]] == costmap_2d::NO_INFORMATION){ 01207 if(obstacle_trans_array_[adjacentPoints[i]] > obstacle_trans_array_[adjacentPoints[max_obs_idx]]){ 01208 max_obs_idx = i; 01209 } 01210 } 01211 } 01212 } 01213 01214 int orientationPoint = adjacentPoints[max_obs_idx]; 01215 unsigned int sx,sy,gx,gy; 01216 costmap_.indexToCells((unsigned int)point,sx,sy); 01217 costmap_.indexToCells((unsigned int)orientationPoint,gx,gy); 01218 int x = gx-sx; 01219 int y = gy-sy; 01220 double yaw = std::atan2(y,x); 01221 01222 return yaw; 01223 01224 } 01225 01226 unsigned int HectorExplorationPlanner::angleDanger(float angle){ 01227 float angle_fraction = std::pow(angle,3); 01228 unsigned int result = static_cast<unsigned int>(p_goal_angle_penalty_ * angle_fraction); 01229 return result; 01230 } 01231 01232 float HectorExplorationPlanner::getDistanceWeight(const geometry_msgs::PoseStamped &point1, const geometry_msgs::PoseStamped &point2){ 01233 float distance = std::sqrt(std::pow(point1.pose.position.x - point2.pose.position.x,2) + std::pow(point1.pose.position.y - point2.pose.position.y,2)); 01234 if(distance < 0.5){ 01235 return 5.0; 01236 } else { 01237 return 1; 01238 } 01239 } 01240 01241 /* 01242 These functions calculate the index of an adjacent point (left,upleft,up,upright,right,downright,down,downleft) to the 01243 given point. If there is no such point (meaning the point would cause the index to be out of bounds), -1 is returned. 01244 */ 01245 inline void HectorExplorationPlanner::getStraightPoints(int point, int points[]){ 01246 01247 points[0] = left(point); 01248 points[1] = up(point); 01249 points[2] = right(point); 01250 points[3] = down(point); 01251 01252 } 01253 01254 inline void HectorExplorationPlanner::getDiagonalPoints(int point, int points[]){ 01255 01256 points[0] = upleft(point); 01257 points[1] = upright(point); 01258 points[2] = downright(point); 01259 points[3] = downleft(point); 01260 01261 } 01262 01263 inline void HectorExplorationPlanner::getAdjacentPoints(int point, int points[]){ 01264 01265 points[0] = left(point); 01266 points[1] = up(point); 01267 points[2] = right(point); 01268 points[3] = down(point); 01269 points[4] = upleft(point); 01270 points[5] = upright(point); 01271 points[6] = downright(point); 01272 points[7] = downleft(point); 01273 01274 } 01275 01276 inline int HectorExplorationPlanner::left(int point){ 01277 // only go left if no index error and if current point is not already on the left boundary 01278 if((point % map_width_ != 0)){ 01279 return point-1; 01280 } 01281 return -1; 01282 } 01283 inline int HectorExplorationPlanner::upleft(int point){ 01284 if((point % map_width_ != 0) && (point >= (int)map_width_)){ 01285 return point-map_width_-1; 01286 } 01287 return -1; 01288 01289 } 01290 inline int HectorExplorationPlanner::up(int point){ 01291 if(point >= (int)map_width_){ 01292 return point-map_width_; 01293 } 01294 return -1; 01295 } 01296 inline int HectorExplorationPlanner::upright(int point){ 01297 if((point >= (int)map_width_) && ((point + 1) % (int)map_width_ != 0)){ 01298 return point-map_width_+1; 01299 } 01300 return -1; 01301 } 01302 inline int HectorExplorationPlanner::right(int point){ 01303 if((point + 1) % map_width_ != 0){ 01304 return point+1; 01305 } 01306 return -1; 01307 01308 } 01309 inline int HectorExplorationPlanner::downright(int point){ 01310 if(((point + 1) % map_width_ != 0) && ((point/map_width_) < (map_width_-1))){ 01311 return point+map_width_+1; 01312 } 01313 return -1; 01314 01315 } 01316 inline int HectorExplorationPlanner::down(int point){ 01317 if((point/map_width_) < (map_width_-1)){ 01318 return point+map_width_; 01319 } 01320 return -1; 01321 01322 } 01323 inline int HectorExplorationPlanner::downleft(int point){ 01324 if(((point/map_width_) < (map_width_-1)) && (point % map_width_ != 0)){ 01325 return point+map_width_-1; 01326 } 01327 return -1; 01328 01329 } 01330 01331 // // visualization (export to another method?) 01332 // visualization_msgs::Marker marker; 01333 // marker.header.frame_id = "map"; 01334 // marker.header.stamp = ros::Time(); 01335 // marker.ns = "hector_global_planner"; 01336 // marker.id = i + 500; 01337 // marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 01338 // marker.action = visualization_msgs::Marker::ADD; 01339 // marker.pose.position = goals[i].pose.position; 01340 // marker.scale.x = 0.2; 01341 // marker.scale.y = 0.2; 01342 // marker.scale.z = 0.2; 01343 // marker.color.a = 1.0; 01344 // marker.color.r = 0.0; 01345 // marker.color.g = 0.0; 01346 // marker.color.b = 1.0; 01347 // marker.lifetime = ros::Duration(5,0); 01348 // marker.text = boost::lexical_cast<std::string>((int)init_cost) + " - " + boost::lexical_cast<std::string>(getDistanceWeight(start,goals[i])); 01349 // visualization_pub_.publish(marker); 01350 01351 //void HectorExplorationPlanner::saveMaps(std::string path){ 01352 01353 // char costmapPath[1000]; 01354 // sprintf(costmapPath,"%s.map",path.data()); 01355 // char explorationPath[1000]; 01356 // sprintf(explorationPath,"%s.expl",path.data()); 01357 // char obstaclePath[1000]; 01358 // sprintf(obstaclePath,"%s.obs",path.data()); 01359 // char frontierPath[1000]; 01360 // sprintf(frontierPath,"%s.front",path.data()); 01361 01362 01363 // costmap.saveMap(costmapPath); 01364 // FILE *fp_expl = fopen(explorationPath,"w"); 01365 // FILE *fp_obs = fopen(obstaclePath,"w"); 01366 // FILE *fp_front = fopen(frontierPath,"w"); 01367 01368 // if (!fp_expl || !fp_obs || !fp_front) 01369 // { 01370 // ROS_WARN("[hector_exploration_planner] Cannot save maps"); 01371 // return; 01372 // } 01373 01374 // for(unsigned int y = 0; y < map_height_; ++y){ 01375 // for(unsigned int x = 0;x < map_width_; ++x){ 01376 // unsigned int expl = exploration_trans_array_[costmap.getIndex(x,y)]; 01377 // unsigned int obs = obstacle_trans_array_[costmap.getIndex(x,y)]; 01378 // int blobVal = frontier_map_array_[costmap.getIndex(x,y)]; 01379 // fprintf(fp_front,"%d\t", blobVal); 01380 // fprintf(fp_expl,"%d\t", expl); 01381 // fprintf(fp_obs, "%d\t", obs); 01382 // } 01383 // fprintf(fp_expl,"\n"); 01384 // fprintf(fp_obs,"\n"); 01385 // fprintf(fp_front,"\n"); 01386 // } 01387 01388 // fclose(fp_expl); 01389 // fclose(fp_obs); 01390 // fclose(fp_front); 01391 // ROS_INFO("[hector_exploration_planner] Maps have been saved!"); 01392 // return; 01393 01394 //} 01395 01396 // // add last point to path (goal point) 01397 // for(unsigned int i = 0; i < goals.size(); ++i){ 01398 // unsigned int mx,my; 01399 // costmap.worldToMap(goals[i].pose.position.x,goals[i].pose.position.y,mx,my); 01400 01401 // if(currentPoint == (int)costmap.getIndex(mx,my)){ 01402 // plan.push_back(goals[i]); 01403 // previous_goal_ = currentPoint; 01404 // } 01405 01406 // } 01407