00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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 #include <Eigen/Geometry>
00035
00036 #include <hector_exploration_planner/ExplorationPlannerConfig.h>
00037
00038 #define STRAIGHT_COST 100
00039 #define DIAGONAL_COST 141
00040
00041
00042
00043
00044 using namespace hector_exploration_planner;
00045
00046 HectorExplorationPlanner::HectorExplorationPlanner()
00047 : costmap_ros_(0)
00048 , costmap_(0)
00049 , initialized_(false)
00050 , map_width_(0)
00051 , map_height_(0)
00052 , num_map_cells_(0)
00053 {}
00054
00055 HectorExplorationPlanner::~HectorExplorationPlanner(){
00056 this->deleteMapData();
00057 }
00058
00059 HectorExplorationPlanner::HectorExplorationPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros_in) :
00060 costmap_ros_(NULL), initialized_(false) {
00061 HectorExplorationPlanner::initialize(name, costmap_ros_in);
00062 }
00063
00064 void HectorExplorationPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros_in){
00065
00066 last_mode_ = FRONTIER_EXPLORE;
00067
00068
00069 if(initialized_){
00070 ROS_ERROR("[hector_exploration_planner] HectorExplorationPlanner is already initialized_! Please check why initialize() got called twice.");
00071 return;
00072 }
00073
00074 ROS_INFO("[hector_exploration_planner] Initializing HectorExplorationPlanner");
00075
00076
00077 this->costmap_ros_ = costmap_ros_in;
00078 this->setupMapData();
00079
00080
00081 ros::NodeHandle private_nh_("~/" + name);
00082 ros::NodeHandle nh;
00083 visualization_pub_ = private_nh_.advertise<visualization_msgs::Marker>("visualization_marker", 1);
00084
00085 observation_pose_pub_ = private_nh_.advertise<geometry_msgs::PoseStamped>("observation_pose", 1, true);
00086 goal_pose_pub_ = private_nh_.advertise<geometry_msgs::PoseStamped>("goal_pose", 1, true);
00087
00088 dyn_rec_server_.reset(new dynamic_reconfigure::Server<hector_exploration_planner::ExplorationPlannerConfig>(ros::NodeHandle("~/hector_exploration_planner")));
00089
00090 dyn_rec_server_->setCallback(boost::bind(&HectorExplorationPlanner::dynRecParamCallback, this, _1, _2));
00091
00092 path_service_client_ = nh.serviceClient<hector_nav_msgs::GetRobotTrajectory>("trajectory");
00093
00094 ROS_INFO("[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_);
00095
00096
00097 this->name = name;
00098 this->initialized_ = true;
00099 this->previous_goal_ = -1;
00100
00101 vis_.reset(new ExplorationTransformVis("exploration_transform"));
00102 close_path_vis_.reset(new ExplorationTransformVis("close_path_exploration_transform"));
00103 inner_vis_.reset(new ExplorationTransformVis("inner_exploration_transform"));
00104 obstacle_vis_.reset(new ExplorationTransformVis("obstacle_transform"));
00105 }
00106
00107 void HectorExplorationPlanner::dynRecParamCallback(hector_exploration_planner::ExplorationPlannerConfig &config, uint32_t level)
00108 {
00109 p_plan_in_unknown_ = config.plan_in_unknown;
00110 p_explore_close_to_path_ = config.explore_close_to_path;
00111 p_use_inflated_obs_ = config.use_inflated_obstacles;
00112 p_goal_angle_penalty_ = config.goal_angle_penalty;
00113 p_alpha_ = config.security_constant;
00114 p_dist_for_goal_reached_ = config.dist_for_goal_reached;
00115 p_same_frontier_dist_ = config.same_frontier_distance;
00116 p_min_frontier_size_ = config.min_frontier_size;
00117 p_min_obstacle_dist_ = config.min_obstacle_dist * STRAIGHT_COST;
00118 p_obstacle_cutoff_dist_ = config.obstacle_cutoff_distance;
00119
00120 double angle_rad = config.observation_pose_allowed_angle * (M_PI / 180.0);
00121 p_cos_of_allowed_observation_pose_angle_ = cos(angle_rad);
00122 p_close_to_path_target_distance_ = config.close_to_path_target_distance;
00123 }
00124
00125 bool HectorExplorationPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &original_goal, std::vector<geometry_msgs::PoseStamped> &plan){
00126
00127 this->setupMapData();
00128
00129
00130
00131 if ((original_goal.pose.orientation.w == 0.0) && (original_goal.pose.orientation.x == 0.0) &&
00132 (original_goal.pose.orientation.y == 0.0) && (original_goal.pose.orientation.z == 0.0)){
00133 ROS_ERROR("Trying to plan with invalid quaternion, this shouldn't be done anymore, but we'll start exploration for now.");
00134 return doExploration(start,plan);
00135 }
00136
00137
00138
00139 ROS_INFO("[hector_exploration_planner] planning: starting to make a plan to given goal point");
00140
00141
00142 resetMaps();
00143 plan.clear();
00144
00145 std::vector<geometry_msgs::PoseStamped> goals;
00146
00147
00148
00149
00150 goal_pose_pub_.publish(original_goal);
00151
00152 geometry_msgs::PoseStamped adjusted_goal;
00153 if (!this->getObservationPose(original_goal, 0.5, adjusted_goal)){
00154 ROS_ERROR("getObservationPose returned false, no area around target point available to drive to!");
00155 return false;
00156 }
00157
00158 observation_pose_pub_.publish(adjusted_goal);
00159
00160
00161 goals.push_back(adjusted_goal);
00162
00163
00164 if(!buildexploration_trans_array_(start,goals,true)){
00165 return false;
00166 }
00167 if(!getTrajectory(start,goals,plan)){
00168 return false;
00169 }
00170
00171
00172 plan.push_back(adjusted_goal);
00173 unsigned int mx,my;
00174 costmap_->worldToMap(adjusted_goal.pose.position.x,adjusted_goal.pose.position.y,mx,my);
00175 previous_goal_ = costmap_->getIndex(mx,my);
00176
00177 ROS_INFO("[hector_exploration_planner] planning: plan has been found! plansize: %u ", (unsigned int)plan.size());
00178 return true;
00179 }
00180
00181 bool HectorExplorationPlanner::doExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan){
00182
00183 ROS_INFO("[hector_exploration_planner] exploration: starting exploration");
00184
00185 this->setupMapData();
00186
00187
00188
00189 resetMaps();
00190 clearFrontiers();
00191 plan.clear();
00192
00193 std::vector<geometry_msgs::PoseStamped> goals;
00194
00195
00196 buildobstacle_trans_array_(p_use_inflated_obs_);
00197
00198
00199 bool frontiers_found = false;
00200
00201 if (p_explore_close_to_path_){
00202
00203 frontiers_found = findFrontiersCloseToPath(goals);
00204
00205 if (!frontiers_found){
00206 ROS_WARN("Close Exploration desired, but no frontiers found. Falling back to normal exploration!");
00207 frontiers_found = findFrontiers(goals);
00208 }
00209
00210 }else{
00211 frontiers_found = findFrontiers(goals);
00212 }
00213
00214
00215 if(frontiers_found){
00216
00217 last_mode_ = FRONTIER_EXPLORE;
00218 ROS_INFO("[hector_exploration_planner] exploration: found %u frontiers!", (unsigned int)goals.size());
00219 } else {
00220 ROS_INFO("[hector_exploration_planner] exploration: no frontiers have been found! starting inner-exploration");
00221 return doInnerExploration(start,plan);
00222 }
00223
00224
00225 if(!buildexploration_trans_array_(start,goals,true)){
00226 return false;
00227 }
00228
00229 if(!getTrajectory(start,goals,plan)){
00230 ROS_INFO("[hector_exploration_planner] exploration: could not plan to frontier, starting inner-exploration");
00231 return doInnerExploration(start,plan);
00232 }
00233
00234
00235 if(!plan.empty()){
00236 geometry_msgs::PoseStamped thisgoal = plan.back();
00237 unsigned int mx,my;
00238 costmap_->worldToMap(thisgoal.pose.position.x,thisgoal.pose.position.y,mx,my);
00239 previous_goal_ = costmap_->getIndex(mx,my);
00240 }
00241
00242
00243 ROS_INFO("[hector_exploration_planner] exploration: plan to a frontier has been found! plansize: %u", (unsigned int)plan.size());
00244 return true;
00245 }
00246
00247 bool HectorExplorationPlanner::doInnerExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan){
00248 ROS_INFO("[hector_exploration_planner] inner-exploration: starting exploration");
00249
00250
00251
00252 resetMaps();
00253 clearFrontiers();
00254 plan.clear();
00255
00256 std::vector<geometry_msgs::PoseStamped> goals;
00257
00258
00259 buildobstacle_trans_array_(p_use_inflated_obs_);
00260
00261
00262 if (last_mode_ == INNER_EXPLORE){
00263
00264 tf::Stamped<tf::Pose> robotPose;
00265 if(!costmap_ros_->getRobotPose(robotPose)){
00266 ROS_WARN("[hector_exploration_planner]: Failed to get RobotPose");
00267 }
00268
00269 unsigned int xm, ym;
00270 costmap_->indexToCells(previous_goal_, xm, ym);
00271
00272 double xw, yw;
00273 costmap_->mapToWorld(xm, ym, xw, yw);
00274
00275 double dx = xw - robotPose.getOrigin().getX();
00276 double dy = yw - robotPose.getOrigin().getY();
00277
00278
00279
00280 if ( (dx*dx + dy*dy) > 0.5*0.5){
00281
00282 geometry_msgs::PoseStamped robotPoseMsg;
00283 tf::poseStampedTFToMsg(robotPose, robotPoseMsg);
00284
00285 geometry_msgs::PoseStamped goalMsg;
00286 goalMsg.pose.position.x = xw;
00287 goalMsg.pose.position.y = yw;
00288 goalMsg.pose.orientation.w = 1.0;
00289
00290 if(makePlan(robotPoseMsg, goalMsg, plan)){
00291
00292 ROS_INFO("[hector_exploration_planner] inner-exploration: Planning to previous inner frontier");
00293 last_mode_ = INNER_EXPLORE;
00294 return true;
00295 }
00296 }
00297 }
00298
00299
00300 if(findInnerFrontier(goals)){
00301 ROS_INFO("[hector_exploration_planner] inner-exploration: found %u inner-frontiers!", (unsigned int)goals.size());
00302 } else {
00303 ROS_WARN("[hector_exploration_planner] inner-exploration: no inner-frontiers have been found! exploration failed!");
00304 return false;
00305 }
00306
00307
00308 if(!buildexploration_trans_array_(start,goals,false)){
00309 ROS_WARN("[hector_exploration_planner] inner-exploration: Creating exploration transform failed!");
00310 return false;
00311 }
00312 if(!getTrajectory(start,goals,plan)){
00313 ROS_WARN("[hector_exploration_planner] inner-exploration: could not plan to inner-frontier. exploration failed!");
00314 return false;
00315 }
00316
00317
00318
00319 int plansize = plan.size() - 5;
00320 if(plansize > 0 ){
00321 plan.resize(plansize);
00322 }
00323
00324
00325 if(!plan.empty()){
00326 const geometry_msgs::PoseStamped& thisgoal = plan.back();
00327 unsigned int mx,my;
00328 costmap_->worldToMap(thisgoal.pose.position.x,thisgoal.pose.position.y,mx,my);
00329 previous_goal_ = costmap_->getIndex(mx,my);
00330 last_mode_ = INNER_EXPLORE;
00331 }
00332
00333 ROS_INFO("[hector_exploration_planner] inner-exploration: plan to an inner-frontier has been found! plansize: %u", (unsigned int)plan.size());
00334 return true;
00335 }
00336
00337 bool HectorExplorationPlanner::getObservationPose(const geometry_msgs::PoseStamped& observation_pose, const double desired_distance, geometry_msgs::PoseStamped& new_observation_pose)
00338 {
00339
00340
00341
00342
00343 unsigned int mxs,mys;
00344 costmap_->worldToMap(observation_pose.pose.position.x, observation_pose.pose.position.y, mxs, mys);
00345
00346 double pose_yaw = tf::getYaw(observation_pose.pose.orientation);
00347
00348 Eigen::Vector2f obs_pose_dir_vec (cos(pose_yaw), sin(pose_yaw));
00349
00350 this->buildobstacle_trans_array_(true);
00351
00352 int searchSize = 2.0 / costmap_->getResolution();
00353
00354 int min_x = mxs - searchSize/2;
00355 int min_y = mys - searchSize/2;
00356
00357 if (min_x < 0){
00358 min_x = 0;
00359 }
00360
00361 if (min_y < 0){
00362 min_y = 0;
00363 }
00364
00365 int max_x = mxs + searchSize/2;
00366 int max_y = mys + searchSize/2;
00367
00368 if (max_x > static_cast<int>(costmap_->getSizeInCellsX())){
00369 max_x = static_cast<int>(costmap_->getSizeInCellsX()-1);
00370 }
00371
00372 if (max_y > static_cast<int>(costmap_->getSizeInCellsY())){
00373 max_y = static_cast<int>(costmap_->getSizeInCellsY()-1);
00374 }
00375
00376 int closest_x = -1;
00377 int closest_y = -1;
00378
00379 unsigned int closest_sqr_dist = UINT_MAX;
00380
00381 bool no_information = true;
00382
00383 for (int x = min_x; x < max_x; ++x){
00384 for (int y = min_y; y < max_y; ++y){
00385
00386 unsigned int point = costmap_->getIndex(x,y);
00387
00388 unsigned int obstacle_trans_val = obstacle_trans_array_[point];
00389
00390 if ( (obstacle_trans_val != UINT_MAX) && (obstacle_trans_val != 0) && (occupancy_grid_array_[point] != costmap_2d::NO_INFORMATION)){
00391
00392 no_information = false;
00393
00394 int diff_x = x - (int)mxs;
00395 int diff_y = y - (int)mys;
00396
00397 unsigned int sqr_dist = diff_x*diff_x + diff_y*diff_y;
00398
00399
00400
00401 if (sqr_dist < closest_sqr_dist){
00402
00403 Eigen::Vector2f curr_dir_vec(static_cast<float>(diff_x), static_cast<float>(diff_y));
00404 curr_dir_vec.normalize();
00405
00406 if (curr_dir_vec.dot(obs_pose_dir_vec) < p_cos_of_allowed_observation_pose_angle_){
00407
00408 closest_x = (unsigned int)x;
00409 closest_y = (unsigned int)y;
00410 closest_sqr_dist = sqr_dist;
00411 }
00412 }
00413 }
00414 }
00415 }
00416
00417 if (no_information){
00418 new_observation_pose.pose = observation_pose.pose;
00419 new_observation_pose.pose.position.z = 0.0;
00420 ROS_INFO("Observation pose unchanged as no information available around goal area");
00421 return true;
00422 }
00423
00424
00425
00426
00427
00428 if ((closest_x > -1) && (closest_y > -1)){
00429
00430 Eigen::Vector2d closest_point_world;
00431 costmap_->mapToWorld(closest_x, closest_y, closest_point_world.x(), closest_point_world.y());
00432
00433 Eigen::Vector2d original_goal_pose(observation_pose.pose.position.x, observation_pose.pose.position.y);
00434
00435
00436 new_observation_pose.header.frame_id = "map";
00437 new_observation_pose.header.stamp = observation_pose.header.stamp;
00438
00439 Eigen::Vector2d dir_vec(original_goal_pose - closest_point_world);
00440
00441 double distance = dir_vec.norm();
00442
00443
00444 if (distance < (costmap_->getResolution() * 1.5)){
00445 new_observation_pose.pose = observation_pose.pose;
00446 new_observation_pose.pose.position.z = 0.0;
00447 ROS_INFO("Observation pose unchanged");
00448 }else{
00449
00450 if (desired_distance < distance){
00451 new_observation_pose.pose.position.x = closest_point_world.x();
00452 new_observation_pose.pose.position.y = closest_point_world.y();
00453 new_observation_pose.pose.position.z = 0.0;
00454 }else{
00455
00456 double test_distance = distance;
00457
00458 Eigen::Vector2d last_valid_pos(closest_point_world);
00459
00460 do{
00461 test_distance += 0.1;
00462
00463 double distance_factor = test_distance / distance;
00464
00465 Eigen::Vector2d new_pos(original_goal_pose - dir_vec*distance_factor);
00466
00467 unsigned int x, y;
00468 costmap_->worldToMap(new_pos[0], new_pos[1], x, y);
00469 unsigned int idx = costmap_->getIndex(x,y);
00470
00471 if(!this->isFree(idx)){
00472 break;
00473 }
00474
00475 last_valid_pos = new_pos;
00476
00477 }while (test_distance < desired_distance);
00478
00479 new_observation_pose.pose.position.x = last_valid_pos.x();
00480 new_observation_pose.pose.position.y = last_valid_pos.y();
00481 new_observation_pose.pose.position.z = 0.0;
00482 }
00483
00484 double angle = std::atan2(dir_vec.y(), dir_vec.x());
00485
00486 new_observation_pose.pose.orientation.w = cos(angle*0.5);
00487 new_observation_pose.pose.orientation.x = 0.0;
00488 new_observation_pose.pose.orientation.y = 0.0;
00489 new_observation_pose.pose.orientation.z = sin(angle*0.5);
00490 ROS_INFO("Observation pose moved away from wall");
00491 }
00492
00493 return true;
00494 }else{
00495
00496 ROS_ERROR("Couldn't find observation pose for given point.");
00497 return false;
00498 }
00499 }
00500
00501 bool HectorExplorationPlanner::doAlternativeExploration(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan, std::vector<geometry_msgs::PoseStamped> &oldplan){
00502 ROS_INFO("[hector_exploration_planner] alternative exploration: starting alternative exploration");
00503
00504
00505 resetMaps();
00506 plan.clear();
00507
00508 std::vector<geometry_msgs::PoseStamped> goals;
00509
00510 std::vector<geometry_msgs::PoseStamped> old_frontier;
00511 old_frontier.push_back(oldplan.back());
00512
00513
00514 buildobstacle_trans_array_(p_use_inflated_obs_);
00515
00516
00517 if(findFrontiers(goals,old_frontier)){
00518 ROS_INFO("[hector_exploration_planner] alternative exploration: found %u frontiers!", (unsigned int) goals.size());
00519 } else {
00520 ROS_WARN("[hector_exploration_planner] alternative exploration: no frontiers have been found!");
00521 return false;
00522 }
00523
00524
00525 if(!buildexploration_trans_array_(start,goals,true)){
00526 return false;
00527 }
00528 if(!getTrajectory(start,goals,plan)){
00529 return false;
00530 }
00531
00532 const geometry_msgs::PoseStamped& this_goal = plan.back();
00533 unsigned int mx,my;
00534 costmap_->worldToMap(this_goal.pose.position.x,this_goal.pose.position.y,mx,my);
00535 previous_goal_ = costmap_->getIndex(mx,my);
00536
00537 ROS_INFO("[hector_exploration_planner] alternative exploration: plan to a frontier has been found! plansize: %u ", (unsigned int)plan.size());
00538 return true;
00539 }
00540
00541 float HectorExplorationPlanner::angleDifferenceWall(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal){
00542
00543 unsigned int mxs,mys;
00544 costmap_->worldToMap(start.pose.position.x,start.pose.position.y,mxs,mys);
00545
00546 unsigned int gx,gy;
00547 costmap_->worldToMap(goal.pose.position.x,goal.pose.position.y,gx,gy);
00548
00549 int goal_proj_x = gx-mxs;
00550 int goal_proj_y = gy-mys;
00551
00552 float start_angle = tf::getYaw(start.pose.orientation);
00553 float goal_angle = std::atan2(goal_proj_y,goal_proj_x);
00554
00555 float both_angle = 0;
00556 if(start_angle > goal_angle){
00557 both_angle = start_angle - goal_angle;
00558 } else {
00559 both_angle = goal_angle - start_angle;
00560 }
00561
00562 return both_angle;
00563 }
00564
00565 bool HectorExplorationPlanner::exploreWalls(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> &plan){
00566
00567
00568 int startExploreWall = 1;
00569
00570 ROS_DEBUG("[hector_exploration_planner] wall-follow: exploreWalls");
00571 unsigned int mx,my;
00572 if(!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)){
00573 ROS_WARN("[hector_exploration_planner] wall-follow: The start coordinates are outside the costmap!");
00574 return false;
00575 }
00576 int currentPoint=costmap_->getIndex(mx, my);
00577 int nextPoint;
00578 int oldDirection = -1;
00579 int k=0;
00580 int loop=0;
00581
00582 while(k<50){
00583 int adjacentPoints [8];
00584 getAdjacentPoints(currentPoint, adjacentPoints);
00585 int dirPoints [3];
00586
00587 unsigned int minDelta = UINT_MAX;
00588 unsigned int maxDelta = 0;
00589 unsigned int thisDelta;
00590 float minAngle=3.1415;
00591
00592 geometry_msgs::PoseStamped trajPoint;
00593 unsigned int gx,gy;
00594
00595 if(oldDirection==-1){
00596
00597 for ( int i=0; i<8; i++){
00598 costmap_->indexToCells((unsigned int)adjacentPoints[i],gx,gy);
00599 double wx,wy;
00600 costmap_->mapToWorld(gx,gy,wx,wy);
00601 std::string global_frame = costmap_ros_->getGlobalFrameID();
00602 trajPoint.header.frame_id = global_frame;
00603 trajPoint.pose.position.x = wx;
00604 trajPoint.pose.position.y = wy;
00605 trajPoint.pose.position.z = 0.0;
00606 float yaw = angleDifferenceWall(start, trajPoint);
00607 if(yaw < minAngle){
00608 minAngle=yaw;
00609 oldDirection=i;
00610 }
00611 }
00612 }
00613
00614
00615
00616 if (oldDirection == 0){
00617 dirPoints[0]=oldDirection+4;
00618 dirPoints[1]=oldDirection;
00619 dirPoints[2]=oldDirection+7;
00620 }
00621 else if (oldDirection < 3){
00622 dirPoints[0]=oldDirection+4;
00623 dirPoints[1]=oldDirection;
00624 dirPoints[2]=oldDirection+3;
00625 }
00626 else if (oldDirection == 3){
00627 dirPoints[0]=oldDirection+4;
00628 dirPoints[1]=oldDirection;
00629 dirPoints[2]=oldDirection+3;
00630 }
00631 else if (oldDirection == 4){
00632 dirPoints[0]=oldDirection-3;
00633 dirPoints[1]=oldDirection;
00634 dirPoints[2]=oldDirection-4;
00635 }
00636 else if (oldDirection < 7){
00637 dirPoints[0]=oldDirection-3;
00638 dirPoints[1]=oldDirection;
00639 dirPoints[2]=oldDirection-4;
00640 }
00641 else if (oldDirection == 7){
00642 dirPoints[0]=oldDirection-7;
00643 dirPoints[1]=oldDirection;
00644 dirPoints[2]=oldDirection-4;
00645 }
00646
00647
00648 if(startExploreWall == -1){
00649 if(obstacle_trans_array_[adjacentPoints[dirPoints[0]]] <= obstacle_trans_array_[adjacentPoints[dirPoints[2]]]){
00650 startExploreWall = 0;
00651 ROS_INFO("[hector_exploration_planner] wall-follow: RHR");
00652 }
00653 else {
00654 startExploreWall = 1;
00655 ROS_INFO("[hector_exploration_planner] wall-follow: LHR");
00656 }
00657 }
00658
00659
00660
00661
00662 if(startExploreWall == 1){
00663 int temp=dirPoints[0];
00664 dirPoints[0]=dirPoints[2];
00665 dirPoints[2]=temp;
00666 }
00667
00668
00669
00670 int t=0;
00671 for(int i=0; i<3; i++){
00672 thisDelta = obstacle_trans_array_[adjacentPoints[dirPoints[i]]];
00673
00674 if (thisDelta > 3000 || loop > 7)
00675 {
00676 int plansize = plan.size() - 4;
00677 if(plansize > 0 ){
00678 plan.resize(plansize);
00679 }
00680 ROS_DEBUG("[hector_exploration_planner] wall-follow: END: exploreWalls. Plansize %d", (int)plan.size());
00681 return !plan.empty();
00682 }
00683
00684 if(thisDelta >= (unsigned int) p_min_obstacle_dist_){
00685 if(obstacle_trans_array_[currentPoint] >= (unsigned int) p_min_obstacle_dist_){
00686 if(abs(thisDelta - p_min_obstacle_dist_) < minDelta){
00687 minDelta = abs(thisDelta - p_min_obstacle_dist_);
00688 nextPoint = adjacentPoints[dirPoints[i]];
00689 oldDirection = dirPoints[i];
00690 }
00691 }
00692 if(obstacle_trans_array_[currentPoint] < (unsigned int) p_min_obstacle_dist_){
00693 if(thisDelta > maxDelta){
00694 maxDelta = thisDelta;
00695 nextPoint = adjacentPoints[dirPoints[i]];
00696 oldDirection = dirPoints[i];
00697 }
00698 }
00699 }
00700 else {
00701 if(thisDelta < obstacle_trans_array_[currentPoint]){
00702 t++;
00703 }
00704 if(thisDelta > maxDelta){
00705 maxDelta = thisDelta;
00706 nextPoint = adjacentPoints[dirPoints[i]];
00707 oldDirection = dirPoints[i];
00708
00709 }
00710 }
00711 }
00712
00713 if(t==3 && abs(obstacle_trans_array_[adjacentPoints[dirPoints[0]]] - obstacle_trans_array_[adjacentPoints[dirPoints[1]]]) < STRAIGHT_COST
00714 && abs(obstacle_trans_array_[adjacentPoints[dirPoints[0]]] - obstacle_trans_array_[adjacentPoints[dirPoints[2]]]) < STRAIGHT_COST
00715 && abs(obstacle_trans_array_[adjacentPoints[dirPoints[1]]] - obstacle_trans_array_[adjacentPoints[dirPoints[2]]]) < STRAIGHT_COST){
00716 nextPoint=adjacentPoints[dirPoints[2]];
00717 oldDirection=dirPoints[2];
00718 }
00719
00720 if(oldDirection==dirPoints[2])
00721 loop++;
00722 else
00723 loop=0;
00724
00725
00726 unsigned int sx,sy;
00727 costmap_->indexToCells((unsigned int)currentPoint,sx,sy);
00728 costmap_->indexToCells((unsigned int)nextPoint,gx,gy);
00729 double wx,wy;
00730 costmap_->mapToWorld(sx,sy,wx,wy);
00731 std::string global_frame = costmap_ros_->getGlobalFrameID();
00732 trajPoint.header.frame_id = global_frame;
00733 trajPoint.pose.position.x = wx;
00734 trajPoint.pose.position.y = wy;
00735 trajPoint.pose.position.z = 0.0;
00736
00737 int dx = gx-sx;
00738 int dy = gy-sy;
00739 double yaw_path = std::atan2(dy,dx);
00740 trajPoint.pose.orientation.x = 0.0;
00741 trajPoint.pose.orientation.y = 0.0;
00742 trajPoint.pose.orientation.z = sin(yaw_path*0.5f);
00743 trajPoint.pose.orientation.w = cos(yaw_path*0.5f);
00744 plan.push_back(trajPoint);
00745
00746 currentPoint=nextPoint;
00747 k++;
00748 }
00749 ROS_DEBUG("[hector_exploration_planner] wall-follow: END: exploreWalls. Plansize %d", (int)plan.size());
00750 return !plan.empty();
00751 }
00752
00753 void HectorExplorationPlanner::setupMapData()
00754 {
00755 #ifdef COSTMAP_2D_LAYERED_COSTMAP_H_
00756 costmap_ = costmap_ros_->getCostmap();
00757 #else
00758 if (costmap_) delete costmap_;
00759 costmap_ = new costmap_2d::Costmap2D;
00760 costmap_ros_->getCostmapCopy(*costmap_);
00761 #endif
00762
00763 if ((this->map_width_ != costmap_->getSizeInCellsX()) || (this->map_height_ != costmap_->getSizeInCellsY())){
00764 map_width_ = costmap_->getSizeInCellsX();
00765 map_height_ = costmap_->getSizeInCellsY();
00766 num_map_cells_ = map_width_ * map_height_;
00767
00768
00769 exploration_trans_array_.reset(new unsigned int[num_map_cells_]);
00770 obstacle_trans_array_.reset(new unsigned int[num_map_cells_]);
00771 is_goal_array_.reset(new bool[num_map_cells_]);
00772 frontier_map_array_.reset(new int[num_map_cells_]);
00773 clearFrontiers();
00774 resetMaps();
00775 }
00776
00777
00778 occupancy_grid_array_ = costmap_->getCharMap();
00779 }
00780
00781 void HectorExplorationPlanner::deleteMapData()
00782 {
00783 exploration_trans_array_.reset();
00784 obstacle_trans_array_.reset();
00785 is_goal_array_.reset();
00786 frontier_map_array_.reset();
00787 }
00788
00789
00790 bool HectorExplorationPlanner::buildexploration_trans_array_(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals, bool useAnglePenalty, bool use_cell_danger){
00791
00792 ROS_DEBUG("[hector_exploration_planner] buildexploration_trans_array_");
00793
00794
00795 std::fill_n(exploration_trans_array_.get(), num_map_cells_, UINT_MAX);
00796 std::fill_n(is_goal_array_.get(), num_map_cells_, false);
00797
00798 std::queue<int> myqueue;
00799
00800 size_t num_free_goals = 0;
00801
00802
00803 for(unsigned int i = 0; i < goals.size(); ++i){
00804
00805 unsigned int mx,my;
00806
00807 if(!costmap_->worldToMap(goals[i].pose.position.x,goals[i].pose.position.y,mx,my)){
00808
00809 continue;
00810 }
00811
00812 int goal_point = costmap_->getIndex(mx,my);
00813
00814
00815 if(!isFree(goal_point)){
00816 continue;
00817 }else{
00818 ++num_free_goals;
00819 }
00820
00821 unsigned int init_cost = 0;
00822 if(false){
00823 init_cost = angleDanger(angleDifference(start,goals[i])) * getDistanceWeight(start,goals[i]);
00824 }
00825
00826 exploration_trans_array_[goal_point] = init_cost;
00827
00828
00829 if(false && isValid(previous_goal_)){
00830 if(isSameFrontier(goal_point, previous_goal_)){
00831 ROS_DEBUG("[hector_exploration_planner] same frontier: init with 0");
00832 exploration_trans_array_[goal_point] = 0;
00833 }
00834 }
00835
00836 ROS_DEBUG("[hector_exploration_planner] Goal init cost: %d, point: %d", exploration_trans_array_[goal_point], goal_point);
00837 is_goal_array_[goal_point] = true;
00838 myqueue.push(goal_point);
00839 }
00840
00841 if (num_free_goals == 0){
00842 ROS_WARN("[hector_exploration_planner] All goal coordinates for exploration transform invalid (occupied or out of bounds), aborting.");
00843 return false;
00844 }
00845
00846
00847 if (use_cell_danger){
00848 while(myqueue.size()){
00849 int point = myqueue.front();
00850 myqueue.pop();
00851
00852 unsigned int minimum = exploration_trans_array_[point];
00853
00854 int straightPoints[4];
00855 getStraightPoints(point,straightPoints);
00856 int diagonalPoints[4];
00857 getDiagonalPoints(point,diagonalPoints);
00858
00859
00860 for (int i = 0; i < 4; ++i) {
00861 if (isFree(straightPoints[i])) {
00862 unsigned int neighbor_cost = minimum + STRAIGHT_COST + cellDanger(straightPoints[i]);
00863
00864 if (exploration_trans_array_[straightPoints[i]] > neighbor_cost) {
00865 exploration_trans_array_[straightPoints[i]] = neighbor_cost;
00866 myqueue.push(straightPoints[i]);
00867 }
00868 }
00869
00870 if (isFree(diagonalPoints[i])) {
00871 unsigned int neighbor_cost = minimum + DIAGONAL_COST + cellDanger(diagonalPoints[i]);
00872
00873 if (exploration_trans_array_[diagonalPoints[i]] > neighbor_cost) {
00874 exploration_trans_array_[diagonalPoints[i]] = neighbor_cost;
00875 myqueue.push(diagonalPoints[i]);
00876 }
00877 }
00878 }
00879 }
00880 }else{
00881 while(myqueue.size()){
00882 int point = myqueue.front();
00883 myqueue.pop();
00884
00885 unsigned int minimum = exploration_trans_array_[point];
00886
00887 int straightPoints[4];
00888 getStraightPoints(point,straightPoints);
00889 int diagonalPoints[4];
00890 getDiagonalPoints(point,diagonalPoints);
00891
00892
00893 for (int i = 0; i < 4; ++i) {
00894 if (isFree(straightPoints[i])) {
00895 unsigned int neighbor_cost = minimum + STRAIGHT_COST;
00896
00897 if (exploration_trans_array_[straightPoints[i]] > neighbor_cost) {
00898 exploration_trans_array_[straightPoints[i]] = neighbor_cost;
00899 myqueue.push(straightPoints[i]);
00900 }
00901 }
00902
00903 if (isFree(diagonalPoints[i])) {
00904 unsigned int neighbor_cost = minimum + DIAGONAL_COST;
00905
00906 if (exploration_trans_array_[diagonalPoints[i]] > neighbor_cost) {
00907 exploration_trans_array_[diagonalPoints[i]] = neighbor_cost;
00908 myqueue.push(diagonalPoints[i]);
00909 }
00910 }
00911 }
00912 }
00913 }
00914
00915 ROS_DEBUG("[hector_exploration_planner] END: buildexploration_trans_array_");
00916
00917 vis_->publishVisOnDemand(*costmap_, exploration_trans_array_.get());
00918 return true;
00919 }
00920
00921 bool HectorExplorationPlanner::buildobstacle_trans_array_(bool use_inflated_obstacles){
00922 ROS_DEBUG("[hector_exploration_planner] buildobstacle_trans_array_");
00923 std::queue<int> myqueue;
00924
00925
00926 for(unsigned int i=0; i < num_map_cells_; ++i){
00927 if(occupancy_grid_array_[i] == costmap_2d::LETHAL_OBSTACLE){
00928 myqueue.push(i);
00929 obstacle_trans_array_[i] = 0;
00930 } else if(use_inflated_obstacles){
00931 if(occupancy_grid_array_[i] == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
00932 myqueue.push(i);
00933 obstacle_trans_array_[i] = 0;
00934 }
00935 }
00936 }
00937
00938 unsigned int obstacle_cutoff_value = static_cast<unsigned int>((p_obstacle_cutoff_dist_ / costmap_->getResolution()) * STRAIGHT_COST + 0.5);
00939
00940
00941 while(myqueue.size()){
00942 int point = myqueue.front();
00943 myqueue.pop();
00944
00945 unsigned int minimum = obstacle_trans_array_[point];
00946 if (minimum > obstacle_cutoff_value) continue;
00947
00948 int straightPoints[4];
00949 getStraightPoints(point,straightPoints);
00950 int diagonalPoints[4];
00951 getDiagonalPoints(point,diagonalPoints);
00952
00953
00954 for(int i = 0; i < 4; ++i){
00955 if (isValid(straightPoints[i]) && (obstacle_trans_array_[straightPoints[i]] > minimum + STRAIGHT_COST)) {
00956 obstacle_trans_array_[straightPoints[i]] = minimum + STRAIGHT_COST;
00957 myqueue.push(straightPoints[i]);
00958 }
00959 if (isValid(diagonalPoints[i]) && (obstacle_trans_array_[diagonalPoints[i]] > minimum + DIAGONAL_COST)) {
00960 obstacle_trans_array_[diagonalPoints[i]] = minimum + DIAGONAL_COST;
00961 myqueue.push(diagonalPoints[i]);
00962 }
00963 }
00964 }
00965
00966 ROS_DEBUG("[hector_exploration_planner] END: buildobstacle_trans_array_");
00967
00968 obstacle_vis_->publishVisOnDemand(*costmap_, obstacle_trans_array_.get());
00969 return true;
00970 }
00971
00972 bool HectorExplorationPlanner::getTrajectory(const geometry_msgs::PoseStamped &start, std::vector<geometry_msgs::PoseStamped> goals, std::vector<geometry_msgs::PoseStamped> &plan){
00973
00974 ROS_DEBUG("[hector_exploration_planner] getTrajectory");
00975
00976
00977 unsigned int mx,my;
00978
00979 if(!costmap_->worldToMap(start.pose.position.x,start.pose.position.y,mx,my)){
00980 ROS_WARN("[hector_exploration_planner] The start coordinates are outside the costmap!");
00981 return false;
00982 }
00983
00984 int currentPoint = costmap_->getIndex(mx,my);
00985 int nextPoint = currentPoint;
00986
00987
00988
00989 geometry_msgs::PoseStamped trajPoint;
00990 std::string global_frame = costmap_ros_->getGlobalFrameID();
00991 trajPoint.header.frame_id = global_frame;
00992
00993 while(!is_goal_array_[currentPoint]){
00994 int thisDelta;
00995 int adjacentPoints[8];
00996 getAdjacentPoints(currentPoint,adjacentPoints);
00997
00998 int maxDelta = 0;
00999
01000 for(int i = 0; i < 8; ++i){
01001 if(isFree(adjacentPoints[i])){
01002 thisDelta = exploration_trans_array_[currentPoint] - exploration_trans_array_[adjacentPoints[i]];
01003 if(thisDelta > maxDelta){
01004 maxDelta = thisDelta;
01005 nextPoint = adjacentPoints[i];
01006 }
01007 }
01008 }
01009
01010
01011 if(maxDelta == 0){
01012 ROS_WARN("[hector_exploration_planner] No path to the goal could be found by following gradient!");
01013 return false;
01014 }
01015
01016
01017
01018 unsigned int sx,sy,gx,gy;
01019 costmap_->indexToCells((unsigned int)currentPoint,sx,sy);
01020 costmap_->indexToCells((unsigned int)nextPoint,gx,gy);
01021 double wx,wy;
01022 costmap_->mapToWorld(sx,sy,wx,wy);
01023
01024 trajPoint.pose.position.x = wx;
01025 trajPoint.pose.position.y = wy;
01026 trajPoint.pose.position.z = 0.0;
01027
01028
01029 int dx = gx-sx;
01030 int dy = gy-sy;
01031 double yaw_path = std::atan2(dy,dx);
01032 trajPoint.pose.orientation.x = 0.0;
01033 trajPoint.pose.orientation.y = 0.0;
01034 trajPoint.pose.orientation.z = sin(yaw_path*0.5f);
01035 trajPoint.pose.orientation.w = cos(yaw_path*0.5f);
01036
01037 plan.push_back(trajPoint);
01038
01039 currentPoint = nextPoint;
01040 maxDelta = 0;
01041 }
01042
01043 ROS_DEBUG("[hector_exploration_planner] END: getTrajectory. Plansize %u", (unsigned int)plan.size());
01044 return !plan.empty();
01045 }
01046
01047 bool HectorExplorationPlanner::findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers){
01048 std::vector<geometry_msgs::PoseStamped> empty_vec;
01049 return findFrontiers(frontiers,empty_vec);
01050 }
01051
01052
01053
01054
01055
01056 bool HectorExplorationPlanner::findFrontiersCloseToPath(std::vector<geometry_msgs::PoseStamped> &frontiers){
01057
01058 clearFrontiers();
01059 frontiers.clear();
01060
01061
01062 hector_nav_msgs::GetRobotTrajectory srv_path;
01063 if (path_service_client_.call(srv_path)){
01064
01065 std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
01066
01067
01068 std::vector<geometry_msgs::PoseStamped> goals;
01069
01070 size_t size = traj_vector.size();
01071 ROS_INFO("[hector_exploration_planner] Size of trajectory vector for close exploration %u", (unsigned int)size);
01072
01073 if(size > 0){
01074 geometry_msgs::PoseStamped lastPose = traj_vector[size-1];
01075 goals.push_back(lastPose);
01076
01077 if (size > 1){
01078
01079 for(int i = static_cast<int>(size-2); i >= 0; --i){
01080 const geometry_msgs::PoseStamped& pose = traj_vector[i];
01081 unsigned int x,y;
01082 costmap_->worldToMap(pose.pose.position.x,pose.pose.position.y,x,y);
01083 unsigned int m_point = costmap_->getIndex(x,y);
01084
01085 double dx = lastPose.pose.position.x - pose.pose.position.x;
01086 double dy = lastPose.pose.position.y - pose.pose.position.y;
01087
01088 if((dx*dx) + (dy*dy) > (0.25*0.25)){
01089 goals.push_back(pose);
01090 lastPose = pose;
01091 }
01092 }
01093
01094 ROS_INFO("[hector_exploration_planner] pushed %u goals (trajectory) for close to robot frontier search", (unsigned int)goals.size());
01095
01096
01097 tf::Stamped<tf::Pose> robotPose;
01098 if(!costmap_ros_->getRobotPose(robotPose)){
01099 ROS_WARN("[hector_exploration_planner]: Failed to get RobotPose");
01100 }
01101 geometry_msgs::PoseStamped robotPoseMsg;
01102 tf::poseStampedTFToMsg(robotPose, robotPoseMsg);
01103
01104 if (!buildexploration_trans_array_(robotPoseMsg, goals, false, false)){
01105 ROS_WARN("[hector_exploration_planner]: Creating exploration transform array in find inner frontier failed, aborting.");
01106 return false;
01107 }
01108
01109 close_path_vis_->publishVisOnDemand(*costmap_, exploration_trans_array_.get());
01110
01111 unsigned int explore_threshold = static_cast<unsigned int> (static_cast<double>(STRAIGHT_COST) * (1.0/costmap_->getResolution()) * p_close_to_path_target_distance_);
01112
01113
01114
01115 for(unsigned int i = 0; i < num_map_cells_; ++i){
01116
01117 unsigned int current_val = exploration_trans_array_[i];
01118
01119 if(current_val < UINT_MAX){
01120
01121 if (current_val >= explore_threshold){
01122 geometry_msgs::PoseStamped finalFrontier;
01123 double wx,wy;
01124 unsigned int mx,my;
01125 costmap_->indexToCells(i, mx, my);
01126 costmap_->mapToWorld(mx,my,wx,wy);
01127 std::string global_frame = costmap_ros_->getGlobalFrameID();
01128 finalFrontier.header.frame_id = global_frame;
01129 finalFrontier.pose.position.x = wx;
01130 finalFrontier.pose.position.y = wy;
01131 finalFrontier.pose.position.z = 0.0;
01132
01133 double yaw = getYawToUnknown(costmap_->getIndex(mx,my));
01134
01135
01136
01137 finalFrontier.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
01138
01139 frontiers.push_back(finalFrontier);
01140
01141 }
01142
01143 }
01144 }
01145
01146 return frontiers.size() > 0;
01147
01148
01149
01150
01151
01152 }
01153 }
01154 }
01155
01156
01157
01158
01159
01160
01161 std::vector<int> allFrontiers;
01162
01163
01164 for(unsigned int i = 0; i < num_map_cells_; ++i){
01165 if(isFrontier(i)){
01166 allFrontiers.push_back(i);
01167 }
01168 }
01169
01170 for(unsigned int i = 0; i < allFrontiers.size(); ++i){
01171 if(!isFrontierReached(allFrontiers[i])){
01172 geometry_msgs::PoseStamped finalFrontier;
01173 double wx,wy;
01174 unsigned int mx,my;
01175 costmap_->indexToCells(allFrontiers[i], mx, my);
01176 costmap_->mapToWorld(mx,my,wx,wy);
01177 std::string global_frame = costmap_ros_->getGlobalFrameID();
01178 finalFrontier.header.frame_id = global_frame;
01179 finalFrontier.pose.position.x = wx;
01180 finalFrontier.pose.position.y = wy;
01181 finalFrontier.pose.position.z = 0.0;
01182
01183 double yaw = getYawToUnknown(costmap_->getIndex(mx,my));
01184
01185
01186
01187 finalFrontier.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
01188
01189 frontiers.push_back(finalFrontier);
01190 }
01191
01192 }
01193
01194 return (frontiers.size() > 0);
01195 }
01196
01197
01198
01199
01200
01201 bool HectorExplorationPlanner::findFrontiers(std::vector<geometry_msgs::PoseStamped> &frontiers, std::vector<geometry_msgs::PoseStamped> &noFrontiers){
01202
01203
01204 clearFrontiers();
01205
01206
01207 std::vector<int> allFrontiers;
01208
01209
01210 for(unsigned int i = 0; i < num_map_cells_; ++i){
01211 if(isFrontier(i)){
01212 allFrontiers.push_back(i);
01213 }
01214 }
01215
01216 for(unsigned int i = 0; i < allFrontiers.size(); ++i){
01217 if(!isFrontierReached(allFrontiers[i])){
01218 geometry_msgs::PoseStamped finalFrontier;
01219 double wx,wy;
01220 unsigned int mx,my;
01221 costmap_->indexToCells(allFrontiers[i], mx, my);
01222 costmap_->mapToWorld(mx,my,wx,wy);
01223 std::string global_frame = costmap_ros_->getGlobalFrameID();
01224 finalFrontier.header.frame_id = global_frame;
01225 finalFrontier.pose.position.x = wx;
01226 finalFrontier.pose.position.y = wy;
01227 finalFrontier.pose.position.z = 0.0;
01228
01229 double yaw = getYawToUnknown(costmap_->getIndex(mx,my));
01230
01231
01232
01233 finalFrontier.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
01234
01235 frontiers.push_back(finalFrontier);
01236 }
01237
01238 }
01239
01240 return (frontiers.size() > 0);
01241
01242
01243
01244
01245 int nextBlobValue = 1;
01246 std::list<int> usedBlobs;
01247
01248 for(unsigned int i = 0; i < allFrontiers.size(); ++i){
01249
01250
01251 int currentPoint = allFrontiers[i];
01252 int adjacentPoints[8];
01253 getAdjacentPoints(currentPoint,adjacentPoints);
01254
01255 std::list<int> blobs;
01256
01257 for(int j = 0; j < 8; j++){
01258 if(isValid(adjacentPoints[j]) && (frontier_map_array_[adjacentPoints[j]] > 0)){
01259 blobs.push_back(frontier_map_array_[adjacentPoints[j]]);
01260 }
01261 }
01262 blobs.unique();
01263
01264 if(blobs.empty()){
01265
01266 frontier_map_array_[currentPoint] = nextBlobValue;
01267 usedBlobs.push_back(nextBlobValue);
01268 nextBlobValue++;
01269 } else {
01270
01271 int blobMergeVal = 0;
01272
01273 for(std::list<int>::iterator adjBlob = blobs.begin(); adjBlob != blobs.end(); ++adjBlob){
01274 if(adjBlob == blobs.begin()){
01275 blobMergeVal = *adjBlob;
01276 frontier_map_array_[currentPoint] = blobMergeVal;
01277 } else {
01278
01279 for(unsigned int k = 0; k < allFrontiers.size(); k++){
01280 if(frontier_map_array_[allFrontiers[k]] == *adjBlob){
01281 usedBlobs.remove(*adjBlob);
01282 frontier_map_array_[allFrontiers[k]] = blobMergeVal;
01283 }
01284 }
01285 }
01286 }
01287 }
01288 }
01289
01290 int id = 1;
01291
01292 bool visualization_requested = (visualization_pub_.getNumSubscribers() > 0);
01293
01294
01295 for(std::list<int>::iterator currentBlob = usedBlobs.begin(); currentBlob != usedBlobs.end(); ++currentBlob){
01296 int current_frontier_size = 0;
01297 int max_obs_idx = 0;
01298
01299 for(unsigned int i = 0; i < allFrontiers.size(); ++i){
01300 int point = allFrontiers[i];
01301
01302 if(frontier_map_array_[point] == *currentBlob){
01303 current_frontier_size++;
01304 if(obstacle_trans_array_[point] > obstacle_trans_array_[allFrontiers[max_obs_idx]]){
01305 max_obs_idx = i;
01306 }
01307 }
01308 }
01309
01310 if(current_frontier_size < p_min_frontier_size_){
01311 continue;
01312 }
01313
01314 int frontier_point = allFrontiers[max_obs_idx];
01315 unsigned int x,y;
01316 costmap_->indexToCells(frontier_point,x,y);
01317
01318
01319 bool frontier_is_valid = true;
01320
01321 if(isFrontierReached(frontier_point)){
01322 frontier_is_valid = false;
01323 }
01324
01325 for(size_t i = 0; i < noFrontiers.size(); ++i){
01326 const geometry_msgs::PoseStamped& noFrontier = noFrontiers[i];
01327 unsigned int mx,my;
01328 costmap_->worldToMap(noFrontier.pose.position.x,noFrontier.pose.position.y,mx,my);
01329 int no_frontier_point = costmap_->getIndex(x,y);
01330 if(isSameFrontier(frontier_point,no_frontier_point)){
01331 frontier_is_valid = false;
01332 }
01333 }
01334
01335 geometry_msgs::PoseStamped finalFrontier;
01336 double wx,wy;
01337 costmap_->mapToWorld(x,y,wx,wy);
01338 std::string global_frame = costmap_ros_->getGlobalFrameID();
01339 finalFrontier.header.frame_id = global_frame;
01340 finalFrontier.pose.position.x = wx;
01341 finalFrontier.pose.position.y = wy;
01342 finalFrontier.pose.position.z = 0.0;
01343
01344 double yaw = getYawToUnknown(costmap_->getIndex(x,y));
01345
01346 if(frontier_is_valid){
01347
01348 finalFrontier.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
01349 frontiers.push_back(finalFrontier);
01350 }
01351
01352
01353 if(visualization_requested){
01354 visualization_msgs::Marker marker;
01355 marker.header.frame_id = "map";
01356 marker.header.stamp = ros::Time();
01357 marker.ns = "hector_exploration_planner";
01358 marker.id = id++;
01359 marker.type = visualization_msgs::Marker::ARROW;
01360 marker.action = visualization_msgs::Marker::ADD;
01361 marker.pose.position.x = wx;
01362 marker.pose.position.y = wy;
01363 marker.pose.position.z = 0.0;
01364 marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
01365 marker.scale.x = 0.2;
01366 marker.scale.y = 0.2;
01367 marker.scale.z = 0.2;
01368 marker.color.a = 1.0;
01369
01370 if(frontier_is_valid){
01371 marker.color.r = 0.0;
01372 marker.color.g = 1.0;
01373 }else{
01374 marker.color.r = 1.0;
01375 marker.color.g = 0.0;
01376 }
01377
01378 marker.color.b = 0.0;
01379 marker.lifetime = ros::Duration(5,0);
01380 visualization_pub_.publish(marker);
01381 }
01382
01383 }
01384 return !frontiers.empty();
01385 }
01386
01387 bool HectorExplorationPlanner::findInnerFrontier(std::vector<geometry_msgs::PoseStamped> &innerFrontier){
01388 clearFrontiers();
01389
01390
01391 hector_nav_msgs::GetRobotTrajectory srv_path;
01392 if (path_service_client_.call(srv_path)){
01393
01394 std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
01395
01396
01397 std::vector<geometry_msgs::PoseStamped> goals;
01398
01399 size_t size = traj_vector.size();
01400 ROS_DEBUG("[hector_exploration_planner] size of trajectory vector %u", (unsigned int)size);
01401
01402 if(size > 0){
01403 geometry_msgs::PoseStamped lastPose = traj_vector[size-1];
01404 goals.push_back(lastPose);
01405
01406 if (size > 1){
01407
01408
01409 bool collision_allowed = true;
01410
01411 for(int i = static_cast<int>(size-2); i >= 0; --i){
01412 const geometry_msgs::PoseStamped& pose = traj_vector[i];
01413 unsigned int x,y;
01414 costmap_->worldToMap(pose.pose.position.x,pose.pose.position.y,x,y);
01415 unsigned int m_point = costmap_->getIndex(x,y);
01416
01417 double dx = lastPose.pose.position.x - pose.pose.position.x;
01418 double dy = lastPose.pose.position.y - pose.pose.position.y;
01419
01420 bool point_in_free_space = isFreeFrontiers(m_point);
01421
01422
01423 if(point_in_free_space){
01424 if((dx*dx) + (dy*dy) > (0.25*0.25)){
01425 goals.push_back(pose);
01426 lastPose = pose;
01427 collision_allowed = false;
01428 }
01429 }
01430
01431 if (!point_in_free_space && !collision_allowed){
01432 break;
01433 }
01434 }
01435 }
01436
01437
01438 ROS_DEBUG("[hector_exploration_planner] pushed %u goals (trajectory) for inner frontier-search", (unsigned int)goals.size());
01439
01440
01441 tf::Stamped<tf::Pose> robotPose;
01442 if(!costmap_ros_->getRobotPose(robotPose)){
01443 ROS_WARN("[hector_exploration_planner]: Failed to get RobotPose");
01444 }
01445 geometry_msgs::PoseStamped robotPoseMsg;
01446 tf::poseStampedTFToMsg(robotPose, robotPoseMsg);
01447
01448 if (!buildexploration_trans_array_(robotPoseMsg, goals, false)){
01449 ROS_WARN("[hector_exploration_planner]: Creating exploration transform array in find inner frontier failed, aborting.");
01450 return false;
01451 }
01452
01453 inner_vis_->publishVisOnDemand(*costmap_, exploration_trans_array_.get());
01454
01455 unsigned int x,y;
01456 costmap_->worldToMap(robotPoseMsg.pose.position.x,robotPoseMsg.pose.position.y,x,y);
01457
01458
01459
01460
01461
01462 int max_exploration_trans_point = -1;
01463 unsigned int max_exploration_trans_val = 0;
01464
01465 for(unsigned int i = 0; i < num_map_cells_; ++i){
01466
01467 if(exploration_trans_array_[i] < UINT_MAX){
01468 if(exploration_trans_array_[i] > max_exploration_trans_val){
01469 if(!isFrontierReached(i)){
01470 max_exploration_trans_point = i;
01471 max_exploration_trans_val = exploration_trans_array_[i];
01472 }
01473 }
01474 }
01475 }
01476
01477 if (max_exploration_trans_point == 0){
01478 ROS_WARN("[hector_exploration_planner]: Couldn't find max exploration transform point for inner exploration, aborting.");
01479 return false;
01480 }
01481
01482 geometry_msgs::PoseStamped finalFrontier;
01483 unsigned int fx,fy;
01484 double wfx,wfy;
01485 costmap_->indexToCells(max_exploration_trans_point,fx,fy);
01486 costmap_->mapToWorld(fx,fy,wfx,wfy);
01487 std::string global_frame = costmap_ros_->getGlobalFrameID();
01488 finalFrontier.header.frame_id = global_frame;
01489 finalFrontier.pose.position.x = wfx;
01490 finalFrontier.pose.position.y = wfy;
01491 finalFrontier.pose.position.z = 0.0;
01492
01493
01494 int dx = fx-x;
01495 int dy = fy-y;
01496 double yaw_path = std::atan2(dy,dx);
01497 finalFrontier.pose.orientation.x = 0.0;
01498 finalFrontier.pose.orientation.y = 0.0;
01499 finalFrontier.pose.orientation.z = sin(yaw_path*0.5f);
01500 finalFrontier.pose.orientation.w = cos(yaw_path*0.5f);
01501
01502 innerFrontier.push_back(finalFrontier);
01503
01504 if(visualization_pub_.getNumSubscribers() > 0){
01505 visualization_msgs::Marker marker;
01506 marker.header.frame_id = "map";
01507 marker.header.stamp = ros::Time();
01508 marker.ns = "hector_exploration_planner";
01509 marker.id = 100;
01510 marker.type = visualization_msgs::Marker::ARROW;
01511 marker.action = visualization_msgs::Marker::ADD;
01512 marker.pose.position.x = wfx;
01513 marker.pose.position.y = wfy;
01514 marker.pose.position.z = 0.0;
01515 marker.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_path);
01516 marker.scale.x = 0.2;
01517 marker.scale.y = 0.2;
01518 marker.scale.z = 0.2;
01519 marker.color.a = 1.0;
01520 marker.color.r = 0.0;
01521 marker.color.g = 0.0;
01522 marker.color.b = 1.0;
01523 marker.lifetime = ros::Duration(5,0);
01524 visualization_pub_.publish(marker);
01525 }
01526 return true;
01527 }
01528 }
01529 return false;
01530 }
01531
01532
01533
01534
01535
01536
01537 bool HectorExplorationPlanner::isFrontier(int point){
01538 if(isFreeFrontiers(point)){
01539
01540 int adjacentPoints[8];
01541 getAdjacentPoints(point,adjacentPoints);
01542
01543 for(int i = 0; i < 8; ++i){
01544 if(isValid(adjacentPoints[i])){
01545 if(occupancy_grid_array_[adjacentPoints[i]] == costmap_2d::NO_INFORMATION){
01546
01547 int no_inf_count = 0;
01548 int noInfPoints[8];
01549 getAdjacentPoints(adjacentPoints[i],noInfPoints);
01550 for(int j = 0; j < 8; j++){
01551
01552
01553 if( isValid(noInfPoints[j]) && occupancy_grid_array_[noInfPoints[j]] == costmap_2d::NO_INFORMATION){
01554 ++no_inf_count;
01555
01556 if(no_inf_count > 2){
01557 return true;
01558 }
01559 }
01560 }
01561 }
01562 }
01563 }
01564 }
01565
01566 return false;
01567 }
01568
01569
01570 void HectorExplorationPlanner::resetMaps(){
01571 std::fill_n(exploration_trans_array_.get(), num_map_cells_, UINT_MAX);
01572 std::fill_n(obstacle_trans_array_.get(), num_map_cells_, UINT_MAX);
01573 std::fill_n(is_goal_array_.get(), num_map_cells_, false);
01574 }
01575
01576 void HectorExplorationPlanner::clearFrontiers(){
01577 std::fill_n(frontier_map_array_.get(), num_map_cells_, 0);
01578 }
01579
01580 inline bool HectorExplorationPlanner::isValid(int point){
01581 return (point>=0);
01582 }
01583
01584 bool HectorExplorationPlanner::isFree(int point){
01585
01586 if(isValid(point)){
01587
01588
01589
01590 if(p_use_inflated_obs_){
01591 if(occupancy_grid_array_[point] < costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
01592 return true;
01593 }
01594 } else {
01595 if(occupancy_grid_array_[point] <= costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
01596 return true;
01597 }
01598 }
01599
01600 if(p_plan_in_unknown_){
01601 if(occupancy_grid_array_[point] == costmap_2d::NO_INFORMATION){
01602 return true;
01603 }
01604 }
01605 }
01606 return false;
01607 }
01608
01609 bool HectorExplorationPlanner::isFreeFrontiers(int point){
01610
01611 if(isValid(point)){
01612
01613
01614
01615 if(p_use_inflated_obs_){
01616 if(occupancy_grid_array_[point] < costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
01617 return true;
01618 }
01619 } else {
01620 if(occupancy_grid_array_[point] <= costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
01621 return true;
01622 }
01623 }
01624 }
01625 return false;
01626 }
01627
01628 bool HectorExplorationPlanner::isFrontierReached(int point){
01629
01630 tf::Stamped<tf::Pose> robotPose;
01631 if(!costmap_ros_->getRobotPose(robotPose)){
01632 ROS_WARN("[hector_exploration_planner]: Failed to get RobotPose");
01633 }
01634 geometry_msgs::PoseStamped robotPoseMsg;
01635 tf::poseStampedTFToMsg(robotPose, robotPoseMsg);
01636
01637 unsigned int fx,fy;
01638 double wfx,wfy;
01639 costmap_->indexToCells(point,fx,fy);
01640 costmap_->mapToWorld(fx,fy,wfx,wfy);
01641
01642
01643 double dx = robotPoseMsg.pose.position.x - wfx;
01644 double dy = robotPoseMsg.pose.position.y - wfy;
01645
01646 if ( (dx*dx) + (dy*dy) < (p_dist_for_goal_reached_*p_dist_for_goal_reached_)) {
01647 ROS_DEBUG("[hector_exploration_planner]: frontier is within the squared range of: %f", p_dist_for_goal_reached_);
01648 return true;
01649 }
01650 return false;
01651
01652 }
01653
01654 bool HectorExplorationPlanner::isSameFrontier(int frontier_point1, int frontier_point2){
01655 unsigned int fx1,fy1;
01656 unsigned int fx2,fy2;
01657 double wfx1,wfy1;
01658 double wfx2,wfy2;
01659 costmap_->indexToCells(frontier_point1,fx1,fy1);
01660 costmap_->indexToCells(frontier_point2,fx2,fy2);
01661 costmap_->mapToWorld(fx1,fy1,wfx1,wfy1);
01662 costmap_->mapToWorld(fx2,fy2,wfx2,wfy2);
01663
01664 double dx = wfx1 - wfx2;
01665 double dy = wfy1 - wfy2;
01666
01667 if((dx*dx) + (dy*dy) < (p_same_frontier_dist_*p_same_frontier_dist_)){
01668 return true;
01669 }
01670 return false;
01671 }
01672
01673 inline unsigned int HectorExplorationPlanner::cellDanger(int point){
01674
01675 if ((int)obstacle_trans_array_[point] <= p_min_obstacle_dist_){
01676 return static_cast<unsigned int>(p_alpha_ * std::pow(p_min_obstacle_dist_ - obstacle_trans_array_[point], 2) + .5);
01677 }
01678
01679
01680
01681
01682
01683
01684 return 0;
01685 }
01686
01687 float HectorExplorationPlanner::angleDifference(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal){
01688
01689 unsigned int mxs,mys;
01690 costmap_->worldToMap(start.pose.position.x,start.pose.position.y,mxs,mys);
01691
01692 unsigned int gx,gy;
01693 costmap_->worldToMap(goal.pose.position.x,goal.pose.position.y,gx,gy);
01694
01695 int goal_proj_x = gx-mxs;
01696 int goal_proj_y = gy-mys;
01697
01698 float start_angle = tf::getYaw(start.pose.orientation);
01699 float goal_angle = std::atan2(goal_proj_y,goal_proj_x);
01700
01701 float both_angle = 0;
01702 if(start_angle > goal_angle){
01703 both_angle = start_angle - goal_angle;
01704 } else {
01705 both_angle = goal_angle - start_angle;
01706 }
01707
01708 if(both_angle > M_PI){
01709 both_angle = (M_PI - std::abs(start_angle)) + (M_PI - std::abs(goal_angle));
01710 }
01711
01712 return both_angle;
01713 }
01714
01715
01716 double HectorExplorationPlanner::getYawToUnknown(int point){
01717 int adjacentPoints[8];
01718 getAdjacentPoints(point,adjacentPoints);
01719
01720 int max_obs_idx = 0;
01721 unsigned int max_obs_dist = 0;
01722
01723 for(int i = 0; i < 8; ++i){
01724 if(isValid(adjacentPoints[i])){
01725 if(occupancy_grid_array_[adjacentPoints[i]] == costmap_2d::NO_INFORMATION){
01726 if(obstacle_trans_array_[adjacentPoints[i]] > max_obs_dist){
01727 max_obs_idx = i;
01728 max_obs_dist = obstacle_trans_array_[adjacentPoints[i]];
01729 }
01730 }
01731 }
01732 }
01733
01734 int orientationPoint = adjacentPoints[max_obs_idx];
01735 unsigned int sx,sy,gx,gy;
01736 costmap_->indexToCells((unsigned int)point,sx,sy);
01737 costmap_->indexToCells((unsigned int)orientationPoint,gx,gy);
01738 int x = gx-sx;
01739 int y = gy-sy;
01740 double yaw = std::atan2(y,x);
01741
01742 return yaw;
01743
01744 }
01745
01746 unsigned int HectorExplorationPlanner::angleDanger(float angle){
01747 float angle_fraction = std::pow(angle,3);
01748 unsigned int result = static_cast<unsigned int>(p_goal_angle_penalty_ * angle_fraction);
01749 return result;
01750 }
01751
01752 float HectorExplorationPlanner::getDistanceWeight(const geometry_msgs::PoseStamped &point1, const geometry_msgs::PoseStamped &point2){
01753 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));
01754 if(distance < 0.5){
01755 return 5.0;
01756 } else {
01757 return 1;
01758 }
01759 }
01760
01761
01762
01763
01764
01765 inline void HectorExplorationPlanner::getStraightPoints(int point, int points[]){
01766
01767 points[0] = left(point);
01768 points[1] = up(point);
01769 points[2] = right(point);
01770 points[3] = down(point);
01771
01772 }
01773
01774 inline void HectorExplorationPlanner::getDiagonalPoints(int point, int points[]){
01775
01776 points[0] = upleft(point);
01777 points[1] = upright(point);
01778 points[2] = downright(point);
01779 points[3] = downleft(point);
01780
01781 }
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799 inline void HectorExplorationPlanner::getAdjacentPoints(int point, int points[]){
01800
01801 points[0] = left(point);
01802 points[1] = up(point);
01803 points[2] = right(point);
01804 points[3] = down(point);
01805 points[4] = upleft(point);
01806 points[5] = upright(point);
01807 points[6] = downright(point);
01808 points[7] = downleft(point);
01809
01810 }
01811
01812 inline int HectorExplorationPlanner::left(int point){
01813
01814 if((point % map_width_ != 0)){
01815 return point-1;
01816 }
01817 return -1;
01818 }
01819 inline int HectorExplorationPlanner::upleft(int point){
01820 if((point % map_width_ != 0) && (point >= (int)map_width_)){
01821 return point-1-map_width_;
01822 }
01823 return -1;
01824
01825 }
01826 inline int HectorExplorationPlanner::up(int point){
01827 if(point >= (int)map_width_){
01828 return point-map_width_;
01829 }
01830 return -1;
01831 }
01832 inline int HectorExplorationPlanner::upright(int point){
01833 if((point >= (int)map_width_) && ((point + 1) % (int)map_width_ != 0)){
01834 return point-map_width_+1;
01835 }
01836 return -1;
01837 }
01838 inline int HectorExplorationPlanner::right(int point){
01839 if((point + 1) % map_width_ != 0){
01840 return point+1;
01841 }
01842 return -1;
01843
01844 }
01845 inline int HectorExplorationPlanner::downright(int point){
01846 if(((point + 1) % map_width_ != 0) && ((point/map_width_) < (map_height_-1))){
01847 return point+map_width_+1;
01848 }
01849 return -1;
01850
01851 }
01852 inline int HectorExplorationPlanner::down(int point){
01853 if((point/map_width_) < (map_height_-1)){
01854 return point+map_width_;
01855 }
01856 return -1;
01857
01858 }
01859 inline int HectorExplorationPlanner::downleft(int point){
01860 if(((point/map_width_) < (map_height_-1)) && (point % map_width_ != 0)){
01861 return point+map_width_-1;
01862 }
01863 return -1;
01864
01865 }
01866
01867
01868
01869
01870
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894
01895
01896
01897
01898
01899
01900
01901
01902
01903
01904
01905
01906
01907
01908
01909
01910
01911
01912
01913
01914
01915
01916
01917
01918
01919
01920
01921
01922
01923
01924
01925
01926
01927
01928
01929
01930
01931
01932
01933
01934
01935
01936
01937
01938
01939
01940
01941
01942
01943