00001 #include <ExplorationPlanner.h>
00002 #include "ros/ros.h"
00003 #include "hungarian.h"
00004 #include "munkres.h"
00005 #include "boost_matrix.h"
00006 #include <ros/console.h>
00007 #include <costmap_2d/costmap_2d_ros.h>
00008 #include <costmap_2d/costmap_2d.h>
00009 #include <navfn/navfn_ros.h>
00010 #include <visualization_msgs/Marker.h>
00011 #include <visualization_msgs/MarkerArray.h>
00012 #include <dynamic_reconfigure/server.h>
00013 #include <string.h>
00014 #include <stdlib.h>
00015 #include <geometry_msgs/PolygonStamped.h>
00016 #include <nav_msgs/GridCells.h>
00017 #include <adhoc_communication/ExpCluster.h>
00018 #include <adhoc_communication/ExpClusterElement.h>
00019 #include <adhoc_communication/ExpAuction.h>
00020 #include <adhoc_communication/ExpFrontier.h>
00021 #include <adhoc_communication/ExpFrontierElement.h>
00022 #include <adhoc_communication/SendExpFrontier.h>
00023 #include <adhoc_communication/SendExpAuction.h>
00024 #include <adhoc_communication/SendExpCluster.h>
00025 #include <boost/numeric/ublas/matrix.hpp>
00026 #include <boost/numeric/ublas/io.hpp>
00027
00028
00029 #include <adhoc_communication/MmListOfPoints.h>
00030 #include <map_merger/TransformPoint.h>
00031 #include <base_local_planner/trajectory_planner_ros.h>
00032 #include <math.h>
00033
00034 #define MAX_DISTANCE 2000 // max distance to starting point
00035 #define MAX_GOAL_RANGE 0.2 // min distance between frontiers (search)
00036 #define MINIMAL_FRONTIER_RANGE 0.2 // distance between frontiers (selection)
00037 #define INNER_DISTANCE 5 // radius (in cells) around goal point without obstacles (backoff goal point)
00038 #define MAX_NEIGHBOR_DIST 1 // radius (in cells) around selected goal without obstacles
00039 #define CLUSTER_MERGING_DIST 0.8 // max (euclidean) distance between clusters that are merged
00040
00041 using namespace explorationPlanner;
00042
00043 template <typename T>
00044 std::string NumberToString ( T Number )
00045 {
00046 std::ostringstream ss;
00047 ss << Number;
00048 return ss.str();
00049 }
00050
00051 ExplorationPlanner::ExplorationPlanner(int robot_id, bool robot_prefix_empty, std::string robot_name_parameter):
00052 costmap_ros_(0), occupancy_grid_array_(0), exploration_trans_array_(0), obstacle_trans_array_(
00053 0), frontier_map_array_(0), is_goal_array_(0), map_width_(0), map_height_(
00054 0), num_map_cells_(0), initialized_(false), last_mode_(
00055 FRONTIER_EXPLORE), p_alpha_(0), p_dist_for_goal_reached_(1), p_goal_angle_penalty_(
00056 0), p_min_frontier_size_(0), p_min_obstacle_dist_(0), p_plan_in_unknown_(
00057 true), p_same_frontier_dist_(0), p_use_inflated_obs_(false), previous_goal_(
00058 0), inflated(0), lethal(0), free(0), threshold_free(127)
00059 , threshold_inflated(252), threshold_lethal(253),frontier_id_count(0),
00060 exploration_travel_path_global(0), cluster_id(0), initialized_planner(false),
00061 auction_is_running(false), auction_start(false), auction_finished(true),
00062 start_thr_auction(false), auction_id_number(1), next_auction_position_x(0),
00063 next_auction_position_y(0), other_robots_position_x(0), other_robots_position_y(0),
00064 number_of_completed_auctions(0), number_of_uncompleted_auctions(0), first_run(true),
00065 first_negotiation_run(true), robot_prefix_empty_param(false){
00066
00067
00068 trajectory_strategy = "euclidean";
00069 robot_prefix_empty_param = robot_prefix_empty;
00070
00071
00072 responded_t init_responded;
00073 init_responded.auction_number = 0;
00074 init_responded.robot_number = 0;
00075 robots_already_responded.push_back(init_responded);
00076
00077 auction_running = false;
00078 std::stringstream robot_number;
00079 robot_number << robot_id;
00080
00081 std::string prefix = "/robot_";
00082 std::string robo_name = prefix.append(robot_number.str());
00083
00084 if(robot_prefix_empty_param == true)
00085 {
00086
00087 robo_name = "";
00088 robot_str = robot_name_parameter;
00089 }
00090 std::string sendFrontier_msgs = robo_name +"/adhoc_communication/send_frontier";
00091 std::string sendAuction_msgs = robo_name +"/adhoc_communication/send_auction";
00092
00093 ros::NodeHandle tmp;
00094 nh_service = &tmp;
00095
00096 ROS_DEBUG("Sending frontier: '%s' SendAuction: '%s'", sendFrontier_msgs.c_str(), sendAuction_msgs.c_str());
00097
00098 ssendFrontier = nh_service->serviceClient<adhoc_communication::SendExpFrontier>(sendFrontier_msgs);
00099 ssendAuction = nh_service->serviceClient<adhoc_communication::SendExpAuction>(sendAuction_msgs);
00100
00101
00102
00103
00104
00105
00106
00107 pub_visited_frontiers_points = nh_visited_Point.advertise <visualization_msgs::MarkerArray> ("visitedfrontierPoints", 2000, true);
00108
00109 pub_Point = nh_Point.advertise < geometry_msgs::PointStamped> ("goalPoint", 100, true);
00110 pub_frontiers_points = nh_frontiers_points.advertise <visualization_msgs::MarkerArray> ("frontierPoints", 2000, true);
00111
00112
00113
00114
00115
00116
00117
00118
00119 pub_cluster_grid_0 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_0", 2000, true);
00120 pub_cluster_grid_1 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_1", 2000, true);
00121 pub_cluster_grid_2 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_2", 2000, true);
00122 pub_cluster_grid_3 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_3", 2000, true);
00123 pub_cluster_grid_4 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_4", 2000, true);
00124 pub_cluster_grid_5 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_5", 2000, true);
00125 pub_cluster_grid_6 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_6", 2000, true);
00126 pub_cluster_grid_7 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_7", 2000, true);
00127 pub_cluster_grid_8 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_8", 2000, true);
00128 pub_cluster_grid_9 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_9", 2000, true);
00129 pub_cluster_grid_10 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_10", 2000, true);
00130 pub_cluster_grid_11 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_11", 2000, true);
00131 pub_cluster_grid_12 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_12", 2000, true);
00132 pub_cluster_grid_13 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_13", 2000, true);
00133 pub_cluster_grid_14 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_14", 2000, true);
00134 pub_cluster_grid_15 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_15", 2000, true);
00135 pub_cluster_grid_16 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_16", 2000, true);
00136 pub_cluster_grid_17 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_17", 2000, true);
00137 pub_cluster_grid_18 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_18", 2000, true);
00138 pub_cluster_grid_19 = nh_cluster_grid.advertise<nav_msgs::GridCells>("cluster_grid_19", 2000, true);
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 sub_frontiers = nh_frontier.subscribe(robo_name+"/frontiers", 10000, &ExplorationPlanner::frontierCallback, this);
00153 sub_visited_frontiers = nh_visited_frontier.subscribe(robo_name+"/visited_frontiers", 10000, &ExplorationPlanner::visited_frontierCallback, this);
00154 sub_negotioation = nh_negotiation.subscribe(robo_name+"/negotiation_list", 10000, &ExplorationPlanner::negotiationCallback, this);
00155 sub_auctioning = nh_auction.subscribe(robo_name+"/auction", 1000, &ExplorationPlanner::auctionCallback, this);
00156 sub_position = nh_position.subscribe(robo_name+"/all_positions", 1000, &ExplorationPlanner::positionCallback, this);
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194 srand((unsigned)time(0));
00195 }
00196
00197 void ExplorationPlanner::Callbacks()
00198 {
00199 ros::Rate r(10);
00200 while(ros::ok())
00201 {
00202
00203
00204 if(robot_name == 1)
00205 {
00206 sub_frontiers = nh_frontier.subscribe("/robot_0/frontiers", 10000, &ExplorationPlanner::frontierCallback, this);
00207 sub_visited_frontiers = nh_visited_frontier.subscribe("/robot_0/visited_frontiers", 10000, &ExplorationPlanner::visited_frontierCallback, this);
00208 sub_negotioation = nh_negotiation.subscribe("/robot_0/negotiation_list", 10000, &ExplorationPlanner::negotiationCallback, this);
00209 sub_auctioning = nh_auction.subscribe("/robot_1/auction", 1000, &ExplorationPlanner::auctionCallback, this);
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219 }
00220 else if(robot_name == 0)
00221 {
00222 sub_frontiers = nh_frontier.subscribe("/robot_1/frontiers", 10000, &ExplorationPlanner::frontierCallback, this);
00223 sub_visited_frontiers = nh_visited_frontier.subscribe("/robot_1/visited_frontiers", 10000, &ExplorationPlanner::visited_frontierCallback, this);
00224 sub_negotioation = nh_negotiation.subscribe("/robot_1/negotiation_list", 10000, &ExplorationPlanner::negotiationCallback, this);
00225 sub_auctioning = nh_auction.subscribe("/robot_0/auction", 1000, &ExplorationPlanner::auctionCallback, this);
00226
00227
00228
00229
00230
00231
00232
00233
00234 }
00235
00236
00237 r.sleep();
00238
00239 }
00240 }
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256 void ExplorationPlanner::initialize_planner(std::string name,
00257 costmap_2d::Costmap2DROS *costmap, costmap_2d::Costmap2DROS *costmap_global) {
00258
00259 ROS_INFO("Initializing the planner");
00260
00261
00262
00263 this->costmap_ros_ = costmap;
00264 this->costmap_global_ros_ = costmap_global;
00265
00266 if(initialized_planner == false)
00267 {
00268 nav.initialize("navigation_path", costmap_global_ros_);
00269 initialized_planner = true;
00270 }
00271
00272 this->setupMapData();
00273
00274 last_mode_ = FRONTIER_EXPLORE;
00275 this->initialized_ = true;
00276
00277
00278
00279
00280
00281 unknown = 0, free = 0, lethal = 0, inflated = 0;
00282
00283 }
00284
00285
00286 bool ExplorationPlanner::clusterFrontiers()
00287 {
00288
00289
00290 int strategy = 1;
00291
00292
00293
00294
00295
00296
00297 bool cluster_found_flag = false, same_id = false;
00298
00299 for(int i = 0; i < frontiers.size(); i++)
00300 {
00301 ROS_DEBUG("Frontier at: %d and cluster size: %lu",i, clusters.size());
00302 cluster_found_flag = false;
00303 bool frontier_used = false;
00304 same_id = false;
00305
00306 for(int j = 0; j < clusters.size(); j++)
00307 {
00308 ROS_DEBUG("cluster %d contains %lu elements", j, clusters.at(j).cluster_element.size());
00309 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
00310 {
00311 ROS_DEBUG("accessing cluster %d and element: %d", j, n);
00312
00313 if(fabs(frontiers.at(i).x_coordinate - clusters.at(j).cluster_element.at(n).x_coordinate) < MAX_NEIGHBOR_DIST && fabs(frontiers.at(i).y_coordinate - clusters.at(j).cluster_element.at(n).y_coordinate) < MAX_NEIGHBOR_DIST)
00314 {
00315 for(int m = 0; m < clusters.at(j).cluster_element.size(); m++)
00316 {
00317 ROS_DEBUG("checking id %d with element id: %d",frontiers.at(i).id,clusters.at(j).cluster_element.at(m).id);
00318
00319 if(robot_prefix_empty_param == true)
00320 {
00321 if(frontiers.at(i).id == clusters.at(j).cluster_element.at(m).id && frontiers.at(i).detected_by_robot_str.compare(clusters.at(j).cluster_element.at(m).detected_by_robot_str) == 0)
00322 {
00323 ROS_DEBUG("SAME ID FOUND !!!!!!!");
00324 frontier_used = true;
00325 same_id = true;
00326 break;
00327 }
00328 }else
00329 {
00330 if(frontiers.at(i).id == clusters.at(j).cluster_element.at(m).id)
00331 {
00332 ROS_DEBUG("SAME ID FOUND !!!!!!!");
00333 frontier_used = true;
00334 same_id = true;
00335 break;
00336 }
00337 }
00338
00339
00340 }
00341 if(same_id == false)
00342 {
00343 cluster_found_flag = true;
00344 }
00345 }
00346 if(same_id == true || cluster_found_flag == true)
00347 {
00348 break;
00349 }
00350 }
00351 if(same_id == true)
00352 {
00353 break;
00354 }else
00355 {
00356 if(cluster_found_flag == true)
00357 {
00358 ROS_DEBUG("Frontier: %d attached", frontiers.at(i).id);
00359 clusters.at(j).cluster_element.push_back(frontiers.at(i));
00360 frontier_used = true;
00361 break;
00362 }
00363 }
00364 }
00365 if(cluster_found_flag == false && same_id == false)
00366 {
00367 ROS_DEBUG("ADD CLUSTER");
00368 cluster_t cluster_new;
00369 cluster_new.cluster_element.push_back(frontiers.at(i));
00370 cluster_new.id = (robot_name * 10000) + cluster_id++;
00371
00372 cluster_mutex.lock();
00373 clusters.push_back(cluster_new);
00374 cluster_mutex.unlock();
00375
00376 ROS_DEBUG("Frontier: %d in new cluster", frontiers.at(i).id);
00377 frontier_used = true;
00378 }
00379 if(frontier_used == false)
00380 {
00381 ROS_WARN("Frontier: %d not used", frontiers.at(i).id);
00382 }
00383 }
00384
00385
00386
00387
00388
00389
00390 bool run_without_merging = false;
00391
00392 if(strategy == 1)
00393 {
00394 while(run_without_merging == false)
00395 {
00396 bool merge_clusters = false;
00397 for(int i = 0; i < clusters.size(); i++)
00398 {
00399 for(int m = 0; m < clusters.at(i).cluster_element.size(); m ++)
00400 {
00401 if(clusters.at(i).cluster_element.size() > 1)
00402 {
00403 for(int j = 0; j < clusters.size(); j++)
00404 {
00405 if(clusters.at(i).id != clusters.at(j).id)
00406 {
00407 if(clusters.at(j).cluster_element.size() > 1)
00408 {
00409 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
00410 {
00411 if(fabs(clusters.at(i).cluster_element.at(m).x_coordinate - clusters.at(j).cluster_element.at(n).x_coordinate) < CLUSTER_MERGING_DIST && fabs(clusters.at(i).cluster_element.at(m).y_coordinate - clusters.at(j).cluster_element.at(n).y_coordinate) < CLUSTER_MERGING_DIST)
00412 {
00413
00414 merge_clusters = true;
00415 break;
00416 }
00417 }
00418 if(merge_clusters == true)
00419 {
00420
00421
00422
00423
00424 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
00425 {
00426 frontier_t frontier_to_merge;
00427 frontier_to_merge = clusters.at(j).cluster_element.at(n);
00428 clusters.at(i).cluster_element.push_back(frontier_to_merge);
00429 }
00430
00431 clusters.erase(clusters.begin() + j);
00432 break;
00433 }
00434 }
00435 }
00436 }
00437 }
00438 if(merge_clusters == true)
00439 break;
00440 }
00441 if(merge_clusters == true)
00442 break;
00443 }
00444 if(merge_clusters == false)
00445 {
00446 run_without_merging = true;
00447 }
00448
00449 }
00450 }
00451 else if(strategy == 2)
00452 {
00453 int MAX_NEIGHBOURS = 4;
00454 double costmap_resolution = costmap_ros_->getCostmap()->getResolution();
00455
00456 while(run_without_merging == false)
00457 {
00458 bool merge_clusters = false;
00459 for(int i = 0; i < clusters.size(); i++)
00460 {
00461 for(int m = 0; m < clusters.at(i).cluster_element.size(); m ++)
00462 {
00463 if(clusters.at(i).cluster_element.size() > 1)
00464 {
00465 for(int j = 0; j < clusters.size(); j++)
00466 {
00467 if(clusters.at(i).id != clusters.at(j).id)
00468 {
00469 if(clusters.at(j).cluster_element.size() > 1)
00470 {
00471 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
00472 {
00473 unsigned char cost;
00474 unsigned int mx,my;
00475 int freespace_detected = 0;
00476 int obstacle_detected = 0;
00477 int elements_detected = 0;
00478
00479 ROS_INFO("Calculating ...");
00480 int x_length = abs(clusters.at(i).cluster_element.at(m).x_coordinate - clusters.at(j).cluster_element.at(n).x_coordinate);
00481 int y_length = abs(clusters.at(i).cluster_element.at(m).y_coordinate - clusters.at(j).cluster_element.at(n).y_coordinate);
00482
00483 ROS_INFO("x_length: %d y_length: %d resolution: %f", x_length, y_length,costmap_resolution);
00484 int x_elements = abs(x_length / costmap_resolution);
00485 int y_elements = abs(y_length / costmap_resolution);
00486
00487 ROS_INFO("alpha");
00488 double alpha;
00489 if(x_length == 0)
00490 {
00491 alpha = 0;
00492 }
00493 else
00494 {
00495 alpha = atan(y_length/x_length);
00496 }
00497
00498 ROS_INFO("atan: %f x_length: %d y_length: %d", alpha, x_length, y_length);
00499
00500 for(int x = 0; x <= x_elements; x++)
00501 {
00502
00503 for(int neighbour = 0; neighbour < MAX_NEIGHBOURS; neighbour++)
00504 {
00505 int y = tan(alpha) * x * costmap_ros_->getCostmap()->getResolution();
00506 ROS_INFO("x: %d y: %d", x, y);
00507 if(!costmap_global_ros_->getCostmap()->worldToMap(clusters.at(i).cluster_element.at(m).x_coordinate + x, clusters.at(i).cluster_element.at(m).y_coordinate + y + neighbour,mx,my))
00508 {
00509 ROS_ERROR("Cannot convert coordinates successfully.");
00510 continue;
00511 }
00512 cost = costmap_global_ros_->getCostmap()->getCost(mx, my);
00513
00514 if(cost == costmap_2d::FREE_SPACE)
00515 {
00516 freespace_detected = true;
00517 }
00518 else if(cost == costmap_2d::LETHAL_OBSTACLE)
00519 {
00520 obstacle_detected = true;
00521 }
00522 elements_detected++;
00523 }
00524
00525
00526 for(int neighbour = 0; neighbour < MAX_NEIGHBOURS; neighbour++)
00527 {
00528 int y = tan(alpha) * x * costmap_ros_->getCostmap()->getResolution();
00529 ROS_INFO("x: %d y: %d", x, y);
00530 if(!costmap_global_ros_->getCostmap()->worldToMap(clusters.at(i).cluster_element.at(m).x_coordinate + x, clusters.at(i).cluster_element.at(m).y_coordinate + y - neighbour,mx,my))
00531 {
00532 ROS_ERROR("Cannot convert coordinates successfully.");
00533 continue;
00534 }
00535 cost = costmap_global_ros_->getCostmap()->getCost(mx, my);
00536
00537 if(cost == costmap_2d::FREE_SPACE)
00538 {
00539 freespace_detected = true;
00540 }
00541 else if(cost == costmap_2d::LETHAL_OBSTACLE)
00542 {
00543 obstacle_detected = true;
00544 }
00545 elements_detected++;
00546 }
00547 }
00548
00549
00550 ROS_INFO("*******************************");
00551 ROS_INFO("elements: %d obstacle: %d", elements_detected, obstacle_detected);
00552 ROS_INFO("*******************************");
00553 if(elements_detected * 0.25 < obstacle_detected)
00554 {
00555 ROS_INFO("Merging clusters");
00556 merge_clusters = true;
00557 break;
00558 }
00559 break;
00560
00561
00562
00563
00564
00565
00566 }
00567 if(merge_clusters == true)
00568 {
00569 ROS_INFO("Merging");
00570
00571
00572
00573
00574 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
00575 {
00576 frontier_t frontier_to_merge;
00577 frontier_to_merge = clusters.at(j).cluster_element.at(n);
00578 clusters.at(i).cluster_element.push_back(frontier_to_merge);
00579 }
00580
00581 clusters.erase(clusters.begin() + j);
00582 j--;
00583 ROS_INFO("Done merging");
00584 break;
00585 }
00586 }
00587 }
00588 break;
00589 }
00590 }
00591 if(merge_clusters == true)
00592 break;
00593 }
00594 if(merge_clusters == true)
00595 break;
00596 }
00597 if(merge_clusters == false)
00598 {
00599 run_without_merging = true;
00600 }
00601
00602 }
00603 }
00604 }
00605
00606 void ExplorationPlanner::visualizeClustersConsole()
00607 {
00608 ROS_INFO("------------------------------------------------------------------");
00609 for(int j = 0; j < clusters.size(); j++)
00610 {
00611 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
00612 {
00613 if(robot_prefix_empty_param == true)
00614 {
00615 ROS_INFO("ID: %6d x: %5.2f y: %5.2f cluster: %5d robot: %s", clusters.at(j).cluster_element.at(n).id, clusters.at(j).cluster_element.at(n).x_coordinate, clusters.at(j).cluster_element.at(n).y_coordinate, clusters.at(j).id, clusters.at(j).cluster_element.at(n).detected_by_robot_str.c_str());
00616 }else
00617 {
00618 ROS_INFO("ID: %6d x: %5.2f y: %5.2f cluster: %5d dist: %d", clusters.at(j).cluster_element.at(n).id, clusters.at(j).cluster_element.at(n).x_coordinate, clusters.at(j).cluster_element.at(n).y_coordinate, clusters.at(j).id, clusters.at(j).cluster_element.at(n).dist_to_robot);
00619 }
00620 }
00621 }
00622 ROS_INFO("------------------------------------------------------------------");
00623 }
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639 bool ExplorationPlanner::transformToOwnCoordinates_frontiers()
00640 {
00641 ROS_INFO("Transform frontier coordinates");
00642
00643 store_frontier_mutex.lock();
00644
00645 for(int i = 0; i < frontiers.size(); i++)
00646 {
00647 bool same_robot = false;
00648 if(robot_prefix_empty_param == true)
00649 {
00650 if(frontiers.at(i).detected_by_robot_str.compare(robot_str) == 0)
00651 same_robot = true;
00652
00653 }else
00654 {
00655 if(frontiers.at(i).detected_by_robot == robot_name)
00656 same_robot = true;
00657 }
00658 if(same_robot == false)
00659 {
00660
00661 bool transform_flag = false;
00662 for(int j=0; j < transformedPointsFromOtherRobot_frontiers.size(); j++)
00663 {
00664 if(robot_prefix_empty_param == 0)
00665 {
00666 if(transformedPointsFromOtherRobot_frontiers.at(j).id == frontiers.at(i).id && frontiers.at(i).detected_by_robot_str.compare(transformedPointsFromOtherRobot_frontiers.at(j).robot_str)== 0)
00667 {
00668 transform_flag = true;
00669 break;
00670 }
00671 }else
00672 {
00673 if(transformedPointsFromOtherRobot_frontiers.at(j).id == frontiers.at(i).id)
00674 {
00675 transform_flag = true;
00676 break;
00677 }
00678 }
00679 }
00680
00681 if(transform_flag != true)
00682 {
00683 std::string robo_name, robo_name2;
00684
00685 if(robot_prefix_empty_param == false)
00686 {
00687 std::stringstream robot_number;
00688 robot_number << frontiers.at(i).detected_by_robot;
00689
00690 std::string prefix = "robot_";
00691 robo_name = prefix.append(robot_number.str());
00692 ROS_DEBUG("Robots name is %s", robo_name.c_str());
00693
00694 std::stringstream robot_number2;
00695 robot_number2 << robot_name;
00696
00697 std::string prefix2 = "/robot_";
00698 robo_name2 = prefix2.append(robot_number2.str());
00699 }
00700 else
00701 {
00702
00703
00704
00705 robo_name = frontiers.at(i).detected_by_robot_str;
00706 robo_name2 = robot_str;
00707 ROS_DEBUG("Robot: %s transforms from robot: %s", robo_name2.c_str(), robo_name.c_str());
00708 }
00709
00710
00711
00712 std::string service_topic = robo_name2.append("/map_merger/transformPoint");
00713
00714
00715
00716 client = nh_transform.serviceClient<map_merger::TransformPoint>(service_topic);
00717
00718 service_message.request.point.x = frontiers.at(i).x_coordinate;
00719 service_message.request.point.y = frontiers.at(i).y_coordinate;
00720 service_message.request.point.src_robot = robo_name;
00721
00722 ROS_DEBUG("Robot name: %s", service_message.request.point.src_robot.c_str());
00723 ROS_DEBUG("Old x: %f y: %f", frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate);
00724
00725 if(client.call(service_message))
00726 {
00727 frontiers.at(i).x_coordinate = service_message.response.point.x;
00728 frontiers.at(i).y_coordinate = service_message.response.point.y;
00729
00730 transform_point_t transform_point;
00731 transform_point.id = frontiers.at(i).id;
00732
00733 if(robot_prefix_empty_param == true)
00734 {
00735 transform_point.robot_str = frontiers.at(i).detected_by_robot_str;
00736 }
00737 transformedPointsFromOtherRobot_frontiers.push_back(transform_point);
00738 ROS_DEBUG("New x: %.1f y: %.1f",service_message.response.point.x, service_message.response.point.y);
00739 }
00740 }
00741 }
00742 }
00743 store_frontier_mutex.unlock();
00744 ROS_INFO(" Transform frontier coordinates DONE");
00745 }
00746
00747
00748 bool ExplorationPlanner::transformToOwnCoordinates_visited_frontiers()
00749 {
00750 ROS_INFO("Transform Visited Frontier Coordinates");
00751
00752 for(int i = 0; i < visited_frontiers.size(); i++)
00753 {
00754 bool same_robot = false;
00755 if(robot_prefix_empty_param == true)
00756 {
00757 if(visited_frontiers.at(i).detected_by_robot_str.compare(robot_str) == 0)
00758 same_robot = true;
00759
00760 }else
00761 {
00762 if(visited_frontiers.at(i).detected_by_robot == robot_name)
00763 same_robot = true;
00764 }
00765 if(same_robot == false)
00766 {
00767 bool transform_flag = false;
00768 for(int j=0; j < transformedPointsFromOtherRobot_visited_frontiers.size(); j++)
00769 {
00770 if(robot_prefix_empty_param == 0)
00771 {
00772 if(transformedPointsFromOtherRobot_visited_frontiers.at(j).id == visited_frontiers.at(i).id && visited_frontiers.at(i).detected_by_robot_str.compare(transformedPointsFromOtherRobot_visited_frontiers.at(j).robot_str)== 0)
00773 {
00774 transform_flag = true;
00775 break;
00776 }
00777 }else
00778 {
00779 if(transformedPointsFromOtherRobot_visited_frontiers.at(j).id == visited_frontiers.at(i).id)
00780 {
00781 transform_flag = true;
00782 break;
00783 }
00784 }
00785 }
00786
00787 if(transform_flag != true)
00788 {
00789
00790 std::string robo_name, robo_name2;
00791
00792 if(robot_prefix_empty_param == false)
00793 {
00794 std::stringstream robot_number;
00795 robot_number << visited_frontiers.at(i).detected_by_robot;
00796
00797 std::string prefix = "robot_";
00798 robo_name = prefix.append(robot_number.str());
00799 ROS_DEBUG("Robots name is %s", robo_name.c_str());
00800
00801 std::stringstream robot_number2;
00802 robot_number2 << robot_name;
00803
00804 std::string prefix2 = "/robot_";
00805 robo_name2 = prefix2.append(robot_number2.str());
00806 }
00807 else
00808 {
00809
00810
00811
00812 robo_name = visited_frontiers.at(i).detected_by_robot_str;
00813 robo_name2 = robot_str;
00814 ROS_DEBUG("Robot: %s transforms from robot: %s", robo_name2.c_str(), robo_name.c_str());
00815 }
00816
00817
00818 std::string service_topic = robo_name2.append("/map_merger/transformPoint");
00819
00820 client = nh_transform.serviceClient<map_merger::TransformPoint>(service_topic);
00821
00822 service_message.request.point.x = visited_frontiers.at(i).x_coordinate;
00823 service_message.request.point.y = visited_frontiers.at(i).y_coordinate;
00824 service_message.request.point.src_robot = robo_name;
00825
00826 ROS_DEBUG("Old visited x: %f y: %f", visited_frontiers.at(i).x_coordinate, visited_frontiers.at(i).y_coordinate);
00827
00828 if(client.call(service_message))
00829 {
00830 visited_frontiers.at(i).x_coordinate = service_message.response.point.x;
00831 visited_frontiers.at(i).y_coordinate = service_message.response.point.y;
00832
00833 transform_point_t transform_point;
00834 transform_point.id = visited_frontiers.at(i).id;
00835
00836 if(robot_prefix_empty_param == true)
00837 {
00838 transform_point.robot_str = visited_frontiers.at(i).detected_by_robot_str;
00839 }
00840 transformedPointsFromOtherRobot_visited_frontiers.push_back(transform_point);
00841 ROS_DEBUG("New visited x: %.1f y: %.1f",service_message.response.point.x, service_message.response.point.y);
00842 }
00843 }
00844 }
00845 }
00846 ROS_INFO(" Transform visited frontier coordinates DONE");
00847 }
00848
00849
00850
00851 bool ExplorationPlanner::check_trajectory_plan()
00852 {
00853 geometry_msgs::PoseStamped goalPointSimulated, startPointSimulated;
00854 int distance;
00855
00856 ROS_INFO("Check Trajectory Length");
00857 if (!costmap_global_ros_->getRobotPose(robotPose))
00858 {
00859 ROS_ERROR("Failed to get RobotPose");
00860 }
00861
00862 startPointSimulated.header.seq = start_point_simulated_message++;
00863 startPointSimulated.header.stamp = ros::Time::now();
00864 startPointSimulated.header.frame_id = move_base_frame;
00865 startPointSimulated.pose.position.x = robotPose.getOrigin().getX();
00866 startPointSimulated.pose.position.y = robotPose.getOrigin().getY();
00867 startPointSimulated.pose.position.z = 0;
00868 startPointSimulated.pose.orientation.x = robotPose.getRotation().getX();
00869 startPointSimulated.pose.orientation.y = robotPose.getRotation().getY();
00870 startPointSimulated.pose.orientation.z = robotPose.getRotation().getZ();
00871 startPointSimulated.pose.orientation.w = 1;
00872
00873 for(int i = 0; i<10; i++)
00874 {
00875 if(frontiers.size() > i)
00876 {
00877 std::vector<double> backoffGoal;
00878 bool backoff_flag = smartGoalBackoff(frontiers.at(i).x_coordinate,frontiers.at(i).y_coordinate, costmap_global_ros_, &backoffGoal);
00879
00880
00881 goalPointSimulated.header.seq = goal_point_simulated_message++;
00882 goalPointSimulated.header.stamp = ros::Time::now();
00883 goalPointSimulated.header.frame_id = move_base_frame;
00884
00885
00886 if(backoff_flag == true)
00887 {
00888 goalPointSimulated.pose.position.x = backoffGoal.at(0);
00889 goalPointSimulated.pose.position.y = backoffGoal.at(1);
00890 }else
00891 {
00892 goalPointSimulated.pose.position.x = frontiers.at(i).x_coordinate;
00893 goalPointSimulated.pose.position.y = frontiers.at(i).y_coordinate;
00894 }
00895 goalPointSimulated.pose.position.z = 0;
00896 goalPointSimulated.pose.orientation.x = 0;
00897 goalPointSimulated.pose.orientation.y = 0;
00898 goalPointSimulated.pose.orientation.z = 0;
00899 goalPointSimulated.pose.orientation.w = 1;
00900
00901 std::vector<geometry_msgs::PoseStamped> global_plan;
00902
00903 nav.makePlan(startPointSimulated, goalPointSimulated, global_plan);
00904 distance = global_plan.size();
00905 frontiers.at(i).distance_to_robot = distance;
00906
00907 ROS_DEBUG("Distance: %d Frontier: %d",distance, frontiers.at(i).id);
00908 }else
00909 {
00910 break;
00911 }
00912 }
00913 ROS_DEBUG("trajectory finished");
00914 return(true);
00915 }
00916
00917
00918 int ExplorationPlanner::calculate_travel_path(double x, double y)
00919 {
00920 geometry_msgs::PoseStamped goalPointSimulated, startPointSimulated;
00921 double distance;
00922
00923 ROS_DEBUG("Check Trajectory");
00924 if (!costmap_global_ros_->getRobotPose(robotPose))
00925 {
00926 ROS_ERROR("Failed to get RobotPose");
00927 }
00928
00929 std::vector<double> backoffGoal;
00930 bool backoff_flag = smartGoalBackoff(x,y, costmap_global_ros_, &backoffGoal);
00931
00932 startPointSimulated.header.seq = start_point_simulated_message++;
00933 startPointSimulated.header.stamp = ros::Time::now();
00934 startPointSimulated.header.frame_id = move_base_frame;
00935 startPointSimulated.pose.position.x = robotPose.getOrigin().getX();
00936 startPointSimulated.pose.position.y = robotPose.getOrigin().getY();
00937 startPointSimulated.pose.position.z = 0;
00938 startPointSimulated.pose.orientation.x = 0;
00939 startPointSimulated.pose.orientation.y = 0;
00940 startPointSimulated.pose.orientation.z = 0;
00941 startPointSimulated.pose.orientation.w = 1;
00942
00943
00944 goalPointSimulated.header.seq = goal_point_simulated_message++;
00945 goalPointSimulated.header.stamp = ros::Time::now();
00946 goalPointSimulated.header.frame_id = move_base_frame;
00947
00948
00949 if(backoff_flag == true)
00950 {
00951 goalPointSimulated.pose.position.x = backoffGoal.at(0);
00952 goalPointSimulated.pose.position.y = backoffGoal.at(1);
00953 }else
00954 {
00955 goalPointSimulated.pose.position.x = x;
00956 goalPointSimulated.pose.position.y = y;
00957 }
00958 goalPointSimulated.pose.position.z = 0;
00959 goalPointSimulated.pose.orientation.x = 0;
00960 goalPointSimulated.pose.orientation.y = 0;
00961 goalPointSimulated.pose.orientation.z = 0;
00962 goalPointSimulated.pose.orientation.w = 1;
00963
00964 std::vector<geometry_msgs::PoseStamped> global_plan;
00965
00966 bool successful = nav.makePlan(startPointSimulated, goalPointSimulated, global_plan);
00967
00968 ROS_INFO("Plan elements: %f", (double)global_plan.size()*0.02);
00969
00970 if(successful == true)
00971 {
00972 distance = global_plan.size();
00973 exploration_travel_path_global = exploration_travel_path_global + distance;
00974 return distance;
00975 }else
00976 {
00977 return 0;
00978 }
00979 }
00980
00981 int ExplorationPlanner::check_trajectory_plan(double x, double y)
00982 {
00983 geometry_msgs::PoseStamped goalPointSimulated, startPointSimulated;
00984 int distance;
00985
00986 ROS_DEBUG("Check Trajectory");
00987 if (!costmap_global_ros_->getRobotPose(robotPose))
00988 {
00989 ROS_ERROR("Failed to get RobotPose");
00990 }
00991
00992 std::vector<double> backoffGoal;
00993 bool backoff_flag = smartGoalBackoff(x,y, costmap_global_ros_, &backoffGoal);
00994
00995 startPointSimulated.header.seq = start_point_simulated_message++;
00996 startPointSimulated.header.stamp = ros::Time::now();
00997 startPointSimulated.header.frame_id = move_base_frame;
00998 startPointSimulated.pose.position.x = robotPose.getOrigin().getX();
00999 startPointSimulated.pose.position.y = robotPose.getOrigin().getY();
01000 startPointSimulated.pose.position.z = 0;
01001 startPointSimulated.pose.orientation.x = 0;
01002 startPointSimulated.pose.orientation.y = 0;
01003 startPointSimulated.pose.orientation.z = 0;
01004 startPointSimulated.pose.orientation.w = 1;
01005
01006
01007 goalPointSimulated.header.seq = goal_point_simulated_message++;
01008 goalPointSimulated.header.stamp = ros::Time::now();
01009 goalPointSimulated.header.frame_id = move_base_frame;
01010
01011
01012 if(backoff_flag == true)
01013 {
01014 goalPointSimulated.pose.position.x = backoffGoal.at(0);
01015 goalPointSimulated.pose.position.y = backoffGoal.at(1);
01016 }else
01017 {
01018 goalPointSimulated.pose.position.x = x;
01019 goalPointSimulated.pose.position.y = y;
01020 }
01021 goalPointSimulated.pose.position.z = 0;
01022 goalPointSimulated.pose.orientation.x = 0;
01023 goalPointSimulated.pose.orientation.y = 0;
01024 goalPointSimulated.pose.orientation.z = 0;
01025 goalPointSimulated.pose.orientation.w = 1;
01026
01027 std::vector<geometry_msgs::PoseStamped> global_plan;
01028
01029
01030
01031
01032
01033 bool successful = nav.makePlan(startPointSimulated, goalPointSimulated, global_plan);
01034
01035 if(successful == true)
01036 {
01037 distance = global_plan.size();
01038 return distance;
01039 }else
01040 {
01041
01042 return -1;
01043 }
01044 }
01045
01046 int ExplorationPlanner::estimate_trajectory_plan(double start_x, double start_y, double target_x, double target_y)
01047 {
01048 geometry_msgs::PoseStamped goalPointSimulated, startPointSimulated;
01049 int distance;
01050
01051 ROS_DEBUG("Check Trajectory");
01052 if (!costmap_ros_->getRobotPose(robotPose))
01053 {
01054 ROS_ERROR("Failed to get RobotPose");
01055 }
01056
01057 std::vector<double> backoffGoal;
01058 bool backoff_flag = smartGoalBackoff(target_x,target_y, costmap_global_ros_, &backoffGoal);
01059
01060 startPointSimulated.header.seq = start_point_simulated_message++;
01061 startPointSimulated.header.stamp = ros::Time::now();
01062 startPointSimulated.header.frame_id = move_base_frame;
01063 startPointSimulated.pose.position.x = start_x;
01064 startPointSimulated.pose.position.y = start_y;
01065 startPointSimulated.pose.position.z = 0;
01066 startPointSimulated.pose.orientation.x = 0;
01067 startPointSimulated.pose.orientation.y = 0;
01068 startPointSimulated.pose.orientation.z = 0;
01069 startPointSimulated.pose.orientation.w = 1;
01070
01071
01072 goalPointSimulated.header.seq = goal_point_simulated_message++;
01073 goalPointSimulated.header.stamp = ros::Time::now();
01074 goalPointSimulated.header.frame_id = move_base_frame;
01075 if(backoff_flag == true)
01076 {
01077 goalPointSimulated.pose.position.x = backoffGoal.at(0);
01078 goalPointSimulated.pose.position.y = backoffGoal.at(1);
01079 }else
01080 {
01081 goalPointSimulated.pose.position.x = target_x;
01082 goalPointSimulated.pose.position.y = target_y;
01083 }
01084 goalPointSimulated.pose.position.z = 0;
01085 goalPointSimulated.pose.orientation.x = 0;
01086 goalPointSimulated.pose.orientation.y = 0;
01087 goalPointSimulated.pose.orientation.z = 0;
01088 goalPointSimulated.pose.orientation.w = 1;
01089
01090 std::vector<geometry_msgs::PoseStamped> global_plan;
01091
01092 bool successful = nav.makePlan(startPointSimulated, goalPointSimulated, global_plan);
01093
01094 if(successful == true)
01095 {
01096 distance = global_plan.size();
01097 return distance;
01098 }else
01099 {
01100 return -1;
01101 }
01102 }
01103
01104 void ExplorationPlanner::setRobotConfig(int name, double robot_home_position_x, double robot_home_position_y, std::string frame)
01105 {
01106 robot_name = name;
01107 robot_home_x = robot_home_position_x;
01108 robot_home_y = robot_home_position_y;
01109 move_base_frame = frame;
01110 }
01111
01112 void ExplorationPlanner::home_position_(const geometry_msgs::PointStamped::ConstPtr& msg) {
01113 home_position_x_ = msg.get()->point.x;
01114 home_position_y_ = msg.get()->point.y;
01115 }
01116
01117
01118 void ExplorationPlanner::printFrontiers()
01119 {
01120 for(int i = 0; i < frontiers.size(); i++)
01121 {
01122 if(robot_prefix_empty_param == true)
01123 {
01124 ROS_INFO("Frontier %d: x: %f y: %f robot: %s", frontiers.at(i).id, frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate, frontiers.at(i).detected_by_robot_str.c_str());
01125 }else
01126 {
01127 ROS_INFO("Frontier %d: x: %f y: %f", frontiers.at(i).id, frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate);
01128 }
01129 }
01130 }
01131
01132 bool ExplorationPlanner::storeFrontier(double x, double y, int detected_by_robot, std::string detected_by_robot_str, int id)
01133 {
01134 frontier_t new_frontier;
01135
01136 if(robot_prefix_empty_param == true)
01137 {
01138 ROS_DEBUG("Storing Frontier ID: %d Robot: %s", id, detected_by_robot_str.c_str());
01139 if(id != -1)
01140 {
01141 new_frontier.id = id;
01142 }else
01143 {
01144 new_frontier.id = frontier_id_count++;
01145 }
01146 new_frontier.detected_by_robot_str = detected_by_robot_str;
01147 new_frontier.x_coordinate = x;
01148 new_frontier.y_coordinate = y;
01149
01150 store_frontier_mutex.lock();
01151 frontiers.push_back(new_frontier);
01152 store_frontier_mutex.unlock();
01153 }else
01154 {
01155 if(detected_by_robot != robot_name)
01156 {
01157 new_frontier.id = id;
01158 }
01159 else
01160 {
01161 new_frontier.id = (robot_name * 10000) + frontier_id_count++;
01162 }
01163
01164 new_frontier.detected_by_robot = detected_by_robot;
01165 new_frontier.x_coordinate = x;
01166 new_frontier.y_coordinate = y;
01167
01168 store_frontier_mutex.lock();
01169 frontiers.push_back(new_frontier);
01170 store_frontier_mutex.unlock();
01171 }
01172
01173 return true;
01174 }
01175
01176 bool ExplorationPlanner::removeStoredFrontier(int id, std::string detected_by_robot_str)
01177 {
01178 for(int i= 0; i < frontiers.size(); i++)
01179 {
01180 if(robot_prefix_empty_param == true)
01181 {
01182 ROS_DEBUG("Removing frontier with id '%d' detected by robot '%s'", frontiers.at(i).id, frontiers.at(i).detected_by_robot_str.c_str());
01183 if(frontiers.at(i).id == id && frontiers.at(i).detected_by_robot_str.compare(detected_by_robot_str) == 0)
01184 {
01185 ROS_DEBUG("Removing Frontier ID: %d at position: %d of Robot: %s", frontiers.at(i).id, i, frontiers.at(i).detected_by_robot_str.c_str());
01186 store_frontier_mutex.lock();
01187 frontiers.erase(frontiers.begin()+i);
01188
01189
01190
01191
01192 store_frontier_mutex.unlock();
01193
01194 }
01195 }else
01196 {
01197 if(frontiers.at(i).id == id)
01198 {
01199 store_frontier_mutex.lock();
01200 frontiers.erase(frontiers.begin()+i);
01201 if(i > 0)
01202 {
01203 i --;
01204 }
01205 store_frontier_mutex.unlock();
01206 break;
01207 }
01208 }
01209 }
01210 return true;
01211 }
01212
01213 bool ExplorationPlanner::storeVisitedFrontier(double x, double y, int detected_by_robot, std::string detected_by_robot_str, int id)
01214 {
01215
01216 frontier_t visited_frontier;
01217
01218
01219 if(robot_prefix_empty_param == true)
01220 {
01221 ROS_DEBUG("Storing Visited Frontier ID: %d Robot: %s", visited_frontier_id_count, detected_by_robot_str.c_str());
01222 if(id != -1)
01223 {
01224 visited_frontier.id = id;
01225 }else
01226 {
01227 visited_frontier.id = visited_frontier_id_count++;
01228 }
01229
01230 visited_frontier.detected_by_robot_str = detected_by_robot_str;
01231 visited_frontier.x_coordinate = x;
01232 visited_frontier.y_coordinate = y;
01233
01234 store_visited_mutex.lock();
01235 visited_frontiers.push_back(visited_frontier);
01236 store_visited_mutex.unlock();
01237
01238 }else
01239 {
01240 if(detected_by_robot != robot_name)
01241 {
01242 visited_frontier.id = id;
01243 }
01244 else
01245 {
01246 visited_frontier.id = (robot_name * 10000) + visited_frontier_id_count++;
01247 }
01248
01249 visited_frontier.detected_by_robot = detected_by_robot;
01250 visited_frontier.x_coordinate = x;
01251 visited_frontier.y_coordinate = y;
01252
01253 store_visited_mutex.lock();
01254 visited_frontiers.push_back(visited_frontier);
01255 store_visited_mutex.unlock();
01256 }
01257
01258 bool break_flag = false;
01259 for(int i = 0; i < clusters.size(); i++)
01260 {
01261 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
01262 {
01263 if(clusters.at(i).cluster_element.at(j).id == id)
01264 {
01265 ROS_DEBUG("Set cluster unreachable count to 0");
01266 clusters.at(i).unreachable_frontier_count = 0;
01267 break_flag = true;
01268 break;
01269 }
01270 }
01271 if(break_flag == true)
01272 {
01273 break;
01274 }
01275 }
01276
01277 return true;
01278 }
01279
01280 bool ExplorationPlanner::removeVisitedFrontier(int id, std::string detected_by_robot_str)
01281 {
01282 for(int i= 0; i< visited_frontiers.size(); i++)
01283 {
01284 if(robot_prefix_empty_param == true)
01285 {
01286
01287 if(visited_frontiers.at(i).id == id && visited_frontiers.at(i).detected_by_robot_str.compare(detected_by_robot_str) == 0)
01288 {
01289 ROS_INFO("Removing Visited Frontier ID: %d at position: %d of Robot: %s", visited_frontiers.at(i).id, i, visited_frontiers.at(i).detected_by_robot_str.c_str());
01290 store_visited_mutex.lock();
01291 visited_frontiers.erase(visited_frontiers.begin()+i);
01292
01293
01294
01295
01296 store_visited_mutex.unlock();
01297 break;
01298 }
01299 }else
01300 {
01301 if(visited_frontiers.at(i).id == id)
01302 {
01303 store_visited_mutex.lock();
01304 visited_frontiers.erase(visited_frontiers.begin()+i);
01305 if(i > 0)
01306 {
01307 i --;
01308 }
01309 store_visited_mutex.unlock();
01310 break;
01311 }
01312 }
01313 }
01314 return true;
01315 }
01316 bool ExplorationPlanner::storeUnreachableFrontier(double x, double y, int detected_by_robot, std::string detected_by_robot_str, int id)
01317 {
01318 frontier_t unreachable_frontier;
01319
01320 if(robot_prefix_empty_param == true)
01321 {
01322 ROS_DEBUG("Storing Unreachable Frontier ID: %d Robot: %s", unreachable_frontier_id_count, detected_by_robot_str.c_str());
01323
01324 if(id != -1)
01325 {
01326 unreachable_frontier.id = id;
01327 }else
01328 {
01329 unreachable_frontier.id = unreachable_frontier_id_count++;
01330 }
01331
01332 unreachable_frontier.detected_by_robot_str = detected_by_robot_str;
01333 unreachable_frontier.x_coordinate = x;
01334 unreachable_frontier.y_coordinate = y;
01335
01336 unreachable_frontiers.push_back(unreachable_frontier);
01337
01338 }else
01339 {
01340 if(detected_by_robot != robot_name)
01341 {
01342 unreachable_frontier.id = id;
01343 }
01344 else
01345 {
01346 unreachable_frontier.id = (robot_name * 10000) + unreachable_frontier_id_count++;
01347 }
01348
01349 unreachable_frontier.detected_by_robot = detected_by_robot;
01350 unreachable_frontier.x_coordinate = x;
01351 unreachable_frontier.y_coordinate = y;
01352
01353 frontiers.push_back(unreachable_frontier);
01354 }
01355
01356
01357 bool break_flag = false;
01358 for(int i = 0; i < clusters.size(); i++)
01359 {
01360 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
01361 {
01362 if(clusters.at(i).cluster_element.at(j).id == id)
01363 {
01364 ROS_WARN("Increasing cluster unreachable count");
01365 clusters.at(i).unreachable_frontier_count++;
01366 break_flag = true;
01367 break;
01368 }
01369 }
01370 if(break_flag == true)
01371 {
01372 break;
01373 }
01374 }
01375
01376 return true;
01377
01378 }
01379
01380 bool ExplorationPlanner::removeUnreachableFrontier(int id, std::string detected_by_robot_str)
01381 {
01382 for(int i= 0; i< unreachable_frontiers.size(); i++)
01383 {
01384 if(robot_prefix_empty_param == true)
01385 {
01386 if(unreachable_frontiers.at(i).id == id && unreachable_frontiers.at(i).detected_by_robot_str.compare(detected_by_robot_str) == 0)
01387 {
01388 ROS_INFO("Removing Unreachable Frontier ID: %d at position: %d of Robot: %s", unreachable_frontiers.at(i).id, i, unreachable_frontiers.at(i).detected_by_robot_str.c_str());
01389
01390 unreachable_frontiers.erase(unreachable_frontiers.begin()+i);
01391
01392
01393
01394
01395 break;
01396 }
01397 }else
01398 {
01399 if(unreachable_frontiers.at(i).id == id)
01400 {
01401 unreachable_frontiers.erase(unreachable_frontiers.begin()+i);
01402 if(i > 0)
01403 {
01404 i --;
01405 }
01406 break;
01407 }
01408 }
01409 }
01410
01411 return true;
01412 }
01413
01414 bool ExplorationPlanner::publish_negotiation_list(frontier_t negotiation_frontier, int cluster_number)
01415 {
01416
01417 adhoc_communication::ExpFrontier negotiation_msg;
01418
01419
01420
01421
01422
01423
01424
01425
01426
01427
01429
01430
01431
01432 if(cluster_number != -1)
01433 {
01434 int cluster_vector_position = 0;
01435 for (int i = 0; i < clusters.size(); i++)
01436 {
01437 if(clusters.at(i).id == cluster_number)
01438 {
01439 ROS_DEBUG("Cluster ID: %d is at vector position: %d", cluster_number, i);
01440 cluster_vector_position = i;
01441 break;
01442 }
01443 }
01444
01445 for(int i = 0; i < clusters.at(cluster_vector_position).cluster_element.size(); i++)
01446 {
01447 adhoc_communication::ExpFrontierElement negotiation_element;
01448 negotiation_element.x_coordinate = clusters.at(cluster_vector_position).cluster_element.at(i).x_coordinate;
01449 negotiation_element.y_coordinate = clusters.at(cluster_vector_position).cluster_element.at(i).y_coordinate;
01450 negotiation_element.detected_by_robot = clusters.at(cluster_vector_position).cluster_element.at(i).detected_by_robot;
01451 negotiation_element.id = clusters.at(cluster_vector_position).cluster_element.at(i).id;
01452
01453 frontier_t new_frontier;
01454 new_frontier.x_coordinate = clusters.at(cluster_vector_position).cluster_element.at(i).x_coordinate;
01455 new_frontier.y_coordinate = clusters.at(cluster_vector_position).cluster_element.at(i).y_coordinate;
01456 new_frontier.detected_by_robot = clusters.at(cluster_vector_position).cluster_element.at(i).detected_by_robot;
01457 new_frontier.id = clusters.at(cluster_vector_position).cluster_element.at(i).id;
01458
01459 negotiation_msg.frontier_element.push_back(negotiation_element);
01460 my_negotiation_list.push_back(new_frontier);
01461 }
01462
01463 }else
01464 {
01465 adhoc_communication::ExpFrontierElement negotiation_element;
01466 negotiation_element.detected_by_robot = negotiation_frontier.detected_by_robot;
01467 negotiation_element.x_coordinate = negotiation_frontier.x_coordinate;
01468 negotiation_element.y_coordinate = negotiation_frontier.y_coordinate;
01469 negotiation_element.id = negotiation_frontier.id;
01470
01471 negotiation_msg.frontier_element.push_back(negotiation_element);
01472 }
01473
01474 sendToMulticast("mc_",negotiation_msg, "negotiation_list");
01475
01476 first_negotiation_run = false;
01477 }
01478
01479
01480 bool ExplorationPlanner::sendToMulticast(std::string multi_cast_group, adhoc_communication::ExpFrontier frontier_to_send, std::string topic)
01481 {
01482
01483 adhoc_communication::SendExpFrontier service_frontier;
01484
01485 std::stringstream robot_number;
01486 robot_number << robot_name;
01487 std::string prefix = "robot_";
01488 std::string robo_name = prefix.append(robot_number.str());
01489
01490 if(robot_prefix_empty_param == true)
01491 {
01492 robo_name = robot_str;
01493
01494 }
01495 std::string destination_name = multi_cast_group + robo_name;
01496
01497
01498 ROS_INFO("sending to multicast group '%s' on topic: '%s'",destination_name.c_str(), topic.c_str());
01499 service_frontier.request.dst_robot = destination_name;
01500 service_frontier.request.frontier = frontier_to_send;
01501 service_frontier.request.topic = topic;
01502
01503 if (ssendFrontier.call(service_frontier))
01504 {
01505 ROS_DEBUG("Successfully called service sendToMulticast");
01506
01507 if(service_frontier.response.status)
01508 {
01509 ROS_DEBUG("adhoc comm returned successful transmission");
01510 return true;
01511 }
01512 else
01513 {
01514 ROS_DEBUG("Failed to send to multicast group %s!",destination_name.c_str());
01515 return false;
01516 }
01517 }
01518 else
01519 {
01520 ROS_WARN("Failed to call service sendToMulticast [%s]",ssendFrontier.getService().c_str());
01521 return false;
01522 }
01523 }
01524
01525
01526 bool ExplorationPlanner::sendToMulticastAuction(std::string multi_cast_group, adhoc_communication::ExpAuction auction_to_send, std::string topic)
01527 {
01528 adhoc_communication::SendExpAuction service_auction;
01529
01530 std::stringstream robot_number;
01531 robot_number << robot_name;
01532 std::string prefix = "robot_";
01533 std::string robo_name = prefix.append(robot_number.str());
01534
01535 if(robot_prefix_empty_param == true)
01536 {
01537
01538 robo_name = robot_str;
01539 }
01540
01541 std::string destination_name = multi_cast_group + robo_name;
01542
01543
01544 ROS_INFO("sending auction to multicast group '%s' on topic '%s'",destination_name.c_str(), topic.c_str());
01545 service_auction.request.dst_robot = destination_name;
01546 service_auction.request.auction = auction_to_send;
01547 service_auction.request.topic = topic;
01548
01549 if (ssendAuction.call(service_auction))
01550 {
01551 ROS_DEBUG("Successfully called service sendToMulticast");
01552
01553 if(service_auction.response.status)
01554 {
01555 ROS_DEBUG("Auction was multicasted successfully.");
01556 return true;
01557 }
01558 else
01559 {
01560 ROS_WARN("Failed to send auction to mutlicast group %s!",destination_name.c_str());
01561 return false;
01562 }
01563 }
01564 else
01565 {
01566 ROS_WARN("Failed to call service sendToMulticastAuction [%s]",ssendAuction.getService().c_str());
01567 return false;
01568 }
01569 }
01570
01571 void ExplorationPlanner::negotiationCallback(const adhoc_communication::ExpFrontier::ConstPtr& msg)
01572 {
01573
01574
01575 bool entry_found = false;
01576
01577 adhoc_communication::ExpFrontierElement frontier_element;
01578 for(int j = 0; j < msg.get()->frontier_element.size(); j++)
01579 {
01580 frontier_element = msg.get()->frontier_element.at(j);
01581 for(int i = 0; i < negotiation_list.size(); i++)
01582 {
01583 if(robot_prefix_empty_param == true)
01584 {
01585 if(negotiation_list.at(i).id == frontier_element.id && negotiation_list.at(i).detected_by_robot_str.compare(frontier_element.detected_by_robot_str) == 0)
01586 {
01587 entry_found = true;
01588 }
01589 }else
01590 {
01591 if(negotiation_list.at(i).id == frontier_element.id)
01592 {
01593 entry_found = true;
01594 }
01595 }
01596
01597 }
01598 if(entry_found == false)
01599 {
01600 ROS_DEBUG("Negotiation frontier with ID: %ld", frontier_element.id);
01601 frontier_t negotiation_frontier;
01602 negotiation_frontier.detected_by_robot = frontier_element.detected_by_robot;
01603 negotiation_frontier.id = frontier_element.id;
01604 negotiation_frontier.x_coordinate = frontier_element.x_coordinate;
01605 negotiation_frontier.y_coordinate = frontier_element.y_coordinate;
01606
01607 store_negotiation_mutex.lock();
01608 negotiation_list.push_back(negotiation_frontier);
01609 store_negotiation_mutex.unlock();
01610 }
01611 }
01612 }
01613
01614
01615
01616
01617
01618
01619
01620
01621
01622
01623
01624
01625
01626
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01660
01661
01662
01663
01664
01666
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685 bool ExplorationPlanner::respondToAuction(std::vector<requested_cluster_t> requested_cluster_ids, int auction_id_number)
01686 {
01687
01688
01689
01690
01692
01693
01694 ROS_INFO("Respond to auction");
01695 adhoc_communication::ExpAuction auction_msgs;
01696
01697 for(int i = 0; i < requested_cluster_ids.size(); i++)
01698 {
01699 ROS_INFO("Responding to cluster ids: %d", requested_cluster_ids.at(i).own_cluster_id);
01700 }
01701
01702 for(int n = 0; n < requested_cluster_ids.size(); n++)
01703 {
01704 cluster_mutex.lock();
01705 for(int i = 0; i < clusters.size(); i++)
01706 {
01707 if(clusters.at(i).id == requested_cluster_ids.at(n).own_cluster_id)
01708 {
01709 adhoc_communication::ExpCluster cluster_msg;
01710 adhoc_communication::ExpClusterElement cluster_element_msg;
01711 for(int j = 0; j < requested_cluster_ids.at(n).requested_ids.size(); j++)
01712 {
01713 if(robot_prefix_empty_param == true)
01714 {
01715 cluster_element_msg.id = requested_cluster_ids.at(n).requested_ids.at(j).id;
01716 cluster_element_msg.detected_by_robot_str = requested_cluster_ids.at(n).requested_ids.at(j).robot_str;
01717 }else
01718 {
01719 cluster_element_msg.id = requested_cluster_ids.at(n).requested_ids.at(j).id;
01720 }
01721 cluster_msg.ids_contained.push_back(cluster_element_msg);
01722 }
01723
01724 cluster_msg.bid = calculateAuctionBID(clusters.at(i).id, trajectory_strategy);
01725 auction_msgs.available_clusters.push_back(cluster_msg);
01726 break;
01727 }
01728 }
01729 cluster_mutex.unlock();
01730 }
01731
01732
01733
01734
01735
01736
01737
01738
01739
01740
01741
01742 ROS_INFO("Robot %d publishes auction bids for all clusters", robot_name);
01743
01744
01745
01746
01747
01748
01749
01750
01751
01752
01753 auction_msgs.auction_status_message = false;
01754 auction_msgs.auction_id = auction_id_number;
01755
01756 std::stringstream ss;
01757 ss << robot_name;
01758 std::string prefix = "";
01759 std::string robo_name = prefix.append(ss.str());
01760
01761 auction_msgs.robot_name = robo_name;
01762
01763
01764
01765
01766
01767
01768 sendToMulticastAuction("mc_", auction_msgs, "auction");
01769
01770
01771 start_thr_auction = false;
01772
01773
01774 }
01775
01776
01777 int ExplorationPlanner::calculateAuctionBID(int cluster_number, std::string strategy)
01778 {
01779
01780 int auction_bid = 0;
01781 int cluster_vector_position = -1;
01782 bool cluster_could_be_found = false;
01783
01784 if(clusters.size() > 0)
01785 {
01786 for (int i = 0; i < clusters.size(); i++)
01787 {
01788 if(clusters.at(i).id == cluster_number)
01789 {
01790
01791
01792 cluster_vector_position = i;
01793 cluster_could_be_found = true;
01794 break;
01795
01796
01797
01798
01799
01800 }
01801 }
01802 }
01803 if(cluster_could_be_found == false)
01804 {
01805
01806
01807
01808 ROS_WARN("Cluster could not be found");
01809 return(-1);
01810 }
01811
01812 if (!costmap_global_ros_->getRobotPose(robotPose))
01813 {
01814 ROS_ERROR("Failed to get RobotPose");
01815 }
01816
01817 int distance = -1;
01818 for(int i = 0; i < clusters.at(cluster_vector_position).cluster_element.size(); i++)
01819 {
01820
01821 if(strategy == "trajectory")
01822 {
01823 distance = check_trajectory_plan(clusters.at(cluster_vector_position).cluster_element.at(i).x_coordinate, clusters.at(cluster_vector_position).cluster_element.at(i).y_coordinate);
01824
01825
01826
01827
01828 }else if(strategy == "euclidean")
01829 {
01830 double x = clusters.at(cluster_vector_position).cluster_element.at(i).x_coordinate - robotPose.getOrigin().getX();
01831 double y = clusters.at(cluster_vector_position).cluster_element.at(i).y_coordinate - robotPose.getOrigin().getY();
01832 distance = x * x + y * y;
01833 return distance;
01834 }
01835
01836 if(distance > 0)
01837 {
01838 double x = clusters.at(cluster_vector_position).cluster_element.at(i).x_coordinate - robotPose.getOrigin().getX();
01839 double y = clusters.at(cluster_vector_position).cluster_element.at(i).y_coordinate - robotPose.getOrigin().getY();
01840 double euclidean_distance = x * x + y * y;
01841
01842
01843
01844
01845
01846
01847
01848
01849 if (distance * costmap_ros_->getCostmap()->getResolution() <= sqrt(euclidean_distance)*0.95)
01850 {
01851 ROS_WARN("Euclidean distance smaller then trajectory distance to LOCAL CLUSTER!!!");
01852
01853 }else
01854 {
01855 return distance;
01856 }
01857 }
01858 }
01859 if(distance == -1)
01860 {
01861
01862 }
01863
01864 return(-1);
01865 }
01866
01867
01868 void ExplorationPlanner::positionCallback(const adhoc_communication::MmListOfPoints::ConstPtr& msg)
01869 {
01870 ROS_DEBUG("Position Callback !!!!!");
01871 position_mutex.lock();
01872
01873 other_robots_positions.positions.clear();
01874 ROS_INFO("positions size: %lu", msg.get()->positions.size());
01875 for(int i = 0; i < msg.get()->positions.size(); i++)
01876 {
01877 other_robots_positions.positions.push_back(msg.get()->positions.at(i));
01878 }
01879 position_mutex.unlock();
01880 }
01881
01882
01883 void ExplorationPlanner::auctionCallback(const adhoc_communication::ExpAuction::ConstPtr& msg)
01884 {
01885 auction_running = true;
01886
01887 int robots_int_name;
01888
01889 bool same_robot = false;
01890 if(robot_prefix_empty_param == true)
01891 {
01892 if(robot_str.compare(msg.get()->robot_name.c_str()) == 0)
01893 {
01894 same_robot = true;
01895 }
01896 }else
01897 {
01898 robots_int_name = atoi(msg.get()->robot_name.c_str());
01899 ROS_INFO("Robot name: %d int_name: %d",robot_name, robots_int_name);
01900 if(robots_int_name == robot_name)
01901 {
01902 same_robot = true;
01903 }
01904 }
01905
01906 if(same_robot == false)
01907 {
01908
01909
01910
01911
01912
01913
01914
01915
01916
01917
01918 transformToOwnCoordinates_frontiers();
01919 transformToOwnCoordinates_visited_frontiers();
01920
01921 clearVisitedFrontiers();
01922 clearUnreachableFrontiers();
01923 clearSeenFrontiers(costmap_global_ros_);
01924
01925 clearVisitedAndSeenFrontiersFromClusters();
01926 clusterFrontiers();
01927
01928
01929
01930
01931 if(msg.get()->auction_status_message == true)
01932 {
01933 ROS_INFO("Calling Auction Status");
01934
01935
01936
01937
01938
01939 for(int i = 0; i < msg.get()->requested_clusters.size(); i++)
01940 {
01941 adhoc_communication::ExpCluster cluster_req;
01942 cluster_req = msg.get()->requested_clusters.at(i);
01943 std::string requested_clusters;
01944 for(int j = 0; j < cluster_req.ids_contained.size(); j++)
01945 {
01946 if(j >= 6)
01947 {
01948 break;
01949 }
01950 requested_clusters.append(NumberToString((int)cluster_req.ids_contained.at(j).id));
01951 requested_clusters.append(", ");
01952 }
01953 ROS_INFO("Requested ids: %s from robot: %s", requested_clusters.c_str(), msg.get()->robot_name.c_str());
01954 }
01955
01956
01957
01958 auction_start = msg.get()->start_auction;
01959 auction_finished = msg.get()->auction_finished;
01960 adhoc_communication::ExpCluster occupied_ids;
01961 std::vector<requested_cluster_t> requested_cluster_ids;
01962
01963
01964
01965
01966
01967
01968
01969
01970 if(msg.get()->occupied_ids.size() > 0 || msg.get()->requested_clusters.size() > 0)
01971 {
01972 for(int i = 0; i < msg.get()->occupied_ids.size(); i++)
01973 {
01974 adhoc_communication::ExpClusterElement cluster_element;
01975
01976 cluster_element.id = msg.get()->occupied_ids.at(i).id;
01977 cluster_element.detected_by_robot_str = msg.get()->occupied_ids.at(i).detected_by_robot_str;
01978
01979 occupied_ids.ids_contained.push_back(cluster_element);
01980 }
01981 int occupied_cluster_id = checkClustersID(occupied_ids);
01982 ROS_INFO("Check occupied cluster to be the same. %d", occupied_cluster_id);
01983
01984 if(occupied_cluster_id >=0)
01985 {
01986 ROS_INFO("Adding occupied cluster: %d", occupied_cluster_id);
01987 already_used_ids.push_back(occupied_cluster_id);
01988 }else
01989 {
01990
01991 ROS_INFO("Adding occupied cluster as unrecognized!");
01992 unrecognized_occupied_clusters.push_back(occupied_ids);
01993 }
01994
01995
01996
01997
01998
01999 for(int i = 0; i < unrecognized_occupied_clusters.size(); i++)
02000 {
02001 int unrecognized_occupied_cluster_id = checkClustersID(unrecognized_occupied_clusters.at(i));
02002 if(unrecognized_occupied_cluster_id >=0)
02003 {
02004 ROS_INFO("Unrecognized cluster: %d converted", unrecognized_occupied_cluster_id);
02005 already_used_ids.push_back(unrecognized_occupied_cluster_id);
02006 unrecognized_occupied_clusters.erase(unrecognized_occupied_clusters.begin() + i);
02007
02008 if(i > 0)
02009 i--;
02010 }
02011 }
02012
02013
02014
02015
02016
02017 for(int i = 0; i < msg.get()->requested_clusters.size(); i++)
02018 {
02019 adhoc_communication::ExpCluster requested_cluster;
02020 requested_cluster = msg.get()->requested_clusters.at(i);
02021
02022 int check_cluster_id = checkClustersID(requested_cluster);
02023 if(check_cluster_id >= 0)
02024 {
02025 requested_cluster_t new_cluster_request;
02026 for(int j = 0; j < requested_cluster.ids_contained.size(); j++)
02027 {
02028 transform_point_t cluster_element_point;
02029 if(robot_prefix_empty_param == true)
02030 {
02031 cluster_element_point.id = requested_cluster.ids_contained.at(j).id;
02032 cluster_element_point.robot_str = requested_cluster.ids_contained.at(j).detected_by_robot_str;
02033 }else
02034 {
02035 cluster_element_point.id = requested_cluster.ids_contained.at(j).id;
02036 }
02037
02038 new_cluster_request.requested_ids.push_back(cluster_element_point);
02039 }
02040 new_cluster_request.own_cluster_id = check_cluster_id;
02041
02042 requested_cluster_ids.push_back(new_cluster_request);
02043 }else
02044 {
02045 ROS_WARN("No Matching Cluster Detected");
02046 }
02047 }
02048 }
02049
02050
02051 if(auction_start == true)
02052 {
02053 start_thr_auction = true;
02054 thr_auction_status = boost::thread(&ExplorationPlanner::respondToAuction, this, requested_cluster_ids, msg.get()->auction_id);
02055 }
02056
02057 }else if(msg.get()->auction_status_message == false)
02058 {
02059 bool continue_auction = false;
02060 if(robot_prefix_empty_param == true)
02061 {
02062 if(msg.get()->auction_id == auction_id_number)
02063 {
02064 continue_auction = true;
02065 }
02066 }else
02067 {
02068 if(msg.get()->auction_id == 10000*robot_name + auction_id_number)
02069 {
02070 continue_auction = true;
02071 }
02072 }
02073
02074
02075 if(continue_auction == true)
02076 {
02077
02078 bool robot_already_answered = false;
02079
02080 for(int i = 0; i < robots_already_responded.size(); i++)
02081 {
02082 if(robot_prefix_empty_param == true)
02083 {
02084 if(msg.get()->robot_name.compare(robots_already_responded.at(i).robot_str) == 0 && msg.get()->auction_id == robots_already_responded.at(i).auction_number)
02085 {
02086 ROS_WARN("Same msg already received!!!");
02087 robot_already_answered = true;
02088 break;
02089 }
02090 }else
02091 {
02092 ROS_INFO("Compare msg name: %d responded: %d msg auction: %d responded: %d", robots_int_name, robots_already_responded.at(i).robot_number, msg.get()->auction_id, robots_already_responded.at(i).auction_number);
02093 if(robots_int_name == robots_already_responded.at(i).robot_number && msg.get()->auction_id == robots_already_responded.at(i).auction_number)
02094 {
02095 ROS_WARN("Same msg already received!!!");
02096 robot_already_answered = true;
02097 break;
02098 }
02099 }
02100 }
02101
02102
02103
02104
02105 if(robot_already_answered == false)
02106 {
02107 if(robot_prefix_empty_param == true)
02108 {
02109 ROS_INFO("Auction from robot %s received", msg.get()->robot_name.c_str());
02110 }else
02111 {
02112 ROS_INFO("Auction from robot %d received", robot_name);
02113 }
02114 auction_pair_t auction_pair;
02115 auction_element_t auction_elements;
02116
02117 int has_cluster_id = -1;
02118
02119
02120
02121
02122 for(int i = 0; i < msg.get()->available_clusters.size(); i++)
02123 {
02124 adhoc_communication::ExpCluster cluster_req;
02125 cluster_req = msg.get()->available_clusters.at(i);
02126 ROS_INFO("---------------------- %d ----------------------------", i);
02127 std::string requested_clusters;
02128 for(int j = 0; j < cluster_req.ids_contained.size(); j++)
02129 {
02130 if(j >= 6)
02131 {
02132 break;
02133 }
02134 requested_clusters.append(NumberToString((int)cluster_req.ids_contained.at(j).id));
02135 requested_clusters.append(", ");
02136 }
02137 ROS_INFO("Received ids: %s", requested_clusters.c_str());
02138 }
02139
02140
02141
02142 for(int i = 0; i < msg.get()->available_clusters.size(); i++)
02143 {
02144 adhoc_communication::ExpCluster current_cluster;
02145 current_cluster = msg.get()->available_clusters.at(i);
02146
02147
02148
02149
02150
02151 int current_cluster_id = checkClustersID(current_cluster);
02152
02153 ROS_INFO("Received ID converted to cluster ID: %d", current_cluster_id);
02154 if(current_cluster_id >= 0)
02155 {
02156 auction_pair.cluster_id = current_cluster_id;
02157 auction_pair.bid_value = current_cluster.bid;
02158 auction_elements.auction_element.push_back(auction_pair);
02159 ROS_INFO("BID: %f",current_cluster.bid);
02160 }
02161 }
02162
02163 auction_elements.robot_id = robots_int_name;
02164 auction_elements.detected_by_robot_str = msg.get()->robot_name;
02165
02166 auction_mutex.lock();
02167 auction.push_back(auction_elements);
02168 auction_mutex.unlock();
02169
02170
02171
02172
02173
02174
02175
02176 number_of_auction_bids_received++;
02177 responded_t auction_response;
02178 auction_response.auction_number = msg.get()->auction_id;
02179 auction_response.robot_number = robots_int_name;
02180 if(robot_prefix_empty_param == true)
02181 {
02182 auction_response.robot_str = msg.get()->robot_name;
02183 }
02184 robots_already_responded.push_back(auction_response);
02185
02186 }else
02187 {
02188 ROS_WARN("Robot already answered on this auction");
02189 }
02190 }
02191 }
02192 }
02193 auction_running = false;
02194 }
02195
02196
02197 bool sortCompareElements(const ExplorationPlanner::compare_pair_t &lhs, const ExplorationPlanner::compare_pair_t &rhs)
02198 {
02199 return lhs.identical_ids > rhs.identical_ids;
02200 }
02201
02202 int ExplorationPlanner::checkClustersID(adhoc_communication::ExpCluster cluster_to_check)
02203 {
02204
02205 std::vector<compare_pair_t> compare_clusters;
02206
02207
02208 std::string requested_clusters;
02209
02210
02211
02212
02213
02214
02215
02216
02217
02218
02219
02220 for(int j = 0; j < clusters.size(); j++)
02221 {
02222 double same_id_found = 0;
02223
02224 for(int n = 0; n < clusters.at(j).cluster_element.size(); n++)
02225 {
02226
02227
02228
02229
02230
02231 for(int i = 0; i < cluster_to_check.ids_contained.size(); i++)
02232 {
02233 if(robot_prefix_empty_param == true)
02234 {
02235
02236 if(clusters.at(j).cluster_element.at(n).id == cluster_to_check.ids_contained.at(i).id && clusters.at(j).cluster_element.at(n).detected_by_robot_str.compare(cluster_to_check.ids_contained.at(i).detected_by_robot_str) == 0)
02237 {
02238 same_id_found++;
02239 }
02240 }else
02241 {
02242 if(clusters.at(j).cluster_element.at(n).id == cluster_to_check.ids_contained.at(i).id)
02243 {
02244 same_id_found++;
02245 }
02246 }
02247 }
02248 }
02249 if(same_id_found > clusters.at(j).cluster_element.size() * 0.4)
02250 {
02251
02252 return (clusters.at(j).id);
02253 }
02254 else
02255 {
02256
02257 }
02258
02259
02260
02261
02262
02263
02264
02265
02266
02267
02268
02269
02270
02271
02272
02273
02274
02275
02276
02277
02278
02279
02280
02281
02282
02283
02284
02285 }
02286
02287
02288
02289
02290
02291
02292
02293
02299
02300
02301
02302
02303
02304
02305 return (-1);
02306 }
02307
02308
02309
02310
02311
02312
02313
02314
02315
02316
02317
02318 void ExplorationPlanner::frontierCallback(const adhoc_communication::ExpFrontier::ConstPtr& msg)
02319 {
02320
02321
02322 adhoc_communication::ExpFrontierElement frontier_element;
02323 for(int i = 0; i < msg.get()->frontier_element.size(); i++)
02324 {
02325 frontier_element = msg.get()->frontier_element.at(i);
02326 bool result = true;
02327 for (unsigned int j = 0; j < frontiers.size(); j++)
02328 {
02329 if(robot_prefix_empty_param == true)
02330 {
02331
02332 if(frontiers.at(j).detected_by_robot_str.compare(frontier_element.detected_by_robot_str) == 0 && frontier_element.id == frontiers.at(j).id)
02333 {
02334
02335 result = false;
02336 break;
02337 }
02338 }
02339 else
02340 {
02341 if(frontier_element.detected_by_robot == robot_name)
02342 {
02343 result = false;
02344 break;
02345 }
02346 else
02347 {
02348 if (frontier_element.id == frontiers.at(j).id)
02349 {
02350 result = false;
02351 break;
02352 }
02353 }
02354 }
02355 }
02356 if (result == true)
02357 {
02358
02359 if(robot_prefix_empty_param == true)
02360 {
02361 ROS_DEBUG("Received New Frontier with ID: %ld Robot: %s", frontier_element.id, frontier_element.detected_by_robot_str.c_str());
02362 storeFrontier(frontier_element.x_coordinate, frontier_element.y_coordinate, frontier_element.detected_by_robot, frontier_element.detected_by_robot_str, frontier_element.id);
02363 }else
02364 {
02365 ROS_DEBUG("Received New Frontier of Robot %ld with ID %ld", frontier_element.detected_by_robot, frontier_element.id);
02366 if(frontier_element.detected_by_robot != robot_name)
02367 {
02368 storeFrontier(frontier_element.x_coordinate, frontier_element.y_coordinate, frontier_element.detected_by_robot, "", frontier_element.id);
02369 }
02370 }
02371 }
02372 }
02373
02374 }
02375
02376
02377 void ExplorationPlanner::visited_frontierCallback(const adhoc_communication::ExpFrontier::ConstPtr& msg)
02378 {
02379 adhoc_communication::ExpFrontierElement frontier_element;
02380 for(int i = 0; i < msg.get()->frontier_element.size(); i++)
02381 {
02382 frontier_element = msg.get()->frontier_element.at(i);
02383
02384 bool result = true;
02385 for (unsigned int j = 0; j < visited_frontiers.size(); j++)
02386 {
02387 if(frontier_element.detected_by_robot == robot_name)
02388 {
02389 result = false;
02390 break;
02391 }
02392 else
02393 {
02394 if (frontier_element.id == visited_frontiers.at(j).id)
02395 {
02396 result = false;
02397 break;
02398 }
02399 }
02400 }
02401 if (result == true)
02402 {
02403 ROS_DEBUG("Received New Visited Frontier of Robot %ld with ID %ld", frontier_element.detected_by_robot, frontier_element.id);
02404 if(robot_prefix_empty_param == true)
02405 {
02406 ROS_DEBUG("Storing Visited Frontier ID: %ld Robot: %s", frontier_element.id, frontier_element.detected_by_robot_str.c_str());
02407 storeVisitedFrontier(frontier_element.x_coordinate, frontier_element.y_coordinate, frontier_element.detected_by_robot, frontier_element.detected_by_robot_str, frontier_element.id);
02408 }else
02409 {
02410 if(frontier_element.detected_by_robot != robot_name)
02411 {
02412 storeVisitedFrontier(frontier_element.x_coordinate, frontier_element.y_coordinate, frontier_element.detected_by_robot, "", frontier_element.id);
02413 }
02414 }
02415 }
02416 }
02417 }
02418
02419 bool ExplorationPlanner::publish_frontier_list()
02420 {
02421
02422
02423 publish_subscribe_mutex.lock();
02424
02425 adhoc_communication::ExpFrontier frontier_msg;
02426 for(int i = 0; i<frontiers.size(); i++)
02427 {
02428 adhoc_communication::ExpFrontierElement frontier_element;
02429
02430 frontier_element.id = frontiers.at(i).id;
02431 frontier_element.detected_by_robot = frontiers.at(i).detected_by_robot;
02432 frontier_element.detected_by_robot_str = frontiers.at(i).detected_by_robot_str;
02433 frontier_element.robot_home_position_x = frontiers.at(i).robot_home_x;
02434 frontier_element.robot_home_position_y = frontiers.at(i).robot_home_y;
02435 frontier_element.x_coordinate = frontiers.at(i).x_coordinate;
02436 frontier_element.y_coordinate = frontiers.at(i).y_coordinate;
02437
02438 frontier_msg.frontier_element.push_back(frontier_element);
02439 }
02440
02441
02442 sendToMulticast("mc_",frontier_msg, "frontiers");
02443
02444 publish_subscribe_mutex.unlock();
02445 }
02446
02447 bool ExplorationPlanner::publish_visited_frontier_list()
02448 {
02449
02450
02451 publish_subscribe_mutex.lock();
02452
02453 adhoc_communication::ExpFrontier visited_frontier_msg;
02454
02455 for(int i = 0; i<visited_frontiers.size(); i++)
02456 {
02457 adhoc_communication::ExpFrontierElement visited_frontier_element;
02458
02459 visited_frontier_element.detected_by_robot = visited_frontiers.at(i).detected_by_robot;
02460 visited_frontier_element.detected_by_robot_str = visited_frontiers.at(i).detected_by_robot_str;
02461 visited_frontier_element.robot_home_position_x = visited_frontiers.at(i).robot_home_x;
02462 visited_frontier_element.robot_home_position_y = visited_frontiers.at(i).robot_home_y;
02463 visited_frontier_element.x_coordinate = visited_frontiers.at(i).x_coordinate;
02464 visited_frontier_element.y_coordinate = visited_frontiers.at(i).y_coordinate;
02465
02466 visited_frontier_msg.frontier_element.push_back(visited_frontier_element);
02467 }
02468
02469
02470 sendToMulticast("mc_",visited_frontier_msg, "visited_frontiers");
02471
02472 publish_subscribe_mutex.unlock();
02473 }
02474
02475
02476
02477
02478
02479
02480
02481
02482
02483
02484 bool ExplorationPlanner::check_efficiency_of_goal(double x, double y) {
02485
02486
02487 double diff_home_x = visited_frontiers.at(0).x_coordinate - x;
02488 double diff_home_y = visited_frontiers.at(0).y_coordinate - y;
02489
02490 if (fabs(diff_home_x) <= MAX_DISTANCE && fabs(diff_home_y) <= MAX_DISTANCE)
02491 {
02492 for (int i = 0; i < visited_frontiers.size(); i++)
02493 {
02494
02495
02496
02497
02498 double diff_x = visited_frontiers.at(i).x_coordinate - x;
02499 double diff_y = visited_frontiers.at(i).y_coordinate - y;
02500
02501 if (fabs(diff_x) <= MAX_GOAL_RANGE && fabs(diff_y) <= MAX_GOAL_RANGE) {
02502 ROS_DEBUG("x: %f y: %f too close to visited at x: %f y: %f dif_x: %f dif_y: %f", x, y, visited_frontiers.at(i).x_coordinate, visited_frontiers.at(i).y_coordinate, diff_x, diff_y);
02503 return false;
02504 }
02505 }
02506 for (int i = 0; i < unreachable_frontiers.size(); i++)
02507 {
02508
02509
02510
02511
02512 double diff_x = unreachable_frontiers.at(i).x_coordinate - x;
02513 double diff_y = unreachable_frontiers.at(i).y_coordinate - y;
02514
02515 if (fabs(diff_x) <= MAX_GOAL_RANGE && fabs(diff_y) <= MAX_GOAL_RANGE) {
02516 ROS_DEBUG("x: %f y: %f too close to unreachable at x: %f y: %f dif_x: %f dif_y: %f", x, y, unreachable_frontiers.at(i).x_coordinate, unreachable_frontiers.at(i).y_coordinate, diff_x, diff_y);
02517 return false;
02518 }
02519 }
02520 return true;
02521 }
02522 else
02523 {
02524 ROS_WARN("OUT OF HOME RANGE");
02525 return false;
02526 }
02527 }
02528
02529 void ExplorationPlanner::clearVisitedAndSeenFrontiersFromClusters()
02530 {
02531 ROS_INFO("Clear VisitedAndSeenFrontiers from Cluster");
02532 std::vector<int> goals_to_clear;
02533
02534
02535
02536 for(int i = 0; i < clusters.size(); i++)
02537 {
02538 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
02539 {
02540
02541
02542
02543 bool cluster_still_valid = false;
02544 for(int m = 0; m < frontiers.size(); m++)
02545 {
02546 if(clusters.at(i).cluster_element.at(j).id == frontiers.at(m).id)
02547 {
02548
02549 cluster_still_valid = true;
02550 break;
02551 }
02552 }
02553 if(cluster_still_valid == false)
02554 {
02555 clusters.at(i).cluster_element.erase(clusters.at(i).cluster_element.begin() +j);
02556 if(j > 0)
02557 j--;
02558 }
02559 }
02560 }
02561
02562 for(int i = 0; i < clusters.size(); i++)
02563 {
02564 if(clusters.at(i).cluster_element.size() <= 0)
02565 {
02566 clusters.erase(clusters.begin() + i);
02567 }
02568 }
02569
02570
02571
02572
02573
02574
02575
02576
02577
02578
02579
02580
02581
02582
02583
02584
02585
02586
02587
02588
02589
02590
02591
02592
02593
02594
02595
02596
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617
02618
02619
02620
02621
02622
02623
02624
02625
02626
02627
02628
02629
02630
02631
02632
02633
02634 ROS_INFO("Done");
02635 }
02636
02637
02638 void ExplorationPlanner::clearVisitedFrontiers()
02639 {
02640
02641
02642
02643
02644
02645
02646
02647
02648
02649
02650 std::vector<int> goals_to_clear;
02651
02652 for (int i = 1; i < visited_frontiers.size(); i++)
02653 {
02654 for (int j = 0; j < frontiers.size(); j++)
02655 {
02656 double diff_x = visited_frontiers.at(i).x_coordinate - frontiers.at(j).x_coordinate;
02657 double diff_y = visited_frontiers.at(i).y_coordinate - frontiers.at(j).y_coordinate;
02658
02659 if (fabs(diff_x) <= MAX_GOAL_RANGE && fabs(diff_y) <= MAX_GOAL_RANGE) {
02660 if(robot_prefix_empty_param == true)
02661 {
02662 removeStoredFrontier(frontiers.at(j).id, frontiers.at(j).detected_by_robot_str);
02663 if(j > 0)
02664 {
02665 j--;
02666 }
02667 }else
02668 {
02669 removeStoredFrontier(frontiers.at(j).id, "");
02670 if(j > 0)
02671 {
02672 j--;
02673 }
02674
02675 }
02676 break;
02677 }
02678 }
02679 }
02680
02681
02682
02683
02684
02685 }
02686
02687 void ExplorationPlanner::clearUnreachableFrontiers()
02688 {
02689
02690
02691
02692
02693
02694
02695
02696
02697
02698
02699 std::vector<int> goals_to_clear;
02700
02701 for (int i = 1; i < unreachable_frontiers.size(); i++)
02702 {
02703 for (int j = 0; j < frontiers.size(); j++)
02704 {
02705 double diff_x = unreachable_frontiers.at(i).x_coordinate - frontiers.at(j).x_coordinate;
02706 double diff_y = unreachable_frontiers.at(i).y_coordinate - frontiers.at(j).y_coordinate;
02707
02708 if (fabs(diff_x) <= 0.2 && fabs(diff_y) <= 0.2) {
02709
02710 if(robot_prefix_empty_param == true)
02711 {
02712 removeStoredFrontier(frontiers.at(j).id, frontiers.at(j).detected_by_robot_str);
02713 if(j > 0)
02714 {
02715 j--;
02716 }
02717 }else
02718 {
02719 removeStoredFrontier(frontiers.at(j).id, "");
02720 if(j > 0)
02721 {
02722 j--;
02723 }
02724
02725 }
02726 break;
02727 }
02728 }
02729 }
02730
02731
02732
02733
02734
02735 }
02736
02737 void ExplorationPlanner::clearSeenFrontiers(costmap_2d::Costmap2DROS *global_costmap)
02738 {
02739
02740 unsigned int mx, my, point;
02741 std::vector<int> neighbours, goals_to_clear;
02742
02743
02744
02745
02746
02747
02748
02749 if(frontiers.size() > 1)
02750 {
02751 for(int i = 0; i < frontiers.size(); i++)
02752 {
02753 unsigned int new_mx, new_my;
02754 bool unknown_found = false;
02755 bool obstacle_found = false;
02756 bool freespace_found = false;
02757
02758 mx = 0;
02759 my = 0;
02760
02761
02762 if(!global_costmap->getCostmap()->worldToMap(frontiers.at(i).x_coordinate,frontiers.at(i).y_coordinate,mx,my))
02763 {
02764 ROS_ERROR("Cannot convert coordinates successfully.");
02765 continue;
02766 }
02767
02768
02769 neighbours = getMapNeighbours(mx, my, 6);
02770
02771
02772 for (int j = 0; j < neighbours.size()/2; j++)
02773 {
02774
02775
02776
02777 new_mx = neighbours.at(j*2);
02778 new_my = neighbours.at(j*2+1);
02779
02780
02781
02782
02783 unsigned char cost = global_costmap->getCostmap()->getCost(new_mx, new_my);
02784
02785
02786 if(cost == costmap_2d::NO_INFORMATION)
02787 {
02788 unknown_found = true;
02789 }
02790 else if(cost == costmap_2d::FREE_SPACE)
02791 {
02792 freespace_found = true;
02793 }
02794 else if(cost == costmap_2d::LETHAL_OBSTACLE)
02795 {
02796 obstacle_found = true;
02797 }
02798
02799 }
02800
02801 if(unknown_found == false || obstacle_found == true || freespace_found == false)
02802 {
02803
02804
02805 if(robot_prefix_empty_param == true)
02806 {
02807 removeStoredFrontier(frontiers.at(i).id, frontiers.at(i).detected_by_robot_str);
02808 if(i > 0)
02809 {
02810 i--;
02811 }
02812 }else
02813 {
02814 removeStoredFrontier(frontiers.at(i).id, "");
02815 if(i > 0)
02816 {
02817 i--;
02818 }
02819 }
02820 seen_frontier_list.push_back(frontiers.at(i));
02821
02822 }
02823 }
02824
02825
02826
02827
02828
02829
02830
02831 }
02832 }
02833
02834 bool ExplorationPlanner::smartGoalBackoff(double x, double y, costmap_2d::Costmap2DROS *global_costmap, std::vector<double> *new_goal)
02835 {
02836 unsigned int mx, my, new_mx, new_my, inner_mx, inner_my;
02837 double wx, wy;
02838 std::vector<int> neighbours, inner_neighbours;
02839
02840
02841
02842
02843
02844 if(!global_costmap->getCostmap()->worldToMap(x,y,mx,my))
02845 {
02846 ROS_ERROR("Cannot convert coordinates successfully.");
02847 }
02848
02849
02850 neighbours = getMapNeighbours(mx, my, 40);
02851
02852 for (int j = 0; j< neighbours.size()/2; j++)
02853 {
02854
02855 new_mx = neighbours.at(j*2);
02856 new_my = neighbours.at(j*2+1);
02857
02858 unsigned char cost = global_costmap->getCostmap()->getCost(new_mx, new_my);
02859
02860 if( cost == costmap_2d::FREE_SPACE)
02861 {
02862 bool back_off_goal_found = true;
02863
02864 inner_neighbours = getMapNeighbours(new_mx, new_my, INNER_DISTANCE);
02865 for (int i = 0; i< inner_neighbours.size()/2; i++)
02866 {
02867
02868 inner_mx = inner_neighbours.at(i*2);
02869 inner_my = inner_neighbours.at(i*2+1);
02870
02871 unsigned char inner_cost = global_costmap->getCostmap()->getCost(inner_mx, inner_my);
02872 if( inner_cost != costmap_2d::FREE_SPACE)
02873 {
02874 back_off_goal_found = false;
02875 break;
02876 }
02877 }
02878
02879 if(back_off_goal_found == true)
02880 {
02881 global_costmap->getCostmap()->mapToWorld(new_mx, new_my, wx, wy);
02882 new_goal->push_back(wx);
02883 new_goal->push_back(wy);
02884 return true;
02885 }
02886 }
02887 }
02888 return false;
02889 }
02890
02891 std::vector<int> ExplorationPlanner::getMapNeighbours(unsigned int point_x, unsigned int point_y, int distance)
02892 {
02893 std::vector<int> neighbours;
02894
02895 for(int i = 0; i< distance; i++)
02896 {
02897 neighbours.push_back(point_x+i+1);
02898 neighbours.push_back(point_y);
02899
02900 neighbours.push_back(point_x-i-1);
02901 neighbours.push_back(point_y);
02902
02903 neighbours.push_back(point_x);
02904 neighbours.push_back(point_y+i+1);
02905
02906 neighbours.push_back(point_x);
02907 neighbours.push_back(point_y-i-1);
02908 }
02909 return neighbours;
02910 }
02911
02912
02913
02914
02915
02916
02917 void ExplorationPlanner::findFrontiers() {
02918
02919 ROS_INFO("Find Frontiers");
02920 allFrontiers.clear();
02921 int select_frontier = 1;
02922 std::vector<double> final_goal,start_points;
02923
02924
02925
02926
02927
02928
02929
02930
02931
02932
02933 int new_frontier_point = 0;
02934 for (unsigned int i = 0; i < num_map_cells_; i++) {
02935
02936 int new_frontier_point = isFrontier(i);
02937 if (new_frontier_point != 0) {
02938
02939
02940
02941
02942
02943
02944
02945
02946 allFrontiers.push_back(new_frontier_point);
02947 }
02948 }
02949 ROS_INFO("Found %lu frontier cells which are transformed into frontiers points. Starting transformation...", allFrontiers.size());
02950
02951
02952
02953
02954
02955
02956
02957
02958
02959
02960
02961
02962
02963 for (unsigned int i = 0; i < allFrontiers.size(); ++i) {
02964
02965 geometry_msgs::PoseStamped finalFrontier;
02966 double wx, wy, wx2, wy2, wx3, wy3;
02967 unsigned int mx, my, mx2, my2, mx3, my3;
02968 bool result;
02969
02970
02971 costmap_ros_->getCostmap()->indexToCells(allFrontiers.at(i), mx, my);
02972 costmap_ros_->getCostmap()->mapToWorld(mx, my, wx, wy);
02973
02974
02975
02976 if(select_frontier == 1)
02977 {
02978 result = true;
02979 for (unsigned int j = 0; j < frontiers.size(); j++)
02980 {
02981 if (fabs(wx - frontiers.at(j).x_coordinate) <= MAX_GOAL_RANGE && fabs(wy - frontiers.at(j).y_coordinate) <= MAX_GOAL_RANGE)
02982 {
02983 result = false;
02984 break;
02985 }
02986 }
02987 if (result == true)
02988 {
02989 storeFrontier(wx,wy,robot_name,robot_str,-1);
02990 }
02991 }
02992 else if(select_frontier == 2)
02993 {
02994 std::vector<int> neighbour_index;
02995
02996 for (unsigned int j = 0; j < allFrontiers.size(); ++j)
02997 {
02998 costmap_ros_->getCostmap()->indexToCells(allFrontiers[j], mx2, my2);
02999 costmap_ros_->getCostmap()->mapToWorld(mx2, my2, wx2, wy2);
03000
03001 if (fabs(wx - wx2) <= MINIMAL_FRONTIER_RANGE && fabs(wy - wy2) <= MINIMAL_FRONTIER_RANGE && fabs(wx - wx2) != 0 && fabs(wy - wy2) != 0)
03002 {
03003 neighbour_index.push_back(allFrontiers[j]);
03004 }
03005 }
03006
03007
03008 for (unsigned int n = 0; n < neighbour_index.size(); ++n)
03009 {
03010 costmap_ros_->getCostmap()->indexToCells(neighbour_index[n], mx2, my2);
03011 costmap_ros_->getCostmap()->mapToWorld(mx2, my2, wx2, wy2);
03012
03013 while(true)
03014 {
03015 bool end_point_found = true;
03016 for (unsigned int k = 0; k < allFrontiers.size(); ++k)
03017 {
03018 costmap_ros_->getCostmap()->indexToCells(allFrontiers[k], mx3, my3);
03019 costmap_ros_->getCostmap()->mapToWorld(mx3, my3, wx3, wy3);
03020
03021 if (fabs(wx2 - wx3) <= MINIMAL_FRONTIER_RANGE && fabs(wy2 - wy3) <= MINIMAL_FRONTIER_RANGE && wx2 != wx3 && wy2 != wy3 && wx != wx3 && wy != wy3)
03022 {
03023 wx = wx2;
03024 wy = wy2;
03025 wx2 = wx3;
03026 wy2 = wy3;
03027 end_point_found = false;
03028 }
03029 }
03030 if(end_point_found == true)
03031 {
03032 start_points.push_back(wx2);
03033 start_points.push_back(wy2);
03034
03035 break;
03036 }
03037 }
03038 }
03039 goal_buffer_x.push_back(start_points.at(0)+(start_points.at(2)-start_points.at(0))/2);
03040 goal_buffer_y.push_back(start_points.at(1)+(start_points.at(3)-start_points.at(1))/2);
03041 }
03042 }
03043
03044 ROS_INFO("Size of all frontiers in the list: %lu", frontiers.size());
03045 }
03046
03047
03048
03049 bool ExplorationPlanner::auctioning(std::vector<double> *final_goal, std::vector<int> *clusters_available_in_pool, std::vector<std::string> *robot_str_name)
03050 {
03051
03052 ROS_INFO("Start Auctioning");
03053
03054
03055
03056
03057 int timer_count = 0;
03058 number_of_auction_bids_received = 0;
03059 std::string robo_name;
03060
03061 float r = static_cast<float>(rand()) / static_cast<float>(RAND_MAX);
03062 float wait_if_auction_runs = r * 2;
03063
03064 if(wait_if_auction_runs < 0)
03065 wait_if_auction_runs = wait_if_auction_runs * (-1);
03066
03067 ROS_INFO("Waiting %f second if a auction is running",wait_if_auction_runs);
03068 ros::Duration(wait_if_auction_runs).sleep();
03069
03070 while(auction_running)
03071 {
03072 ROS_INFO("Waiting %f second because an auction is currently running",wait_if_auction_runs);
03073 ros::Duration(wait_if_auction_runs).sleep();
03074 float wait_if_auction_runs = r * 2;
03075 wait_if_auction_runs = wait_if_auction_runs - robot_name;
03076 if(wait_if_auction_runs < 0)
03077 wait_if_auction_runs = wait_if_auction_runs * (-1);
03078
03079 }
03080
03081
03082
03083
03084
03085
03086
03087
03088
03089
03090
03091
03092
03093
03094
03095
03096
03097
03098
03099
03100 if(auction_running)
03101 {
03102 ROS_INFO("Auction is running, leaving method");
03103 return false;
03104 }
03105 else
03106 {
03107 ROS_INFO("No auction is running, clearing");
03108 auction.clear();
03109 }
03110
03111
03112
03113 adhoc_communication::ExpAuction reqest_clusters, auction_msg;
03114
03115
03116
03117 auction_msg.start_auction = true;
03118 auction_msg.auction_finished = false;
03119 auction_msg.auction_status_message = true;
03120
03121 if(robot_prefix_empty_param == false)
03122 {
03123 std::stringstream ss;
03124 ss << robot_name;
03125 std::string prefix = "";
03126 robo_name = prefix.append(ss.str());
03127
03128 auction_msg.robot_name = robo_name;
03129 }else
03130 {
03131 auction_msg.robot_name = robot_str;
03132 robo_name = robot_str;
03133 }
03134
03135
03136
03137
03138
03139
03140
03141
03142
03143
03144
03145
03146
03147
03148
03149
03150
03151
03152 for(int i = 0; i < clusters.size(); i++)
03153 {
03154 adhoc_communication::ExpCluster cluster_request;
03155
03156
03157 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
03158 {
03159 adhoc_communication::ExpClusterElement cluster_request_element;
03160 if(robot_prefix_empty_param == true)
03161 {
03162 cluster_request_element.id = clusters.at(i).cluster_element.at(j).id;
03163 cluster_request_element.detected_by_robot_str = clusters.at(i).cluster_element.at(j).detected_by_robot_str;
03164 }else
03165 {
03166 cluster_request_element.id = clusters.at(i).cluster_element.at(j).id;
03167 }
03168 cluster_request.ids_contained.push_back(cluster_request_element);
03169 }
03170
03171 auction_msg.requested_clusters.push_back(cluster_request);
03172 }
03173
03174 if(robot_prefix_empty_param == true)
03175 {
03176 auction_msg.auction_id = auction_id_number;
03177 }else
03178 {
03179 auction_msg.auction_id = 10000*robot_name + auction_id_number;
03180 }
03181
03182
03183
03184
03185
03186
03187
03188
03189
03190
03191
03192
03193
03194
03195
03196
03197
03198
03199
03200
03201 std::string numbers_of_operating_robots;
03202
03203 ROS_INFO("Robot %d starting an auction", robot_name);
03204
03205
03206
03207
03208
03209
03210 sendToMulticastAuction("mc_", auction_msg, "auction");
03211
03212
03213
03214
03215
03216
03217
03218
03219
03220
03221
03222
03223
03224
03225
03226
03227
03228
03229
03230
03231
03232
03233
03234
03235
03236
03237
03238 ros::Duration(3).sleep();
03239
03240
03241
03242
03243
03244
03245
03246 ROS_INFO("number of auction bids received: %d", number_of_auction_bids_received);
03247
03248
03249 if(number_of_auction_bids_received > 0)
03250 number_of_completed_auctions++;
03251 else
03252 number_of_uncompleted_auctions++;
03253
03254
03255
03256
03257
03258
03259
03260
03261
03262
03263
03264
03265
03266 ROS_INFO("Selecting the most attractive cluster");
03267 bool cluster_selected_flag = selectClusterBasedOnAuction(final_goal, clusters_available_in_pool, robot_str_name);
03268
03269
03270
03271
03272 ROS_INFO("Stop the auction");
03273
03274
03275
03276 auction_msg.start_auction = false;
03277 auction_msg.auction_finished = true;
03278 auction_msg.auction_status_message = true;
03279 auction_msg.robot_name = robo_name;
03280 auction_msg.requested_clusters.clear();
03281
03282 if(cluster_selected_flag == true)
03283 {
03284
03285
03286
03287 std::vector<transform_point_t> occupied_ids;
03288 clusterIdToElementIds(final_goal->at(4), &occupied_ids);
03289 for(int i = 0; i < occupied_ids.size(); i++)
03290 {
03291
03292 adhoc_communication::ExpAuctionElement auction_element;
03293 auction_element.id = occupied_ids.at(i).id;
03294 auction_element.detected_by_robot_str = occupied_ids.at(i).robot_str;
03295 auction_msg.occupied_ids.push_back(auction_element);
03296 }
03297 }
03298
03299
03300
03301
03302
03303
03304
03305
03306
03307
03308
03309
03310 sendToMulticastAuction("mc_", auction_msg, "auction");
03311
03312
03313 first_run = false;
03314 auction_id_number++;
03315
03316 ROS_INFO("return %d", cluster_selected_flag);
03317 return (cluster_selected_flag);
03318 }
03319
03320
03321
03322 bool ExplorationPlanner::clusterIdToElementIds(int cluster_id, std::vector<transform_point_t>* occupied_ids)
03323 {
03324 for(int i = 0; i < clusters.size(); i++)
03325 {
03326 if(clusters.at(i).id == cluster_id)
03327 {
03328 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
03329 {
03330 transform_point_t point;
03331 point.id = clusters.at(i).cluster_element.at(j).id;
03332 point.robot_str = clusters.at(i).cluster_element.at(j).detected_by_robot_str;
03333
03334 occupied_ids->push_back(point);
03335 }
03336 }
03337 }
03338 }
03339
03340 std::vector< std::vector<int> > array_to_matrix(int* m, int rows, int cols) {
03341 int i,j;
03342 std::vector< std::vector<int> > r;
03343 r.resize(rows, std::vector<int>(cols, 0));
03344
03345 for(i=0;i<rows;i++)
03346 {
03347 for(j=0;j<cols;j++)
03348 r[i][j] = m[i*cols+j];
03349 }
03350 return r;
03351 }
03352
03353
03354 bool ExplorationPlanner::selectClusterBasedOnAuction(std::vector<double> *goal, std::vector<int> *cluster_in_use_already_count, std::vector<std::string> *robot_str_name_to_return)
03355 {
03356 ROS_INFO("Select the best cluster based on auction bids");
03357
03358 int own_row_to_select_cluster = 0;
03359 int own_column_to_select = 0;
03360
03361
03362
03363
03364
03365
03366
03367
03368 int method_used = 2;
03369
03370 int count = 0;
03371 int auction_cluster_element_id = -1;
03372
03373 auction_element_t auction_elements;
03374 auction_pair_t auction_pair;
03375
03376
03377
03378
03379
03380 ROS_INFO("Cluster Size: %lu", clusters.size());
03381 for(int i = 0; i < clusters.size(); i++)
03382 {
03383 ROS_INFO("Calculate cluster with ID: %d", clusters.at(i).id);
03384 int my_auction_bid = calculateAuctionBID(clusters.at(i).id, trajectory_strategy);
03385 if(my_auction_bid == -1)
03386 {
03387 ROS_WARN("Own BID calculation failed");
03388 }
03389 ROS_INFO("Own bid calculated for cluster '%d' is '%d'", clusters.at(i).id, my_auction_bid);
03390 auction_pair.cluster_id = clusters.at(i).id;
03391 auction_pair.bid_value = my_auction_bid;
03392 auction_elements.auction_element.push_back(auction_pair);
03393 }
03394 auction_elements.robot_id = robot_name;
03395 auction.push_back(auction_elements);
03396
03397
03398
03399
03400
03401
03402 std::vector<int> clusters_to_erase;
03403 for(int j = 0; j < clusters.size(); j++)
03404 {
03405 for(int i = 0; i < already_used_ids.size(); i++)
03406 {
03407 if(already_used_ids.at(i) == clusters.at(j).id)
03408 {
03409
03410 for(int m = 0; m < auction.size(); m++)
03411 {
03412 for(int n = 0; n < auction.at(m).auction_element.size(); n++)
03413 {
03414 if(auction.at(m).auction_element.at(n).cluster_id == already_used_ids.at(i))
03415 {
03416 ROS_INFO("Already used Cluster %d found .... unconsider it",auction.at(m).auction_element.at(n).cluster_id);
03417
03418
03419 auction.at(m).auction_element.at(n).bid_value = 10000;
03420 }
03421 }
03422 }
03423 }
03424 }
03425 }
03426
03427
03428 ROS_INFO("Matrix");
03429
03430
03431
03432
03433 int col = 0, row = 0;
03434
03435 int robots_operating;
03436 nh.param<int>("robots_in_simulation",robots_operating, 1);
03437 ROS_INFO("robots operating: %d", robots_operating);
03438
03439 row = auction.size();
03440
03441
03442
03443
03444
03445
03446
03447
03448
03449
03450
03451
03452
03453
03454
03455
03456
03457
03458
03459 col = clusters.size();
03460 ROS_INFO("robots: %d clusters: %d", row, col);
03461
03462
03463
03464
03465 int max_size = std::max(col,row);
03466 boost::numeric::ublas::matrix<double> m(max_size,max_size);
03467
03468 ROS_INFO("Empty the matrix");
03469
03470
03471
03472 for(int i = 0; i < m.size1(); i++)
03473 {
03474 for(int j = 0; j < m.size2(); j++)
03475 {
03476 m(i,j) = 0;
03477 }
03478 }
03479
03480
03481 ROS_INFO("Fill matrix");
03482
03483
03484
03485
03486
03487
03488
03489
03490 for(int i = 0; i < row; i++)
03491 {
03492 ROS_INFO(" ");
03493 ROS_INFO("****** i: %d auction size: %lu ******", i, auction.size());
03494 for(int j = 0; j < col; j++)
03495 {
03496 ROS_INFO("j: %d cluster size: %lu", j, clusters.size());
03497 bool found_a_bid = false;
03498 bool cluster_valid_flag = false;
03499
03500
03501
03502 for(int n = 0; n < auction.at(i).auction_element.size(); n++)
03503 {
03504 cluster_valid_flag = true;
03505
03506
03507
03508 if(clusters.at(j).id == auction.at(i).auction_element.at(n).cluster_id && auction.at(i).auction_element.at(n).bid_value >= 0)
03509 {
03510
03511
03512
03513
03514
03515
03516
03517
03518
03519
03520
03521
03522
03523
03524
03525
03526
03527
03528
03529 m(j,i) = auction.at(i).auction_element.at(n).bid_value;
03530
03531
03532
03533
03534
03535 found_a_bid = true;
03536 break;
03537 }
03538
03539
03540
03541
03542
03543
03544
03545 }
03546
03547
03548
03549
03550
03551
03552
03553 ROS_INFO("Cluster elements checked, found BID: %d", found_a_bid);
03554
03555
03556
03557
03558
03559 if(found_a_bid == false)
03560 {
03561 ROS_INFO("No BID received ...");
03562 int distance = -1;
03563 other_robots_position_x = -1;
03564 other_robots_position_y = -1;
03565
03566 ROS_INFO("---- clusters: %lu element size: %lu robots positions: %lu ----", clusters.size(), clusters.at(j).cluster_element.size(), other_robots_positions.positions.size());
03567 for(int d = 0; d < clusters.at(j).cluster_element.size(); d++)
03568 {
03569 ROS_INFO("Access clusters.at(%d).cluster_element.at(%d)", j, d);
03570 position_mutex.lock();
03571
03572 for(int u = 0; u < other_robots_positions.positions.size(); u++)
03573 {
03574 adhoc_communication::MmPoint position_of_robot;
03575 position_of_robot = other_robots_positions.positions.at(u);
03576
03577 if(robot_prefix_empty_param == true)
03578 {
03579 ROS_INFO("position of robot: %s compare with auction robot: %s", position_of_robot.src_robot.c_str(), auction.at(i).detected_by_robot_str.c_str());
03580 if(position_of_robot.src_robot.compare(auction.at(i).detected_by_robot_str) == 0)
03581 {
03582 other_robots_position_x = position_of_robot.x;
03583 other_robots_position_y = position_of_robot.y;
03584
03585 break;
03586 }
03587 else
03588 {
03589 ROS_ERROR("Unable to look up robots position!");
03590 }
03591 }else
03592 {
03593 int robots_int_id = atoi(position_of_robot.src_robot.substr(6,1).c_str());
03594 ROS_INFO("Robots int id: %d", robots_int_id);
03595 if(auction.at(i).robot_id == robots_int_id)
03596 {
03597 other_robots_position_x = position_of_robot.x;
03598 other_robots_position_y = position_of_robot.y;
03599
03600 ROS_INFO("Robots %d position_x: %f position_y: %f", robots_int_id, other_robots_position_x, other_robots_position_y);
03601 break;
03602 }else
03603 {
03604
03605 ROS_ERROR("Unable to look up robots position!");
03606 }
03607 }
03608 }
03609 position_mutex.unlock();
03610
03611
03612 if(other_robots_position_x != -1 && other_robots_position_y != -1)
03613 {
03614
03615 if(trajectory_strategy == "trajectory")
03616 {
03617 distance = estimate_trajectory_plan(other_robots_position_x, other_robots_position_y, clusters.at(j).cluster_element.at(d).x_coordinate, clusters.at(j).cluster_element.at(d).y_coordinate);
03618 ROS_INFO("estimated TRAJECTORY distance is: %d", distance);
03619
03620 }else if(trajectory_strategy == "euclidean" && j < clusters.size() && d < clusters.at(j).cluster_element.size())
03621 {
03622 double x = clusters.at(j).cluster_element.at(d).x_coordinate - other_robots_position_x;
03623 double y = clusters.at(j).cluster_element.at(d).y_coordinate - other_robots_position_y;
03624 distance = x * x + y * y;
03625 ROS_INFO("estimated EUCLIDEAN distance is: %d", distance);
03626
03627 if(distance != -1)
03628 break;
03629 }
03630 }
03631
03632
03633
03634
03635
03636
03637 if(distance != -1)
03638 {
03639 double x = clusters.at(j).cluster_element.at(d).x_coordinate - other_robots_position_x;
03640 double y = clusters.at(j).cluster_element.at(d).y_coordinate - other_robots_position_y;
03641 double euclidean_distance = x * x + y * y;
03642
03643
03644 if (distance * costmap_ros_->getCostmap()->getResolution() <= sqrt(euclidean_distance)*0.95)
03645 {
03646 ROS_ERROR("Euclidean distance is smaller then the trajectory path at recalculation");
03647 distance = -1;
03648 }else
03649 {
03650 break;
03651 }
03652 }
03653 }
03654 if(distance == -1)
03655 {
03656
03657
03658
03659
03660
03661 ROS_ERROR("Unable to calculate the others BID at all");
03662 m(j,i) = 10000;
03663 }
03664 else
03665 {
03666 ROS_INFO("Estimated trajectory length: %d", distance);
03667
03668
03669
03670
03671
03672
03673
03674
03675
03676
03677
03678
03679
03680
03681
03682
03683
03684
03685
03686 m(j,i) = distance;
03687
03688
03689
03690
03691 }
03692 }
03693 ROS_INFO("Column filled");
03694 }
03695 ROS_INFO("No columns left. Check next robot");
03696 }
03697
03698
03699 ROS_INFO("Completed");
03700
03701
03702
03703
03704
03705
03706 if(col < row)
03707 {
03708 ROS_ERROR("Number of clusters is less the number of robots. Select the closest");
03709 if(col == 0)
03710 {
03711 ROS_ERROR("No cluster anymore available");
03712
03713 return false;
03714 }
03715 method_used = 0;
03716 }
03717
03718
03719
03720
03721
03722
03723
03724 if(method_used == 0)
03725 {
03726
03727
03728
03729
03730
03731
03732
03733
03734
03735 ROS_INFO("Columns left: %d", col);
03736
03737
03738
03739
03740
03741
03742
03743
03744
03745 if(auction.size() > 0)
03746 {
03747 if(auction.back().auction_element.size() > 0)
03748 {
03749 auction_cluster_element_id = auction.back().auction_element.front().cluster_id;
03750 }else
03751 {
03752 return false;
03753 }
03754 }
03755 }
03756 if(method_used == 1)
03757 {
03758
03759
03760
03761
03762
03763
03764 int mat[col*row];
03765 int counter = 0;
03766 for(int j = 0; j < col; j++)
03767 {
03768 for(int i = 0; i < row; i++)
03769 {
03770 mat[counter] = m(j,i);
03771 counter++;
03772 }
03773 }
03774
03775 std::vector< std::vector<int> > m2 = array_to_matrix(mat,col,row);
03776
03777
03778
03779
03780
03781 own_row_to_select_cluster = row-1;
03782 ROS_INFO("Own row: %d", own_row_to_select_cluster);
03783
03784
03785 int r[3*3] = {14,15,15,30,1,95,22,14,12};
03786 std::vector< std::vector<int> > m3 = array_to_matrix(r,3,3);
03787
03788
03789 Hungarian hungarian(m2, col, row, HUNGARIAN_MODE_MINIMIZE_COST);
03790
03791
03792
03793 fprintf(stderr, "cost-matrix:");
03794 hungarian.print_cost();
03795
03796
03797 for(int i = 0; i < 5; i++)
03798 {
03799 if(hungarian.solve() == true)
03800 break;
03801 }
03802
03803 const std::vector< std::vector<int> > assignment = hungarian.assignment();
03804
03805
03806 fprintf(stderr, "assignment:");
03807 hungarian.print_assignment();
03808
03809
03810
03811
03812
03813
03814
03815
03816
03817
03818
03819 for(int i = 0; i < assignment.size(); i++)
03820 {
03821 if(assignment.at(i).at(own_row_to_select_cluster) == 1)
03822 {
03823 auction_cluster_element_id = clusters.at(i).id;
03824 ROS_INFO("Selected Cluster at position : %d %d",i ,own_row_to_select_cluster);
03825 break;
03826 }
03827 }
03828
03829 }
03830 if(method_used == 2)
03831 {
03832
03833 Matrix<double> mat = convert_boost_matrix_to_munkres_matrix<double>(m);
03834 ROS_INFO("Matrix (%ux%u):",mat.rows(),mat.columns());
03835
03836
03837 for ( int new_row = 0 ; new_row < mat.rows(); new_row++ ) {
03838 if(new_row > 9)
03839 {
03840 int rows_left = mat.rows() - new_row + 1;
03841 std::cout << "... (" << rows_left << " more)";
03842 break;
03843 }
03844 for ( int new_col = 0 ; new_col < mat.columns(); new_col++ ) {
03845 if(new_col > 9)
03846 {
03847 int columns_left = mat.columns() - new_col + 1;
03848 std::cout << "... (" << columns_left << " more)";
03849 break;
03850 }
03851 std::cout.width(2);
03852 std::cout << mat(new_row,new_col) << " ";
03853 }
03854 std::cout << std::endl;
03855 }
03856 std::cout << std::endl;
03857
03858
03859
03860
03861
03862
03863
03864
03866
03867
03868
03869
03870
03871
03872
03875
03876
03879
03880
03881
03882
03883
03884
03885
03886
03887
03888
03889
03890
03891
03892
03893
03894
03895
03896 Munkres munk;
03897 munk.solve(mat);
03898
03899 ROS_INFO("Solved :");
03900
03901 for ( int new_row = 0 ; new_row < mat.rows(); new_row++ ) {
03902 if(new_row > 9)
03903 {
03904 int rows_left = mat.rows() - new_row + 1;
03905 std::cout << "... (" << rows_left << " more)";
03906 break;
03907 }
03908 for ( int new_col = 0 ; new_col < mat.columns(); new_col++ ) {
03909 if(new_col > 9)
03910 {
03911 int columns_left = mat.columns() - new_col + 1;
03912 std::cout << "... (" << columns_left << " more)";
03913 break;
03914 }
03915 std::cout.width(2);
03916 std::cout << mat(new_row,new_col) << " ";
03917 }
03918 std::cout << std::endl;
03919 }
03920
03921 std::cout << std::endl;
03922
03923
03924 own_row_to_select_cluster = row-1;
03925 for(int i = 0; i < mat.columns(); i++)
03926 {
03927 if(mat(i,own_row_to_select_cluster) == 1)
03928 {
03929 auction_cluster_element_id = clusters.at(i).id;
03930 own_column_to_select = i;
03931 ROS_INFO("Selected Cluster at position : %d %d with BID: %f",i ,own_row_to_select_cluster, mat(i, own_row_to_select_cluster));
03932 break;
03933 }
03934 }
03935
03936
03937
03938
03939 }
03940
03941
03942
03943
03944
03945
03946
03947
03948
03949
03950
03951 if(auction_cluster_element_id != -1)
03952 {
03953
03954
03955
03956
03957
03958
03959
03960
03961
03962
03963
03964
03965
03966
03967 while(ros::ok())
03968 {
03969 ROS_INFO("Try to determining goal");
03970 std::vector<double> new_goal;
03971 std::vector<std::string> robot_str_name;
03972 bool goal_determined = determine_goal(6, &new_goal, count, auction_cluster_element_id, &robot_str_name);
03973
03974
03975
03976 if(goal_determined == true)
03977 {
03978
03979
03980
03981
03982
03983
03984 double determined_goal_id = new_goal.at(4);
03985 bool used_cluster = false;
03986 for(int m = 0; m < already_used_ids.size(); m++)
03987 {
03988 if(determined_goal_id == already_used_ids.at(m))
03989 {
03990 ROS_INFO("Cluster in use already");
03991 cluster_in_use_already_count->push_back(1);
03992 used_cluster = true;
03993 count++;
03994 break;
03995 }
03996 }
03997 if(used_cluster == false)
03998 {
03999 ROS_INFO("Cluster was not used by any robot before");
04000 goal->push_back(new_goal.at(0));
04001 goal->push_back(new_goal.at(1));
04002 goal->push_back(new_goal.at(2));
04003 goal->push_back(new_goal.at(3));
04004 goal->push_back(new_goal.at(4));
04005
04006 robot_str_name_to_return->push_back(robot_str_name.at(0));
04007 return true;
04008 }
04009
04010 }else
04011 {
04012 if(clusters.size() <= count)
04013 {
04014 ROS_INFO("Not possible to select any goal from the available clusters");
04015 return false;
04016 }else
04017 {
04018 ROS_INFO("Current cluster empty ... select next one");
04019 }
04020 count++;
04021 }
04022 }
04023 return false;
04024 }else
04025 {
04026
04027
04028
04029
04030 ROS_INFO("Auction has failed, no auction elements are existent. Choosing nearest cluster");
04031 std::vector<double> new_goal;
04032 std::vector<std::string> robot_str_name;
04033 bool goal_determined = determine_goal(4, &new_goal, count, -1, &robot_str_name);
04034 if(goal_determined == true)
04035 {
04036 goal->push_back(new_goal.at(0));
04037 goal->push_back(new_goal.at(1));
04038 goal->push_back(new_goal.at(2));
04039 goal->push_back(new_goal.at(3));
04040 goal->push_back(new_goal.at(4));
04041 return true;
04042 }else
04043 {
04044 return false;
04045 }
04046 }
04047 return false;
04048 }
04049
04050
04051 bool ExplorationPlanner::negotiate_Frontier(double x, double y, int detected_by, int id, int cluster_id_number)
04052 {
04053
04054 ROS_INFO("Negotiating Frontier with id: %d at Cluster: %d", id, cluster_id_number);
04055
04056 int cluster_vector_position = 0;
04057 for (int i = 0; i < clusters.size(); i++)
04058 {
04059 if(clusters.at(i).id == cluster_id_number)
04060 {
04061 cluster_vector_position = i;
04062 break;
04063 }
04064 }
04065
04066 ROS_DEBUG("cluster vector position: %d", cluster_vector_position);
04067
04068 bool entry_found = false;
04069 bool id_in_actual_cluster = false;
04070
04071 for(int i = 0; i< negotiation_list.size(); i++)
04072 {
04073 for(int k = 0; k < clusters.at(cluster_vector_position).cluster_element.size(); k++)
04074 {
04075 id_in_actual_cluster = false;
04076 if(negotiation_list.at(i).id == clusters.at(cluster_vector_position).cluster_element.at(k).id)
04077 {
04078 ROS_INFO(" Same ID detected");
04079 for(int j = 0; j < clusters.at(cluster_vector_position).cluster_element.size(); j++)
04080 {
04081 for(int m = 0; m < my_negotiation_list.size(); m++)
04082 {
04083 if(clusters.at(cluster_vector_position).cluster_element.at(j).id == my_negotiation_list.at(m).id)
04084 {
04085 ROS_INFO(" But is in the current working cluster! everything alright");
04086 id_in_actual_cluster = true;
04087 return true;
04088 }
04089 }
04090 }
04091
04092 double used_cluster_ids = 0;
04093 if(id_in_actual_cluster == false)
04094 {
04095 ROS_INFO("Checking how many goals in the cluster are already occupied");
04096 for(int n = 0; n < negotiation_list.size(); n++)
04097 {
04098 for(int m = 0; m < clusters.at(cluster_vector_position).cluster_element.size(); m++)
04099 {
04100 if(negotiation_list.at(n).id == clusters.at(cluster_vector_position).cluster_element.at(m).id)
04101 {
04102 used_cluster_ids++;
04103 }
04104 }
04105 }
04106
04107 if(double(used_cluster_ids / (double)clusters.at(cluster_vector_position).cluster_element.size()) >= 0.1)
04108 {
04109 ROS_INFO("Negotiation failed the other Robot got cluster %d already", cluster_id_number);
04110 entry_found = true;
04111 return false;
04112 }
04113 }
04114 }
04115 }
04116 }
04117 if(entry_found == false)
04118 {
04119 frontier_t new_frontier;
04120 new_frontier.x_coordinate = x;
04121 new_frontier.y_coordinate = y;
04122 new_frontier.detected_by_robot = detected_by;
04123 new_frontier.id = id;
04124
04125 publish_negotiation_list(new_frontier, cluster_id_number);
04126
04127 return true;
04128 }
04129 return false;
04130 }
04131
04132
04133 bool ExplorationPlanner::determine_goal(int strategy, std::vector<double> *final_goal, int count, int actual_cluster_id, std::vector<std::string> *robot_str_name)
04134 {
04135 if (!costmap_ros_->getRobotPose(robotPose))
04136 {
04137 ROS_ERROR("Failed to get RobotPose");
04138 }
04139
04140 if(strategy == 1)
04141 {
04142 for (int i = frontiers.size() - 1 - count; i >= 0; i--)
04143 {
04144 if (check_efficiency_of_goal(frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate) == true)
04145 {
04146 ROS_INFO("------------------------------------------------------------------");
04147 ROS_INFO("Determined frontier with ID: %d at x: %f y: %f detected by Robot %d", frontiers.at(i).id, frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate, frontiers.at(i).detected_by_robot);
04148
04149
04150
04151
04152
04153
04154
04155
04156 final_goal->push_back(frontiers.at(i).x_coordinate);
04157 final_goal->push_back(frontiers.at(i).y_coordinate);
04158 final_goal->push_back(frontiers.at(i).detected_by_robot);
04159 final_goal->push_back(frontiers.at(i).id);
04160
04161 robot_str_name->push_back(frontiers.at(i).detected_by_robot_str);
04162 return true;
04163 }
04164 }
04165 return false;
04166 }
04167
04168 else if(strategy == 2)
04169 {
04170 for (int i = 0 + count; i < frontiers.size(); i++)
04171 {
04172 if (check_efficiency_of_goal(frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate) == true)
04173 {
04174 ROS_INFO("------------------------------------------------------------------");
04175 ROS_INFO("Determined frontier with ID: %d at x: %f y: %f detected by Robot %d", frontiers.at(i).id, frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate, frontiers.at(i).detected_by_robot);
04176
04177 final_goal->push_back(frontiers.at(i).x_coordinate);
04178 final_goal->push_back(frontiers.at(i).y_coordinate);
04179 final_goal->push_back(frontiers.at(i).detected_by_robot);
04180 final_goal->push_back(frontiers.at(i).id);
04181
04182 robot_str_name->push_back(frontiers.at(i).detected_by_robot_str);
04183 return true;
04184 }
04185 }
04186 }
04187 else if(strategy == 3)
04188 {
04189 while(true)
04190 {
04191 if(frontiers.size() > 0)
04192 {
04193
04194 int i = int(frontiers.size()*rand()/(RAND_MAX));
04195
04196 ROS_INFO("Random frontier ID: %d", frontiers.at(i).id);
04197
04198 if (check_efficiency_of_goal(frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate) == true)
04199 {
04200 ROS_INFO("------------------------------------------------------------------");
04201 ROS_INFO("Determined frontier with ID: %d at x: %f y: %f detected by Robot %d", frontiers.at(i).id, frontiers.at(i).x_coordinate, frontiers.at(i).y_coordinate, frontiers.at(i).detected_by_robot);
04202
04203 final_goal->push_back(frontiers.at(i).x_coordinate);
04204 final_goal->push_back(frontiers.at(i).y_coordinate);
04205 final_goal->push_back(frontiers.at(i).detected_by_robot);
04206 final_goal->push_back(frontiers.at(i).id);
04207
04208 robot_str_name->push_back(frontiers.at(i).detected_by_robot_str);
04209 return true;
04210 }
04211 }
04212 break;
04213 }
04214 }
04215 else if(strategy == 4)
04216 {
04217 int cluster_vector_position = 0;
04218
04219 if(actual_cluster_id != -1)
04220 {
04221 if(clusters.size() > 0)
04222 {
04223 for (int i = 0; i < clusters.size(); i++)
04224 {
04225 if(clusters.at(i).id == actual_cluster_id)
04226 {
04227 if(clusters.at(i).cluster_element.size() > 0)
04228 {
04229 cluster_vector_position = i;
04230 }
04231 break;
04232 }
04233 }
04234 }
04235 }
04236
04237 ROS_INFO("Calculated vector position of cluster %d", actual_cluster_id);
04238
04239
04240
04241
04242
04243
04244
04245 int nothing_found_in_actual_cluster = 0;
04246 int visited_clusters = 0;
04247 for (int i = 0 + count; i < clusters.size(); i++)
04248 {
04249
04250 i = i+ cluster_vector_position;
04251 i = i % (clusters.size());
04252 for (int j = 0; j < clusters.at(i).cluster_element.size(); j++)
04253 {
04254 if (check_efficiency_of_goal(clusters.at(i).cluster_element.at(j).x_coordinate, clusters.at(i).cluster_element.at(j).y_coordinate) == true)
04255 {
04256 ROS_INFO("------------------------------------------------------------------");
04257 ROS_INFO("Robot %d determined Goal: %d at Clusters: %d",robot_name, (int)clusters.at(i).cluster_element.at(j).id, (int)clusters.at(i).id);
04258
04259 final_goal->push_back(clusters.at(i).cluster_element.at(j).x_coordinate);
04260 final_goal->push_back(clusters.at(i).cluster_element.at(j).y_coordinate);
04261 final_goal->push_back(clusters.at(i).cluster_element.at(j).detected_by_robot);
04262 final_goal->push_back(clusters.at(i).cluster_element.at(j).id);
04263
04264
04265 final_goal->push_back(clusters.at(i).id);
04266 robot_str_name->push_back(clusters.at(i).cluster_element.at(j).detected_by_robot_str);
04267 return true;
04268 }
04269
04270 }
04271
04272 nothing_found_in_actual_cluster ++;
04273 visited_clusters ++;
04274
04275 if(nothing_found_in_actual_cluster == 1)
04276 {
04277
04278 i=0;
04279 cluster_vector_position = 0;
04280 }
04281
04282 if(visited_clusters == clusters.size())
04283 {
04284 break;
04285 }
04286 }
04287 }
04288 else if(strategy == 5)
04289 {
04290 int cluster_vector_position = 0;
04291 bool cluster_could_be_found = false;
04292
04293 if(actual_cluster_id != -1)
04294 {
04295 if(clusters.size() > 0)
04296 {
04297 for (int i = 0; i < clusters.size(); i++)
04298 {
04299 if(clusters.at(i).id == actual_cluster_id)
04300 {
04301 if(clusters.at(i).cluster_element.size() > 0)
04302 {
04303 cluster_vector_position = i;
04304 cluster_could_be_found = true;
04305 break;
04306 }
04307 }
04308 }
04309 }
04310 if(cluster_could_be_found == false)
04311 {
04312
04313
04314
04315 return false;
04316 }
04317 }
04318 else if(actual_cluster_id == -1)
04319 {
04320
04321 final_goal->push_back(-1.0);
04322 return false;
04323 }
04324 ROS_INFO("Calculated vector position: %d of cluster %d", cluster_vector_position, actual_cluster_id);
04325
04326
04327
04328
04329
04330
04331
04332 int nothing_found_in_actual_cluster = 0;
04333 int visited_clusters = 0;
04334
04335
04336
04337
04338 int position = (cluster_vector_position +count) % (clusters.size());
04339 for (int j = 0; j < clusters.at(position).cluster_element.size(); j++)
04340 {
04341 if(count >= clusters.size())
04342 {
04343 break;
04344 }
04345
04346 if(clusters.at(position).cluster_element.size() > 0)
04347 {
04348 if (check_efficiency_of_goal(clusters.at(position).cluster_element.at(j).x_coordinate, clusters.at(position).cluster_element.at(j).y_coordinate) == true)
04349 {
04350 ROS_INFO("------------------------------------------------------------------");
04351 ROS_INFO("Robot %d determined Goal: %d at Clusters: %d",robot_name, (int)clusters.at(position).cluster_element.at(j).id, (int)clusters.at(position).id);
04352
04353 final_goal->push_back(clusters.at(position).cluster_element.at(j).x_coordinate);
04354 final_goal->push_back(clusters.at(position).cluster_element.at(j).y_coordinate);
04355 final_goal->push_back(clusters.at(position).cluster_element.at(j).detected_by_robot);
04356 final_goal->push_back(clusters.at(position).cluster_element.at(j).id);
04357
04358
04359 final_goal->push_back(clusters.at(position).id);
04360 robot_str_name->push_back(clusters.at(position).cluster_element.at(j).detected_by_robot_str);
04361 return true;
04362 }
04363 }
04364
04365 }
04366
04367
04368
04369
04370 return false;
04371 }
04372 else if(strategy == 6)
04373 {
04374 int cluster_vector_position = 0;
04375 bool cluster_could_be_found = false;
04376
04377 if(actual_cluster_id != -1)
04378 {
04379 if(clusters.size() > 0)
04380 {
04381 for (int i = 0; i < clusters.size(); i++)
04382 {
04383 if(clusters.at(i).id == actual_cluster_id)
04384 {
04385 if(clusters.at(i).cluster_element.size() > 0)
04386 {
04387 cluster_vector_position = i;
04388 cluster_could_be_found = true;
04389 break;
04390 }
04391 }
04392 }
04393 }
04394 if(cluster_could_be_found == false)
04395 {
04396
04397
04398
04399 return false;
04400 }
04401 }
04402 else if(actual_cluster_id == -1)
04403 {
04404
04405 final_goal->push_back(-1.0);
04406 return false;
04407 }
04408 ROS_INFO("Calculated vector position: %d of cluster %d", cluster_vector_position, actual_cluster_id);
04409
04410
04411
04412
04413
04414
04415
04416 int nothing_found_in_actual_cluster = 0;
04417 int visited_clusters = 0;
04418
04419
04420 int position = (cluster_vector_position +count) % (clusters.size());
04421 for (int j = 0; j < clusters.at(position).cluster_element.size(); j++)
04422 {
04423 if(count >= clusters.size())
04424 {
04425 break;
04426 }
04427
04428 if(clusters.at(position).cluster_element.size() > 0)
04429 {
04430 if (check_efficiency_of_goal(clusters.at(position).cluster_element.at(j).x_coordinate, clusters.at(position).cluster_element.at(j).y_coordinate) == true)
04431 {
04432 ROS_INFO("------------------------------------------------------------------");
04433 ROS_INFO("Robot %d determined Goal: %d at Clusters: %d",robot_name, (int)clusters.at(position).cluster_element.at(j).id, (int)clusters.at(position).id);
04434
04435 final_goal->push_back(clusters.at(position).cluster_element.at(j).x_coordinate);
04436 final_goal->push_back(clusters.at(position).cluster_element.at(j).y_coordinate);
04437 final_goal->push_back(clusters.at(position).cluster_element.at(j).detected_by_robot);
04438 final_goal->push_back(clusters.at(position).cluster_element.at(j).id);
04439
04440
04441 final_goal->push_back(clusters.at(position).id);
04442 robot_str_name->push_back(clusters.at(position).cluster_element.at(j).detected_by_robot_str);
04443 return true;
04444 }
04445
04446
04447
04448
04449
04450 }
04451
04452 }
04453 return false;
04454 }
04455 return false;
04456 }
04457
04458 bool sortCluster(const ExplorationPlanner::cluster_t &lhs, const ExplorationPlanner::cluster_t &rhs)
04459 {
04460 if(lhs.cluster_element.size() > 1 && rhs.cluster_element.size() > 1)
04461 {
04462 return lhs.cluster_element.front().dist_to_robot < rhs.cluster_element.front().dist_to_robot;
04463 }
04464 }
04465
04466 void ExplorationPlanner::sort(int strategy)
04467 {
04468
04469
04470
04471
04472
04473
04474
04475 if(strategy == 1)
04476 {
04477 tf::Stamped < tf::Pose > robotPose;
04478 if (!costmap_ros_->getRobotPose(robotPose))
04479 {
04480 ROS_ERROR("Failed to get RobotPose");
04481 }
04482
04483 if (frontiers.size() > 0)
04484 {
04485 for (int i = frontiers.size(); i >= 0; i--) {
04486 for (int j = 0; j < frontiers.size() - 1; j++) {
04487 double x = frontiers.at(j).x_coordinate - robotPose.getOrigin().getX();
04488 double y = frontiers.at(j).y_coordinate - robotPose.getOrigin().getY();
04489 double x_next = frontiers.at(j+1).x_coordinate - robotPose.getOrigin().getX();
04490 double y_next = frontiers.at(j+1).y_coordinate - robotPose.getOrigin().getY();
04491 double euclidean_distance = x * x + y * y;
04492 double euclidean_distance_next = x_next * x_next + y_next * y_next;
04493
04494 if (sqrt(euclidean_distance) > sqrt(euclidean_distance_next)) {
04495 frontier_t temp = frontiers.at(j+1);
04496 frontiers.at(j + 1) = frontiers.at(j);
04497 frontiers.at(j) = temp;
04498 }
04499 }
04500 }
04501 } else
04502 {
04503 ROS_INFO("Sorting not possible, no frontiers available!!!");
04504 }
04505 }
04506 else if(strategy == 2)
04507 {
04508 tf::Stamped < tf::Pose > robotPose;
04509 if (!costmap_ros_->getRobotPose(robotPose))
04510 {
04511 ROS_ERROR("Failed to get RobotPose");
04512 }
04513
04514 if (frontiers.size() > 0)
04515 {
04516 for (int i = frontiers.size(); i >= 0; i--)
04517 {
04518 for (int j = 0; j < frontiers.size() - 1; j++) {
04519 double x = frontiers.at(j).x_coordinate - robotPose.getOrigin().getX();
04520 double y = frontiers.at(j).y_coordinate - robotPose.getOrigin().getY();
04521 double x_next = frontiers.at(j+1).x_coordinate - robotPose.getOrigin().getX();
04522 double y_next = frontiers.at(j+1).y_coordinate - robotPose.getOrigin().getY();
04523 double euclidean_distance = x * x + y * y;
04524 double euclidean_distance_next = x_next * x_next + y_next * y_next;
04525
04526 if (sqrt(euclidean_distance) > sqrt(euclidean_distance_next)) {
04527 frontier_t temp = frontiers.at(j+1);
04528 frontiers.at(j + 1) = frontiers.at(j);
04529 frontiers.at(j) = temp;
04530 }
04531 }
04532 }
04533 }else
04534 {
04535 ROS_INFO("Sorting not possible, no frontiers available!!!");
04536 }
04537 }
04538 else if(strategy == 3)
04539 {
04540 tf::Stamped < tf::Pose > robotPose;
04541 if (!costmap_ros_->getRobotPose(robotPose))
04542 {
04543 ROS_ERROR("Failed to get RobotPose");
04544 }
04545
04546 if (frontiers.size() > 0)
04547 {
04548 check_trajectory_plan();
04549
04550 for (int i = frontiers.size(); i >= frontiers.size()-10; i--)
04551 {
04552
04553
04554 if(frontiers.size()-i >= frontiers.size())
04555 {
04556 break;
04557 }
04558 else
04559 {
04560 for (int j = 0; j < 10-1; j++)
04561 {
04562 if(j >= frontiers.size())
04563 {
04564 break;
04565 }else
04566 {
04567
04568 int dist = frontiers.at(j).distance_to_robot;
04569 int dist_next = frontiers.at(j+1).distance_to_robot;
04570
04571 if (dist > dist_next) {
04572 frontier_t temp = frontiers.at(j+1);
04573 frontiers.at(j + 1) = frontiers.at(j);
04574 frontiers.at(j) = temp;
04575 }
04576 }
04577 }
04578 }
04579 }
04580 } else
04581 {
04582 ROS_INFO("Sorting not possible, no frontiers available!!!");
04583 }
04584 }
04585 else if(strategy == 4)
04586 {
04587 ROS_INFO("sort(4)");
04588 tf::Stamped < tf::Pose > robotPose;
04589 if (!costmap_ros_->getRobotPose(robotPose))
04590 {
04591 ROS_ERROR("Failed to get RobotPose");
04592 }
04593 double pose_x = robotPose.getOrigin().getX();
04594 double pose_y = robotPose.getOrigin().getY();
04595 if(clusters.size() > 0)
04596 {
04597 for(int cluster_number = 0; cluster_number < clusters.size(); cluster_number++)
04598 {
04599 if (clusters.at(cluster_number).cluster_element.size() > 0)
04600 {
04601 ROS_DEBUG("Cluster %d size: %lu",cluster_number, clusters.at(cluster_number).cluster_element.size());
04602 for (int i = clusters.at(cluster_number).cluster_element.size(); i > 0; i--)
04603 {
04604 ROS_DEBUG("Cluster element size: %d", i);
04605 for (int j = 0; j < clusters.at(cluster_number).cluster_element.size()-1; j++)
04606 {
04607 ROS_DEBUG("Cluster element number: %d", j);
04608 double x = clusters.at(cluster_number).cluster_element.at(j).x_coordinate - pose_x;
04609 double y = clusters.at(cluster_number).cluster_element.at(j).y_coordinate - pose_y;
04610 double x_next = clusters.at(cluster_number).cluster_element.at(j+1).x_coordinate - pose_x;
04611 double y_next = clusters.at(cluster_number).cluster_element.at(j+1).y_coordinate - pose_y;
04612 double euclidean_distance = x * x + y * y;
04613 double euclidean_distance_next = x_next * x_next + y_next * y_next;
04614
04615
04616
04617
04618
04619 if (sqrt(euclidean_distance) > sqrt(euclidean_distance_next))
04620 {
04621 frontier_t temp = clusters.at(cluster_number).cluster_element.at(j+1);
04622 clusters.at(cluster_number).cluster_element.at(j+1) = clusters.at(cluster_number).cluster_element.at(j);
04623 clusters.at(cluster_number).cluster_element.at(j) = temp;
04624 }
04625 }
04626 }
04627 }else
04628 {
04629
04630 }
04631 }
04632 }else
04633 {
04634 ROS_INFO("Sorting not possible, no clusters available!!!");
04635 }
04636 }
04637 else if(strategy == 5)
04638 {
04639 ROS_INFO("sort(5)");
04640 tf::Stamped < tf::Pose > robotPose;
04641 if (!costmap_ros_->getRobotPose(robotPose))
04642 {
04643 ROS_ERROR("Failed to get RobotPose");
04644 }
04645 double pose_x = robotPose.getOrigin().getX();
04646 double pose_y = robotPose.getOrigin().getY();
04647
04648 ROS_DEBUG("Iterating over all cluster elements");
04649
04650 for(int i = 0; i < clusters.size(); i++)
04651 {
04652 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
04653 {
04654 clusters.at(i).cluster_element.at(j).dist_to_robot = 0;
04655 }
04656 }
04657
04658 for(int i = 0; i < clusters.size(); i++)
04659 {
04660 if(clusters.at(i).cluster_element.size() > 0)
04661 {
04662 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
04663 {
04664
04665 double x = clusters.at(i).cluster_element.at(j).x_coordinate - pose_x;
04666 double y = clusters.at(i).cluster_element.at(j).y_coordinate - pose_y;
04667 double euclidean_distance = x * x + y * y;
04668 int distance = euclidean_distance;
04669
04670 clusters.at(i).cluster_element.at(j).dist_to_robot = sqrt(distance);
04671
04672
04673
04674
04675
04676 }
04677 }else
04678 {
04679 ROS_DEBUG("Erasing Cluster: %d", clusters.at(i).id);
04680 cluster_mutex.lock();
04681 clusters.erase(clusters.begin() + i);
04682 cluster_mutex.unlock();
04683 if(i > 0)
04684 {
04685 i --;
04686 }
04687 }
04688 }
04689 ROS_INFO("Starting to sort the clusters itself");
04690 std::sort(clusters.begin(), clusters.end(), sortCluster);
04691
04692
04693
04694
04695
04696
04697
04699
04700
04701
04702
04703
04704
04705
04706
04707
04708
04709
04711
04712
04713
04716
04718
04719
04720
04721
04722
04723
04724
04725
04726
04727
04728
04729
04730
04731 }
04732 else if(strategy == 6)
04733 {
04734 ROS_INFO("sort(6)");
04735 tf::Stamped < tf::Pose > robotPose;
04736 if (!costmap_ros_->getRobotPose(robotPose))
04737 {
04738 ROS_ERROR("Failed to get RobotPose");
04739 }
04740 double pose_x = robotPose.getOrigin().getX();
04741 double pose_y = robotPose.getOrigin().getY();
04742
04743 ROS_DEBUG("Iterating over all cluster elements");
04744
04745 for(int i = 0; i < clusters.size(); i++)
04746 {
04747 if(clusters.at(i).cluster_element.size() > 0)
04748 {
04749 for(int j = 0; j < clusters.at(i).cluster_element.size(); j++)
04750 {
04751 random_value = int(rand() % 100);
04752 clusters.at(i).cluster_element.at(j).dist_to_robot = random_value;
04753 }
04754 }else
04755 {
04756 ROS_DEBUG("Erasing Cluster: %d", clusters.at(i).id);
04757 clusters.erase(clusters.begin() + i);
04758 if(i > 0)
04759 {
04760 i --;
04761 }
04762 }
04763 }
04764 ROS_DEBUG("Starting to sort the clusters itself");
04765 std::sort(clusters.begin(), clusters.end(), sortCluster);
04766 }
04767
04768 ROS_INFO("Done sorting");
04769
04770
04771
04772
04773 }
04774
04775 void ExplorationPlanner::simulate() {
04776
04777 geometry_msgs::PointStamped goalPoint;
04778
04779
04780 tf::Stamped < tf::Pose > robotPose;
04781 if (!costmap_ros_->getRobotPose(robotPose))
04782 {
04783 ROS_ERROR("Failed to get RobotPose");
04784 }
04785
04786
04787 for (int i = frontiers.size() - 1; i >= 0; i--) {
04788 goalPoint.header.seq = i + 1;
04789 goalPoint.header.stamp = ros::Time::now();
04790 goalPoint.header.frame_id = "map";
04791 goalPoint.point.x = frontiers.at(i).x_coordinate;
04792 goalPoint.point.y = frontiers.at(i).y_coordinate;
04793
04794 pub_Point.publish < geometry_msgs::PointStamped > (goalPoint);
04795
04796 ros::Duration(1.0).sleep();
04797 }
04798 }
04799
04800 void ExplorationPlanner::clear_Visualized_Cluster_Cells(std::vector<int> ids)
04801 {
04802 nav_msgs::GridCells clearing_cell;
04803 geometry_msgs::Point point;
04804
04805 clearing_cell.header.frame_id = move_base_frame;
04806 clearing_cell.header.stamp = ros::Time::now();
04807 clearing_cell.cell_height = 0.1;
04808 clearing_cell.cell_width = 0.1;
04809 clearing_cell.header.seq = cluster_cells_seq_number++;
04810
04811 point.x = 1000;
04812 point.y = 1000;
04813 point.z = 1000;
04814 clearing_cell.cells.push_back(point);
04815
04816 for(int i = 0; i < 20; i++)
04817 {
04818 bool used_id = false;
04819 for(int n = 0; n < ids.size(); n++)
04820 {
04821 if(ids.at(n) == i)
04822 {
04823 used_id = true;
04824 break;
04825 }
04826 }
04827 if(used_id == false)
04828 {
04829 if(i == 0)
04830 {
04831 pub_cluster_grid_0.publish<nav_msgs::GridCells>(clearing_cell);
04832 }
04833 if(i == 1)
04834 {
04835 pub_cluster_grid_1.publish<nav_msgs::GridCells>(clearing_cell);
04836 }
04837 if(i == 2)
04838 {
04839 pub_cluster_grid_2.publish<nav_msgs::GridCells>(clearing_cell);
04840 }
04841 if(i == 3)
04842 {
04843 pub_cluster_grid_3.publish<nav_msgs::GridCells>(clearing_cell);
04844 }
04845 if(i == 4)
04846 {
04847 pub_cluster_grid_4.publish<nav_msgs::GridCells>(clearing_cell);
04848 }
04849 if(i == 5)
04850 {
04851 pub_cluster_grid_5.publish<nav_msgs::GridCells>(clearing_cell);
04852 }
04853 if(i == 6)
04854 {
04855 pub_cluster_grid_6.publish<nav_msgs::GridCells>(clearing_cell);
04856 }
04857 if(i == 7)
04858 {
04859 pub_cluster_grid_7.publish<nav_msgs::GridCells>(clearing_cell);
04860 }
04861 if(i == 8)
04862 {
04863 pub_cluster_grid_8.publish<nav_msgs::GridCells>(clearing_cell);
04864 }
04865 if(i == 9)
04866 {
04867 pub_cluster_grid_9.publish<nav_msgs::GridCells>(clearing_cell);
04868 }
04869
04870 if(i == 10)
04871 {
04872 pub_cluster_grid_10.publish<nav_msgs::GridCells>(clearing_cell);
04873 }
04874 if(i == 11)
04875 {
04876 pub_cluster_grid_11.publish<nav_msgs::GridCells>(clearing_cell);
04877 }
04878 if(i == 12)
04879 {
04880 pub_cluster_grid_12.publish<nav_msgs::GridCells>(clearing_cell);
04881 }
04882 if(i == 13)
04883 {
04884 pub_cluster_grid_13.publish<nav_msgs::GridCells>(clearing_cell);
04885 }
04886 if(i == 14)
04887 {
04888 pub_cluster_grid_14.publish<nav_msgs::GridCells>(clearing_cell);
04889 }
04890 if(i == 15)
04891 {
04892 pub_cluster_grid_15.publish<nav_msgs::GridCells>(clearing_cell);
04893 }
04894 if(i == 16)
04895 {
04896 pub_cluster_grid_16.publish<nav_msgs::GridCells>(clearing_cell);
04897 }
04898 if(i == 17)
04899 {
04900 pub_cluster_grid_17.publish<nav_msgs::GridCells>(clearing_cell);
04901 }
04902 if(i == 18)
04903 {
04904 pub_cluster_grid_18.publish<nav_msgs::GridCells>(clearing_cell);
04905 }
04906 if(i == 19)
04907 {
04908 pub_cluster_grid_19.publish<nav_msgs::GridCells>(clearing_cell);
04909 }
04910 }
04911 }
04912 }
04913
04914
04915 void ExplorationPlanner::visualize_Cluster_Cells()
04916 {
04917 ROS_INFO("Visualize clusters");
04918
04919 std::vector<int> used_ids;
04920 for(int i= 0; i< clusters.size(); i++)
04921 {
04922 if(clusters.at(i).cluster_element.size() > 0)
04923 {
04924 nav_msgs::GridCells cluster_cells;
04925 geometry_msgs::Point point;
04926 for(int j= 0; j < clusters.at(i).cluster_element.size(); j++)
04927 {
04928 point.x = clusters.at(i).cluster_element.at(j).x_coordinate;
04929 point.y = clusters.at(i).cluster_element.at(j).y_coordinate;
04930 point.z = 0;
04931
04932 cluster_cells.cells.push_back(point);
04933 }
04934 cluster_cells.header.frame_id = move_base_frame;
04935 cluster_cells.header.stamp = ros::Time::now();
04936 cluster_cells.cell_height = 0.5;
04937 cluster_cells.cell_width = 0.5;
04938 cluster_cells.header.seq = cluster_cells_seq_number++;
04939
04940 used_ids.push_back(clusters.at(i).id % 20);
04941
04942 if(clusters.at(i).id % 20 == 0)
04943 {
04944 pub_cluster_grid_0.publish<nav_msgs::GridCells>(cluster_cells);
04945 }
04946 else if(clusters.at(i).id % 20 == 1)
04947 {
04948 pub_cluster_grid_1.publish<nav_msgs::GridCells>(cluster_cells);
04949 }
04950 else if(clusters.at(i).id % 20 == 2)
04951 {
04952 pub_cluster_grid_2.publish<nav_msgs::GridCells>(cluster_cells);
04953 }
04954 else if(clusters.at(i).id % 20 == 3)
04955 {
04956 pub_cluster_grid_3.publish<nav_msgs::GridCells>(cluster_cells);
04957 }
04958 else if(clusters.at(i).id % 20 == 4)
04959 {
04960 pub_cluster_grid_4.publish<nav_msgs::GridCells>(cluster_cells);
04961 }
04962 else if(clusters.at(i).id % 20 == 5)
04963 {
04964 pub_cluster_grid_5.publish<nav_msgs::GridCells>(cluster_cells);
04965 }
04966 else if(clusters.at(i).id % 20 == 6)
04967 {
04968 pub_cluster_grid_6.publish<nav_msgs::GridCells>(cluster_cells);
04969 }
04970 else if(clusters.at(i).id % 20 == 7)
04971 {
04972 pub_cluster_grid_7.publish<nav_msgs::GridCells>(cluster_cells);
04973 }
04974 else if(clusters.at(i).id % 20 == 8)
04975 {
04976 pub_cluster_grid_8.publish<nav_msgs::GridCells>(cluster_cells);
04977 }
04978 else if(clusters.at(i).id % 20 == 9)
04979 {
04980 pub_cluster_grid_9.publish<nav_msgs::GridCells>(cluster_cells);
04981 }
04982
04983 if(clusters.at(i).id % 20 == 10)
04984 {
04985 pub_cluster_grid_10.publish<nav_msgs::GridCells>(cluster_cells);
04986 }
04987 else if(clusters.at(i).id % 20 == 11)
04988 {
04989 pub_cluster_grid_11.publish<nav_msgs::GridCells>(cluster_cells);
04990 }
04991 else if(clusters.at(i).id % 20 == 12)
04992 {
04993 pub_cluster_grid_12.publish<nav_msgs::GridCells>(cluster_cells);
04994 }
04995 else if(clusters.at(i).id % 20 == 13)
04996 {
04997 pub_cluster_grid_13.publish<nav_msgs::GridCells>(cluster_cells);
04998 }
04999 else if(clusters.at(i).id % 20 == 14)
05000 {
05001 pub_cluster_grid_14.publish<nav_msgs::GridCells>(cluster_cells);
05002 }
05003 else if(clusters.at(i).id % 20 == 15)
05004 {
05005 pub_cluster_grid_15.publish<nav_msgs::GridCells>(cluster_cells);
05006 }
05007 else if(clusters.at(i).id % 20 == 16)
05008 {
05009 pub_cluster_grid_16.publish<nav_msgs::GridCells>(cluster_cells);
05010 }
05011 else if(clusters.at(i).id % 20 == 17)
05012 {
05013 pub_cluster_grid_17.publish<nav_msgs::GridCells>(cluster_cells);
05014 }
05015 else if(clusters.at(i).id % 20 == 18)
05016 {
05017 pub_cluster_grid_18.publish<nav_msgs::GridCells>(cluster_cells);
05018 }
05019 else if(clusters.at(i).id % 20 == 19)
05020 {
05021 pub_cluster_grid_19.publish<nav_msgs::GridCells>(cluster_cells);
05022 }
05023 }
05024 }
05025 clear_Visualized_Cluster_Cells(used_ids);
05026 }
05027
05028
05029 void ExplorationPlanner::visualize_Clusters()
05030 {
05031 ROS_INFO("Visualize clusters");
05032
05033 geometry_msgs::PolygonStamped cluster_polygon;
05034
05035
05036 for(int i= 0; i< clusters.size(); i++)
05037 {
05038 double upper_left_x, upper_left_y;
05039 double upper_right_x, upper_right_y;
05040 double down_left_x, down_left_y;
05041 double down_right_x, down_right_y;
05042 double most_left, most_right, upper1, lower1, upper2, lower2;
05043
05044 for(int j= 0; j < clusters.at(i).cluster_element.size(); j++)
05045 {
05046 if(clusters.at(i).cluster_element.at(j).x_coordinate < most_left)
05047 {
05048 most_left = clusters.at(i).cluster_element.at(j).x_coordinate;
05049 if(clusters.at(i).cluster_element.at(j).y_coordinate < lower1)
05050 {
05051 lower1 = clusters.at(i).cluster_element.at(j).y_coordinate;
05052 down_left_x = clusters.at(i).cluster_element.at(j).x_coordinate;
05053 down_left_y = clusters.at(i).cluster_element.at(j).y_coordinate;
05054 }
05055 if(clusters.at(i).cluster_element.at(j).y_coordinate > upper1)
05056 {
05057 upper1 = clusters.at(i).cluster_element.at(j).y_coordinate;
05058 upper_left_x = clusters.at(i).cluster_element.at(j).x_coordinate;
05059 upper_left_y = clusters.at(i).cluster_element.at(j).y_coordinate;
05060 }
05061 }
05062 if(clusters.at(i).cluster_element.at(j).x_coordinate > most_right)
05063 {
05064 most_right = clusters.at(i).cluster_element.at(j).x_coordinate;
05065 if(clusters.at(i).cluster_element.at(j).y_coordinate < lower2)
05066 {
05067 lower2 = clusters.at(i).cluster_element.at(j).y_coordinate;
05068 down_right_x = clusters.at(i).cluster_element.at(j).x_coordinate;
05069 down_right_y = clusters.at(i).cluster_element.at(j).y_coordinate;
05070 }
05071 if(clusters.at(i).cluster_element.at(j).y_coordinate > upper2)
05072 {
05073 upper2 = clusters.at(i).cluster_element.at(j).y_coordinate;
05074 upper_right_x = clusters.at(i).cluster_element.at(j).x_coordinate;
05075 upper_right_y = clusters.at(i).cluster_element.at(j).y_coordinate;
05076 }
05077 }
05078
05079 }
05080
05081 geometry_msgs::Point32 polygon_point;
05082 polygon_point.x = upper_left_x;
05083 polygon_point.y = upper_left_y;
05084 polygon_point.z = 0;
05085 cluster_polygon.polygon.points.push_back(polygon_point);
05086
05087 polygon_point.x = upper_right_x;
05088 polygon_point.y = upper_right_y;
05089 polygon_point.z = 0;
05090 cluster_polygon.polygon.points.push_back(polygon_point);
05091
05092 polygon_point.x = down_left_x;
05093 polygon_point.y = down_left_y;
05094 polygon_point.z = 0;
05095 cluster_polygon.polygon.points.push_back(polygon_point);
05096
05097 polygon_point.x = down_right_x;
05098 polygon_point.y = down_right_y;
05099 polygon_point.z = 0;
05100 cluster_polygon.polygon.points.push_back(polygon_point);
05101
05102 cluster_polygon.header.frame_id = move_base_frame;
05103 cluster_polygon.header.stamp = ros::Time::now();
05104 cluster_polygon.header.seq = i+1;
05105
05106 pub_clusters.publish<geometry_msgs::PolygonStamped>(cluster_polygon);
05107
05108 }
05109 }
05110
05111
05112 void ExplorationPlanner::visualize_Frontiers()
05113 {
05114 visualization_msgs::MarkerArray markerArray;
05115
05116 for (int i = 0; i < frontiers.size(); i++)
05117 {
05118 visualization_msgs::Marker marker;
05119
05120 marker.header.frame_id = move_base_frame;
05121 marker.header.stamp = ros::Time::now();
05122 marker.header.seq = frontier_seq_number++;
05123 marker.ns = "my_namespace";
05124 marker.id = frontier_seq_number;
05125 marker.type = visualization_msgs::Marker::SPHERE;
05126 marker.action = visualization_msgs::Marker::ADD;
05127 marker.pose.position.x = frontiers.at(i).x_coordinate;
05128 marker.pose.position.y = frontiers.at(i).y_coordinate;
05129 marker.pose.position.z = 0;
05130 marker.pose.orientation.x = 0.0;
05131 marker.pose.orientation.y = 0.0;
05132 marker.pose.orientation.z = 0.0;
05133 marker.pose.orientation.w = 1.0;
05134 marker.scale.x = 0.2;
05135 marker.scale.y = 0.2;
05136 marker.scale.z = 0.2;
05137
05138 marker.color.a = 1.0;
05139
05140 if(frontiers.at(i).detected_by_robot == robot_name)
05141 {
05142 marker.color.r = 1.0;
05143 marker.color.g = 0.0;
05144 marker.color.b = 0.0;
05145 }
05146 if(frontiers.at(i).detected_by_robot == 1)
05147 {
05148 marker.color.r = 0.0;
05149 marker.color.g = 1.0;
05150 marker.color.b = 0.0;
05151 }
05152 if(frontiers.at(i).detected_by_robot == 2)
05153 {
05154 marker.color.r = 0.0;
05155 marker.color.g = 0.0;
05156 marker.color.b = 1.0;
05157 }
05158
05159 markerArray.markers.push_back(marker);
05160 }
05161
05162 pub_frontiers_points.publish <visualization_msgs::MarkerArray>(markerArray);
05163 }
05164
05165 void ExplorationPlanner::visualize_visited_Frontiers()
05166 {
05167 visualization_msgs::MarkerArray markerArray;
05168
05169 for (int i = 0; i < visited_frontiers.size(); i++)
05170 {
05171 visualization_msgs::Marker marker;
05172
05173 marker.header.frame_id = move_base_frame;
05174 marker.header.stamp = ros::Time::now();
05175 marker.header.seq = i+1;
05176 marker.ns = "my_namespace";
05177 marker.id = i+1;
05178 marker.type = visualization_msgs::Marker::SPHERE;
05179 marker.action = visualization_msgs::Marker::ADD;
05180 marker.pose.position.x = visited_frontiers.at(i).x_coordinate;
05181 marker.pose.position.y = visited_frontiers.at(i).y_coordinate;
05182 marker.pose.position.z = 0;
05183 marker.pose.orientation.x = 0.0;
05184 marker.pose.orientation.y = 0.0;
05185 marker.pose.orientation.z = 0.0;
05186 marker.pose.orientation.w = 1.0;
05187 marker.scale.x = 0.2;
05188 marker.scale.y = 0.2;
05189 marker.scale.z = 0.2;
05190
05191 marker.color.a = 1.0;
05192
05193 if(visited_frontiers.at(i).detected_by_robot == robot_name)
05194 {
05195 marker.color.a = 0.2;
05196 marker.color.r = 1.0;
05197 marker.color.g = 0.0;
05198 marker.color.b = 0.0;
05199 }
05200 if(visited_frontiers.at(i).detected_by_robot == 1)
05201 {
05202 marker.color.a = 0.2;
05203 marker.color.r = 0.0;
05204 marker.color.g = 1.0;
05205 marker.color.b = 0.0;
05206 }
05207 if(visited_frontiers.at(i).detected_by_robot == 2)
05208 {
05209 marker.color.a = 0.2;
05210 marker.color.r = 0.0;
05211 marker.color.g = 0.0;
05212 marker.color.b = 1.0;
05213 }
05214 if(visited_frontiers.at(i).detected_by_robot == 3)
05215 {
05216 marker.color.a = 0.2;
05217 marker.color.r = 0.5;
05218 marker.color.g = 0.0;
05219 marker.color.b = 1.0;
05220 }
05221 if(visited_frontiers.at(i).detected_by_robot == 4)
05222 {
05223 marker.color.a = 0.2;
05224 marker.color.r = 0.0;
05225 marker.color.g = 0.5;
05226 marker.color.b = 1.0;
05227 }
05228 if(visited_frontiers.at(i).detected_by_robot == 5)
05229 {
05230 marker.color.a = 0.2;
05231 marker.color.r = 0.0;
05232 marker.color.g = 0.0;
05233 marker.color.b = 0.5;
05234 }
05235 if(visited_frontiers.at(i).detected_by_robot == 6)
05236 {
05237 marker.color.a = 0.2;
05238 marker.color.r = 0.5;
05239 marker.color.g = 0.0;
05240 marker.color.b = 0.5;
05241 }
05242 if(visited_frontiers.at(i).detected_by_robot == 7)
05243 {
05244 marker.color.a = 0.2;
05245 marker.color.r = 0.0;
05246 marker.color.g = 0.5;
05247 marker.color.b = 0.5;
05248 }
05249 if(visited_frontiers.at(i).detected_by_robot == 8)
05250 {
05251 marker.color.a = 0.2;
05252 marker.color.r = 0.0;
05253 marker.color.g = 0.5;
05254 marker.color.b = 0.0;
05255 }
05256 if(visited_frontiers.at(i).detected_by_robot == 9)
05257 {
05258 marker.color.a = 0.2;
05259 marker.color.r = 0.5;
05260 marker.color.g = 0.5;
05261 marker.color.b = 0.0;
05262 }
05263 if(visited_frontiers.at(i).detected_by_robot == 10)
05264 {
05265 marker.color.a = 0.2;
05266 marker.color.r = 0.5;
05267 marker.color.g = 0.5;
05268 marker.color.b = 0.5;
05269 }
05270 markerArray.markers.push_back(marker);
05271 }
05272
05273 pub_visited_frontiers_points.publish <visualization_msgs::MarkerArray>(markerArray);
05274 }
05275
05276
05277 void ExplorationPlanner::setupMapData() {
05278
05279 if ((this->map_width_ != costmap_ros_->getCostmap()->getSizeInCellsX())
05280 || (this->map_height_ != costmap_ros_->getCostmap()->getSizeInCellsY())) {
05281 this->deleteMapData();
05282
05283 map_width_ = costmap_ros_->getCostmap()->getSizeInCellsX();
05284 map_height_ = costmap_ros_->getCostmap()->getSizeInCellsY();
05285 num_map_cells_ = map_width_ * map_height_;
05286
05287 ROS_INFO("Costmap size in cells: width:%d height: %d map_cells:%d",map_width_,map_height_,num_map_cells_);
05288
05289
05290
05291
05292
05293
05294
05295
05296 }
05297
05298
05299 occupancy_grid_array_ = costmap_ros_->getCostmap()->getCharMap();
05300
05301 for (unsigned int i = 0; i < num_map_cells_; i++) {
05302
05303 countCostMapBlocks(i);
05304
05305
05306
05307
05308
05309
05310
05311
05312 }
05313 ROS_INFO("--------------------- Iterate through Costmap --------------------");
05314 ROS_INFO("Free: %d Inflated: %d Lethal: %d unknown: %d rest: %d", free,
05315 inflated, lethal, unknown,
05316 num_map_cells_ - (free + inflated + lethal + unknown));
05317 ROS_INFO("------------------------------------------------------------------");
05318 free_space = free;
05319 }
05320
05321 void ExplorationPlanner::deleteMapData() {
05322 if (exploration_trans_array_) {
05323 delete[] exploration_trans_array_;
05324 exploration_trans_array_ = 0;
05325 }
05326 if (obstacle_trans_array_) {
05327 delete[] obstacle_trans_array_;
05328 obstacle_trans_array_ = 0;
05329 }
05330 if (is_goal_array_) {
05331 delete[] is_goal_array_;
05332 is_goal_array_ = 0;
05333 }
05334 if (frontier_map_array_) {
05335 delete[] frontier_map_array_;
05336 frontier_map_array_ = 0;
05337 }
05338 }
05339
05340 void ExplorationPlanner::resetMaps() {
05341 std::fill_n(exploration_trans_array_, num_map_cells_, INT_MAX);
05342 std::fill_n(obstacle_trans_array_, num_map_cells_, INT_MAX);
05343 std::fill_n(is_goal_array_, num_map_cells_, false);
05344 }
05345
05346 bool ExplorationPlanner::isSameFrontier(int frontier_point1,
05347 int frontier_point2) {
05348 unsigned int fx1, fy1;
05349 unsigned int fx2, fy2;
05350 double wfx1, wfy1;
05351 double wfx2, wfy2;
05352 costmap_.indexToCells(frontier_point1, fx1, fy1);
05353 costmap_.indexToCells(frontier_point2, fx2, fy2);
05354 costmap_.mapToWorld(fx1, fy1, wfx1, wfy1);
05355 costmap_.mapToWorld(fx2, fy2, wfx2, wfy2);
05356
05357 double dx = wfx1 - wfx2;
05358 double dy = wfy1 - wfy2;
05359
05360 if ((dx * dx) + (dy * dy)
05361 < (p_same_frontier_dist_ * p_same_frontier_dist_)) {
05362 return true;
05363 }
05364 return false;
05365 }
05366
05367
05368 double ExplorationPlanner::getYawToUnknown(int point) {
05369 int adjacentPoints[8];
05370 getAdjacentPoints(point, adjacentPoints);
05371
05372 int max_obs_idx = 0;
05373
05374 for (int i = 0; i < 8; ++i) {
05375 if (isValid(adjacentPoints[i])) {
05376 if (occupancy_grid_array_[adjacentPoints[i]]
05377 == costmap_2d::NO_INFORMATION) {
05378 if (obstacle_trans_array_[adjacentPoints[i]]
05379 > obstacle_trans_array_[adjacentPoints[max_obs_idx]]) {
05380 max_obs_idx = i;
05381 }
05382 }
05383 }
05384 }
05385
05386 int orientationPoint = adjacentPoints[max_obs_idx];
05387 unsigned int sx, sy, gx, gy;
05388 costmap_.indexToCells((unsigned int) point, sx, sy);
05389 costmap_.indexToCells((unsigned int) orientationPoint, gx, gy);
05390 int x = gx - sx;
05391 int y = gy - sy;
05392 double yaw = std::atan2(y, x);
05393
05394 return yaw;
05395
05396 }
05397
05398 int ExplorationPlanner::isFrontier(int point) {
05399
05400
05401 if (isFree(point)) {
05402
05403
05404
05405
05406
05407
05408
05409
05410
05411 int no_inf_count = 0;
05412 int inscribed_count = 0;
05413 int adjacent_points[16];
05414
05415
05416
05417
05418
05419
05420
05421
05422
05423
05424
05425
05426
05427
05428
05429
05430
05431
05432 if ((int) occupancy_grid_array_[point] == costmap_2d::FREE_SPACE)
05433 {
05434 getAdjacentPoints(point, adjacent_points);
05435 for (int i = 0; i < 16; i++)
05436 {
05437
05438 if (occupancy_grid_array_[adjacent_points[i]] == costmap_2d::NO_INFORMATION) {
05439 no_inf_count++;
05440
05441 }
05442 else if (occupancy_grid_array_[adjacent_points[i]] == costmap_2d::LETHAL_OBSTACLE) {
05443
05444
05445
05446
05447
05448
05449
05450
05451
05452
05453 return(0);
05454 }
05455 }
05456
05457
05458
05459
05460
05461
05462
05463
05464
05465 if (no_inf_count > 0)
05466 {
05467
05468
05469
05470
05471
05472
05473
05474
05475
05476
05477 ; return(point);
05478 }
05479 }
05480 }
05481 return(0);
05482 }
05483
05484
05485 int ExplorationPlanner::backoff (int point)
05486 {
05487 if(occupancy_grid_array_[up(point)] == costmap_2d::NO_INFORMATION && occupancy_grid_array_[left(point)] == costmap_2d::NO_INFORMATION)
05488 {
05489 return(downright(downright(downright(point))));
05490 }
05491 if(occupancy_grid_array_[up(point)] == costmap_2d::NO_INFORMATION && occupancy_grid_array_[right(point)] == costmap_2d::NO_INFORMATION)
05492 {
05493 return(downleft(downleft(downleft(point))));
05494 }
05495 if(occupancy_grid_array_[down(point)] == costmap_2d::NO_INFORMATION && occupancy_grid_array_[left(point)] == costmap_2d::NO_INFORMATION)
05496 {
05497 return(upright(upright(upright(point))));
05498 }
05499 if(occupancy_grid_array_[down(point)] == costmap_2d::NO_INFORMATION && occupancy_grid_array_[right(point)] == costmap_2d::NO_INFORMATION)
05500 {
05501 return(upleft(upleft(upleft(point))));
05502 }
05503
05504
05505 if(occupancy_grid_array_[right(point)] == costmap_2d::NO_INFORMATION)
05506 {
05507 return(left(left(left(point))));
05508 }
05509 if(occupancy_grid_array_[down(point)] == costmap_2d::NO_INFORMATION)
05510 {
05511 return(up(up(up(point))));
05512 }
05513 if(occupancy_grid_array_[left(point)] == costmap_2d::NO_INFORMATION)
05514 {
05515 return(right(right(right(point))));
05516 }
05517 if(occupancy_grid_array_[up(point)] == costmap_2d::NO_INFORMATION)
05518 {
05519 return(down(down(down(point))));
05520 }
05521
05522
05523
05524
05525
05526
05527
05528
05529
05530
05531
05532
05533
05534
05535
05536
05537
05538 else
05539 {
05540 return(point);
05541 }
05542 }
05543
05544 bool ExplorationPlanner::isFrontierReached(int point) {
05545
05546 tf::Stamped < tf::Pose > robotPose;
05547 if (!costmap_ros_->getRobotPose(robotPose)) {
05548 ROS_WARN("[isFrontierReached]: Failed to get RobotPose");
05549 }
05550 geometry_msgs::PoseStamped robotPoseMsg;
05551 tf::poseStampedTFToMsg(robotPose, robotPoseMsg);
05552
05553 unsigned int fx, fy;
05554 double wfx, wfy;
05555 costmap_.indexToCells(point, fx, fy);
05556 costmap_.mapToWorld(fx, fy, wfx, wfy);
05557
05558 double dx = robotPoseMsg.pose.position.x - wfx;
05559 double dy = robotPoseMsg.pose.position.y - wfy;
05560
05561 if ((dx * dx) + (dy * dy)
05562 < (p_dist_for_goal_reached_ * p_dist_for_goal_reached_)) {
05563
05564 return true;
05565 }
05566
05567 return false;
05568
05569 }
05570
05571 inline void ExplorationPlanner::getAdjacentPoints(int point, int points[]) {
05572
05573
05574
05575
05576
05577
05578
05579
05580
05581
05582
05583
05584
05585
05586
05587
05588
05589
05590
05591
05592
05593
05594
05595
05596
05597
05598
05599
05600
05601
05602
05603
05604
05605
05606
05607
05608
05609
05610
05611
05612
05613
05614
05615 points[0] = left(point);
05616 points[1] = up(point);
05617 points[2] = right(point);
05618 points[3] = down(point);
05619 points[4] = upleft(point);
05620 points[5] = upright(point);
05621 points[6] = downright(point);
05622 points[7] = downleft(point);
05623
05624
05625
05626
05627
05628
05629
05630
05631
05632 points[8] = left(points[0]);
05633 points[9] = up(points[1]);
05634 points[10] = right(points[2]);
05635 points[11] = down(points[3]);
05636 points[12] = upleft(points[4]);
05637 points[13] = upright(points[5]);
05638 points[14] = downright(points[6]);
05639 points[15] = downleft(points[7]);
05640
05641
05642
05643
05644
05645
05646
05647
05648
05649
05650
05651
05652
05653
05654
05655
05656
05657
05658
05659
05660
05661
05662
05663
05664
05665
05666
05667
05668
05669
05670
05671
05672
05673
05674
05675
05676
05677
05678
05679 }
05680
05681 bool ExplorationPlanner::countCostMapBlocks(int point) {
05682
05683 if (occupancy_grid_array_[point] == costmap_2d::NO_INFORMATION) {
05684
05685 unknown++;
05686 return true;
05687 } else if ((int) occupancy_grid_array_[point] == costmap_2d::FREE_SPACE) {
05688 free++;
05689
05690 return true;
05691 }
05692
05693 else if ((int) occupancy_grid_array_[point] == costmap_2d::LETHAL_OBSTACLE) {
05694 lethal++;
05695
05696 return true;
05697 } else if ((int) occupancy_grid_array_[point] == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
05698 inflated++;
05699
05700 return true;
05701 } else
05702 {
05703 int undefined = (unsigned char) occupancy_grid_array_[point];
05704
05705
05706 return true;
05707 }
05708 return false;
05709 }
05710
05711 bool ExplorationPlanner::isFree(int point) {
05712 if (isValid(point)) {
05713
05714 return true;
05715 } else {
05716 ROS_ERROR("Point is not valid! Reason: negative number ");
05717 }
05718 return false;
05719 }
05720
05721 inline bool ExplorationPlanner::isValid(int point) {
05722 return (point >= 0);
05723 }
05724
05725 void ExplorationPlanner::clearFrontiers() {
05726 std::fill_n(frontier_map_array_, num_map_cells_, 0);
05727 }
05728
05729 inline int ExplorationPlanner::left(int point) {
05730
05731 if ((point % map_width_ != 0)) {
05732 return point - 1;
05733 }
05734 return -1;
05735 }
05736
05737 inline int ExplorationPlanner::upleft(int point) {
05738 if ((point % map_width_ != 0) && (point >= (int) map_width_)) {
05739 return point - map_width_ - 1;
05740 }
05741 return -1;
05742
05743 }
05744 inline int ExplorationPlanner::up(int point) {
05745 if (point >= (int) map_width_) {
05746 return point - map_width_;
05747 }
05748 return -1;
05749 }
05750 inline int ExplorationPlanner::upright(int point) {
05751 if ((point >= (int) map_width_) && ((point + 1) % (int) map_width_ != 0)) {
05752 return point - map_width_ + 1;
05753 }
05754 return -1;
05755 }
05756 inline int ExplorationPlanner::right(int point) {
05757 if ((point + 1) % map_width_ != 0) {
05758 return point + 1;
05759 }
05760 return -1;
05761
05762 }
05763 inline int ExplorationPlanner::downright(int point) {
05764 if (((point + 1) % map_width_ != 0)
05765 && ((point / map_width_) < (map_width_ - 1))) {
05766 return point + map_width_ + 1;
05767 }
05768 return -1;
05769
05770 }
05771 inline int ExplorationPlanner::down(int point) {
05772 if ((point / map_width_) < (map_width_ - 1)) {
05773 return point + map_width_;
05774 }
05775 return -1;
05776
05777 }
05778 inline int ExplorationPlanner::downleft(int point) {
05779 if (((point / map_width_) < (map_width_ - 1))
05780 && (point % map_width_ != 0)) {
05781 return point + map_width_ - 1;
05782 }
05783 return -1;
05784
05785 }
05786