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