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


explorer
Author(s): Daniel Neuhold , Torsten Andre
autogenerated on Thu Jun 6 2019 20:59:53