explorer.cpp
Go to the documentation of this file.
00001 #include "ros/ros.h"
00002 #include "ExplorationPlanner.h"
00003 #include <ros/console.h>
00004 #include <ExplorationPlanner.h>
00005 #include <boost/lexical_cast.hpp>
00006 #include <move_base/MoveBaseConfig.h>
00007 #include <move_base_msgs/MoveBaseAction.h>
00008 #include <move_base_msgs/MoveBaseActionFeedback.h>
00009 #include <costmap_2d/costmap_2d_ros.h>
00010 //#include <costmap_2d/cell_data.h>
00011 #include <costmap_2d/costmap_2d.h>
00012 #include <costmap_2d/cost_values.h>
00013 #include <costmap_2d/costmap_2d_publisher.h>
00014 #include <costmap_2d/observation.h>
00015 #include <costmap_2d/observation_buffer.h>
00016 #include <tf/transform_listener.h>
00017 #include <std_msgs/String.h>
00018 //#include <costmap_2d/voxel_costmap_2d.h>
00019 #include <actionlib/client/simple_action_client.h>
00020 #include <boost/thread.hpp>
00021 #include <boost/thread/mutex.hpp>
00022 #include <iostream>
00023 #include <navfn/navfn_ros.h>
00024 //#include <sound_play/sound_play.h>
00025 #include <boost/filesystem.hpp>
00026 #include <map_merger/LogMaps.h>
00027 
00028 
00029 //#define PROFILE
00030 //#ifdef PROFILE
00031 //#include <google/profiler.h>
00032 //#include <google/heap-profiler.h>
00033 //#endif
00034 boost::mutex costmap_mutex;
00035 
00036 #define OPERATE_ON_GLOBAL_MAP true              // global or local costmap as basis for exploration
00037 #define OPERATE_WITH_GOAL_BACKOFF false // navigate to a goal point which is close to (but not exactly at) selected goal (in case selected goal is too close to a wall)
00038 
00039 void sleepok(int t, ros::NodeHandle &nh)
00040 {
00041     if (nh.ok())
00042     ros::Duration(1.0).sleep();
00043 }
00044 
00045 class Explorer {
00046 
00047 public:
00048 
00049         Explorer(tf::TransformListener& tf) :
00050         counter(0), rotation_counter(0), nh("~"), exploration_finished(false), number_of_robots(1), accessing_cluster(0), cluster_element_size(0),
00051         cluster_flag(false), cluster_element(-1), cluster_initialize_flag(false), global_iterattions(0), global_iterations_counter(0), 
00052         counter_waiting_for_clusters(0), global_costmap_iteration(0), robot_prefix_empty(false), robot_id(0){
00053 
00054         
00055                 nh.param("frontier_selection",frontier_selection,1); 
00056                 nh.param("local_costmap/width",costmap_width,0); 
00057                 nh.param("number_unreachable_for_cluster", number_unreachable_frontiers_for_cluster,3);
00058                 
00059                 ROS_INFO("Costmap width: %d", costmap_width);
00060 //                frontier_selection = atoi(param.c_str());
00061                 ROS_INFO("Frontier selection is set to: %d", frontier_selection);
00062                 //frontier_selection = 3;
00063                 Simulation = false;      
00064                 
00065 //#ifdef PROFILE
00066 //const char  fname[3] = "TS";
00067 //ProfilerStart(fname);
00068 //HeapProfilerStart(fname);
00069 //#endif
00070 
00071                 srand((unsigned)time(0));
00072                 
00073 //              sound_play::SoundClient sc;
00074 //              sleepok(0.5, nh);
00075 //              sc.say("Hello    World   I    am    Turtlebot");
00076                 
00077                 nh.param<std::string>("move_base_frame",move_base_frame,"map");  
00078                 nh.param<int>("wait_for_planner_result",waitForResult,3);
00079                 // determine host name
00080                 nh.param<std::string>("robot_prefix",robot_prefix,"");
00081                 ROS_INFO("robot prefix: \"%s\"", robot_prefix.c_str());
00082 
00083                 //char hostname_c[1024];
00084                 //hostname_c[1023] = '\0';
00085                 //gethostname(hostname_c, 1023);
00086                 //robot_name = std::string(hostname_c);
00087                
00088                 // create map_merger service
00089                 std::string service = robot_prefix + std::string("/map_merger/logOutput");
00090                 mm_log_client = nh.serviceClient<map_merger::LogMaps>(service.c_str());
00091                 
00092                 if(robot_prefix.empty())
00093                 {
00094                     char hostname_c[1024];
00095                     hostname_c[1023] = '\0';
00096                     gethostname(hostname_c, 1023);
00097                     robot_name = std::string(hostname_c);
00098                     ROS_INFO("NO SIMULATION! Robot name: %s", robot_name.c_str());
00099                                         
00100                     /*
00101                      * THIS IS REQUIRED TO PERFORM COORDINATED EXPLORATION
00102                      * Assign numbers to robot host names in order to make
00103                      * auctioning and frontier selection UNIQUE !!!!!
00104                      * To use explorer node on a real robot system, add your robot names 
00105                      * here and at ExplorationPlanner::lookupRobotName function ... 
00106                      */
00107                     std::string bob = "bob";
00108                     std::string marley = "marley";
00109                     std::string turtlebot = "turtlebot";
00110                     std::string joy = "joy";
00111                     std::string hans = "hans";
00112                     
00113                     if(robot_name.compare(turtlebot) == 0)
00114                         robot_id = 0;
00115                     if(robot_name.compare(joy) == 0)
00116                         robot_id = 1;
00117                     if(robot_name.compare(marley) == 0)
00118                         robot_id = 2;
00119                     if(robot_name.compare(bob) == 0)
00120                         robot_id = 3;
00121                     if(robot_name.compare(hans) == 0)
00122                         robot_id = 4;
00123                
00124                     robot_prefix_empty = true;
00125                     ROS_INFO("Robot name: %s    robot_id: %d", robot_name.c_str(), robot_id);
00126                 }else
00127                 {
00128                     robot_name = robot_prefix;
00129                     ROS_INFO("Move_base_frame: %s",move_base_frame.c_str());               
00130                     robot_id = atoi(move_base_frame.substr(7,1).c_str());
00131 
00132                     ROS_INFO("Robot: %d", robot_id);
00133                 } 
00134                 /*
00135                  * If you want to operate only 1 robot but stage is operating 2
00136                  * then simply kill one of them.
00137                  */
00138 //                if(robot_name == 1)
00139 //                {
00140 //                    ROS_ERROR("Shutting down ROBOT 1");
00141 //                    ros::shutdown();
00142 //                }
00143                 
00144                 
00145                /*
00146                 *  CREATE LOG PATH
00147                 * Following code enables to write the output to a file
00148                 * which is localized at the log_path
00149                 */
00150                 initLogPath();
00151                 csv_file = log_path + std::string("periodical.log");
00152                 log_file = log_path + std::string("exploration.log"); 
00153                 
00154                 ROS_DEBUG("*********************************************");
00155                 ROS_INFO("******* Initializing Simple Navigation ******");
00156                 ROS_DEBUG("                                             ");
00157 
00158                 ROS_DEBUG("Creating global costmap ...");
00159                 costmap2d_global = new costmap_2d::Costmap2DROS("global_costmap", tf);
00160                 ROS_DEBUG("Global costmap created ... now performing costmap -> pause");
00161                 costmap2d_global->pause();
00162                 ROS_DEBUG("Pausing performed");
00163 
00164                 ROS_DEBUG("                                             ");
00165 
00166                 if(OPERATE_ON_GLOBAL_MAP == true)
00167                 {
00168                     costmap2d_local_size = new costmap_2d::Costmap2DROS("local_costmap", tf);
00169                     costmap2d_local_size->pause();
00170                     ROS_DEBUG("Starting Global costmap ...");
00171                     costmap2d_global->start();
00172                     costmap2d_local_size->start();
00173                     
00174                     costmap2d_local = costmap2d_global;                   
00175                 }
00176                 else
00177                 {
00178                     ROS_INFO("Creating local costmap ...");
00179                     costmap2d_local = new costmap_2d::Costmap2DROS("local_costmap", tf);
00180                     ROS_INFO("Local costmap created ... now performing costmap -> pause");
00181                     costmap2d_local->pause();
00182                     ROS_INFO("Pausing performed");
00183                     ROS_INFO("Cost maps created");
00184 
00185                     ROS_INFO("                                             ");
00186 
00187                     ROS_INFO("Starting Global costmap ...");
00188                     costmap2d_global->start();
00189                     ROS_INFO("Starting Local costmap ... ");
00190                     costmap2d_local->start();
00191                     ROS_INFO("BOTH COSTMAPS STARTED AND RUNNING ...");
00192                 }
00193                 ROS_INFO("---------------- COSTMAP DONE ---------------");
00194 
00195                 /*
00196                  * Set the first goal as PointStamped message to visualize in RVIZ.
00197                  * RVIZ requires a minimal history length of 1, which means that at least
00198                  * one entry has to be buffered, before the first Goal is able to be
00199                  * visualized. Therefore set the "first" goal to the point of origin
00200                  * (home position).
00201                  */
00202  
00203                 if (!costmap2d_local->getRobotPose(robotPose)) {
00204                         ROS_ERROR("Failed to get RobotPose");
00205                 }
00206                 visualize_goal_point(robotPose.getOrigin().getX(),
00207                                 robotPose.getOrigin().getY());
00208 
00209                 // transmit three times, since rviz need at least 1 to buffer before visualizing the point
00210                 for (int i = 0; i <= 2; i++) {
00211                         visualize_home_point();
00212                 }
00213 
00214 
00215                 // instantiate the planner
00216                 exploration = new explorationPlanner::ExplorationPlanner(robot_id, robot_prefix_empty, robot_name);
00217                 
00218                 /*
00219                  * Define the first Goal. This is required to have at least one entry
00220                  * within the vector. Therefore set it to the home position.
00221                  */
00222                                
00223                 robot_home_position_x = robotPose.getOrigin().getX();
00224                 robot_home_position_y = robotPose.getOrigin().getY();
00225                 ROS_INFO("Set home point to (%lf,%lf).",robot_home_position_x,robot_home_position_y);
00226                
00227                 exploration->next_auction_position_x = robotPose.getOrigin().getX();
00228                 exploration->next_auction_position_y = robotPose.getOrigin().getY();
00229                 
00230                 exploration->storeVisitedFrontier(robot_home_position_x,robot_home_position_y, robot_id, robot_name, -1);                            
00231                 exploration->storeFrontier(robot_home_position_x,robot_home_position_y, robot_id, robot_name, -1);           
00232                
00233                 exploration->setRobotConfig(robot_id, robot_home_position_x, robot_home_position_y, move_base_frame);
00234                 
00235                 ROS_INFO("                                             ");
00236                 ROS_INFO("************* INITIALIZING DONE *************");
00237                 
00238         }
00239 
00240 
00241         void explore() 
00242         {
00243                 /*
00244                  * Sleep is required to get the actual 
00245                  * costmap updated with obstacle and inflated 
00246                  * obstacle information. This is rquired for the
00247                  * first time explore() is called.
00248                  */
00249                 ROS_DEBUG("Sleeping 5s for costmaps to be updated.");
00250                 geometry_msgs::Twist twi;
00251                 //should be a parameter!! only for testing on Wed/17/9/14
00252                 ros::Publisher twi_publisher = nh.advertise<geometry_msgs::Twist>("/Rosaria/cmd_vel",3);
00253                 twi.angular.z = 0.75;
00254                 twi_publisher.publish(twi);
00255                 ros::Duration(5.0).sleep();
00256                 twi_publisher.publish(twi);
00257                 /*
00258                  * START TAKING THE TIME DURING EXPLORATION     
00259                  */
00260                 time_start = ros::Time::now();
00261                 
00262                 
00263                 while (exploration_finished == false) 
00264                 {
00265                     Simulation == false; 
00266                     if(Simulation == false)
00267                     {
00268                         /*
00269                          * *****************************************************
00270                          * FRONTIER DETERMINATION
00271                          * *****************************************************
00272                          */
00273 
00274                             std::vector<double> final_goal;
00275                             std::vector<double> backoffGoal;
00276                             std::vector<std::string> robot_str;
00277                             
00278                             bool backoff_sucessfull = false, navigate_to_goal = false;
00279                             bool negotiation;
00280                             int count = 0;
00281 
00282                             ROS_INFO("****************** EXPLORE ******************");
00283 
00284                             /*
00285                              * Use mutex to lock the critical section (access to the costmap)
00286                              * since rosspin tries to update the costmap continuously
00287                              */
00288                             costmap_mutex.lock();
00289 
00290                             exploration->transformToOwnCoordinates_frontiers();
00291                             exploration->transformToOwnCoordinates_visited_frontiers();
00292 
00293 //                          ros::Duration(1.0).sleep();
00294                             exploration->initialize_planner("exploration planner", costmap2d_local, costmap2d_global);
00295                             exploration->findFrontiers();
00296 //                            exploration->printFrontiers();
00297                           
00298                             exploration->clearVisitedFrontiers();                       
00299                             exploration->clearUnreachableFrontiers();
00300                             exploration->clearSeenFrontiers(costmap2d_global);       
00301 
00302                             costmap_mutex.unlock();
00303 
00304                             exploration->publish_frontier_list();  
00305                             exploration->publish_visited_frontier_list();  
00306 
00307                             ros::Duration(1).sleep();
00308                             exploration->publish_frontier_list();  
00309                             exploration->publish_visited_frontier_list();  
00310 
00311                             /*
00312                              * Sleep to ensure that frontiers are exchanged
00313                              */
00314                             ros::Duration(1).sleep();
00315                            
00316                             /*
00317                              * *****************************************************
00318                              * FRONTIER SELECTION
00319                              * *****************************************************
00320                              */
00321  
00322                             /*********** EXPLORATION STRATEGY ************
00323                              * 0 ... Navigate to nearest frontier TRAVEL PATH
00324                              * 1 ... Navigate using auctioning with cluster selection using 
00325                              *       NEAREST selection (Kuhn-Munkres)
00326                              * 2 ... Navigate to furthest frontier
00327                              * 3 ... Navigate to nearest frontier EUCLIDEAN DISTANCE
00328                              * 4 ... Navigate to random Frontier
00329                              * 5 ... Cluster frontiers, then navigate to nearest cluster 
00330                              *       using EUCLIDEAN DISTANCE (with and without
00331                              *       negotiation)
00332                              * 6 ... Cluster frontiers, then navigate to random cluster
00333                              *       (with and without negotiation)
00334                              */
00335 
00336                             /******************** SORT *******************
00337                             * Choose which strategy to take.
00338                             * 1 ... Sort the buffer from furthest to nearest frontier
00339                             * 2 ... Sort the buffer from nearest to furthest frontier, normalized to the
00340                             *       robots actual position (EUCLIDEAN DISTANCE)
00341                             * 3 ... Sort the last 10 entries to shortest TRAVEL PATH
00342                             * 4 ... Sort all cluster elements from nearest to furthest (EUCLIDEAN DISTANCE)
00343                             */
00344                             if(frontier_selection < 0 || frontier_selection > 6)
00345                             {
00346                                 ROS_FATAL("You selected an invalid exploration strategy. Please make sure to set it in the interval [0,6]. The current value is %d.", frontier_selection);
00347                             }
00348                             if(frontier_selection == 2)
00349                             {
00350 
00351                                 exploration->sort(1); 
00352 
00353                                 /* 
00354                                  * Choose which strategy to take.
00355                                  * 1 ... least to first goal in list
00356                                  * 2 ... first to last goal in list
00357                                  *
00358                                  * Determine_goal() takes world coordinates stored in
00359                                  * the frontiers list(which contain every found frontier with sufficient
00360                                  * spacing in between) and returns the most attractive one, based on the
00361                                  * above defined strategies.
00362                                  */    
00363                                 while(true)
00364                                 {   
00365                                     goal_determined = exploration->determine_goal(1, &final_goal, count, 0, &robot_str);
00366                                     if(goal_determined == false)
00367                                     {
00368                                         break;
00369                                     }
00370                                     else
00371                                     {   
00372                                         //negotiation = exploration->negotiate_Frontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),final_goal.at(3),-1);
00373                                         negotiation = true; 
00374                                         if(negotiation == true)
00375                                         {
00376                                             break;
00377                                         }
00378                                         count++;
00379                                     }
00380                                 }
00381                             }    
00382                             else if(frontier_selection == 3)
00383                             {                           
00384                                 exploration->sort(2);
00385 
00386                                 while(true)
00387                                 {   
00388                                     goal_determined = exploration->determine_goal(2, &final_goal, count, 0, &robot_str);
00389                                     if(goal_determined == false)
00390                                     {
00391                                         break;
00392                                     }
00393                                     else
00394                                     {
00395                                         //negotiation = exploration->negotiate_Frontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),final_goal.at(3),-1);
00396                                         negotiation = true; 
00397                                         if(negotiation == true)
00398                                         {
00399                                             break;
00400                                         }
00401                                         count++;
00402                                     }
00403                                 }
00404                             }        
00405                             else if(frontier_selection == 0)
00406                             {
00407                                 exploration->sort(2);
00408                                 exploration->sort(3);
00409 
00410                                 while(true)
00411                                 {   
00412                                     goal_determined = exploration->determine_goal(2, &final_goal, count, 0, &robot_str);
00413                                     ROS_DEBUG("Goal_determined: %d   counter: %d",goal_determined, count);
00414                                     if(goal_determined == false)
00415                                     {
00416                                         break;
00417                                     }
00418                                     else
00419                                     {                        
00420                                         //negotiation = exploration->negotiate_Frontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),final_goal.at(3),-1);
00421                                         negotiation = true;
00422                                         if(negotiation == true)
00423                                         {
00424                                             break;
00425                                         }
00426                                         count++;
00427                                     }
00428                                 }
00429 
00430                             }
00431                             else if(frontier_selection == 4)
00432                             {
00433                                 while(true)
00434                                 {   
00435                                     goal_determined = exploration->determine_goal(3, &final_goal, count, 0, &robot_str);
00436                                     ROS_DEBUG("Goal_determined: %d   counter: %d",goal_determined, count);
00437                                     if(goal_determined == false)
00438                                     {
00439                                         break;
00440                                     }
00441                                     else
00442                                     {
00443                                         //negotiation = exploration->negotiate_Frontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),final_goal.at(3),-1);
00444                                         negotiation = true; 
00445                                         if(negotiation == true)
00446                                         {
00447                                             break;
00448                                         }
00449                                         count++;
00450                                     }
00451                                 }
00452 
00453                             }
00454                             else if(frontier_selection == 5)
00455                             {                            
00456                                 if(cluster_initialize_flag == true)
00457                                 {
00458                                     exploration->clearVisitedAndSeenFrontiersFromClusters();
00459                                 }
00460                                 else
00461                                 {
00462                                     /*
00463                                      * This is necessary for the first run 
00464                                      */
00465                                     if(robot_id == 0)
00466                                         ros::Duration(10).sleep();
00467 
00468                                     exploration->sort(2); 
00469                                 }
00470 
00471                                 exploration->clusterFrontiers();                 
00472                                 exploration->sort(4);      
00473                                 exploration->sort(5);
00474 
00475     //                            exploration->visualizeClustersConsole();
00476 
00477                                 clusters_available_in_pool.clear();
00478                                 while(true)
00479                                 {                             
00480                                     final_goal.clear();
00481                                     goal_determined = exploration->determine_goal(4, &final_goal, count, cluster_element, &robot_str);
00482 
00483                                     if(goal_determined == false)
00484                                     {
00485                                         ROS_INFO("Another cluster is not available, no cluster determined");                                    
00486                                         break;
00487                                     }
00488                                     else
00489                                     {
00490                                         if(cluster_initialize_flag == false)
00491                                         {
00492                                             /*
00493                                             *  TODO ... just to reduce the selection of the same 
00494                                             * clusters since no auctioning is implemented jet.
00495                                             */
00496                                            if(robot_id == 1)  
00497                                            {
00498                                                ros::Duration(5).sleep();
00499                                            }
00500 
00501                                            cluster_initialize_flag = true;
00502                                         }
00503 
00504                                         /*
00505                                          * If negotiation is not needed, simply uncomment
00506                                          * and set the negotiation to TRUE.
00507                                          */
00508                                         negotiation = exploration->negotiate_Frontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),final_goal.at(3),final_goal.at(4));
00509 //                                        negotiation = true; 
00510                                         if(negotiation == true)
00511                                         {
00512                                             ROS_DEBUG("Negotiation was successful");
00513                                             cluster_element = final_goal.at(4);
00514                                             counter_waiting_for_clusters = 0;
00515                                             break;
00516                                         }
00517                                         else
00518                                         {
00519                                             cluster_element = final_goal.at(4);
00520                                             counter_waiting_for_clusters++;
00521                                             ROS_WARN("Negotiation was not successful, try next cluster");
00522                                         }
00523                                         count++;
00524                                         /*
00525                                          * In order to make one robot wait until the other finds
00526                                          * another cluster to operate in, one have to fill the clusters_available_in_pool
00527                                          * vector if count is increased at least once which determines
00528                                          * that at least one cluster
00529                                          */
00530                                         clusters_available_in_pool.push_back(1);
00531                                     }
00532                                 }
00533 
00534                             } 
00535                             else if(frontier_selection == 6)
00536                             {                            
00537                                 if(cluster_initialize_flag == true)
00538                                 {
00539                                     exploration->clearVisitedAndSeenFrontiersFromClusters();
00540                                 }
00541 
00542                                 exploration->clusterFrontiers();   
00543                                 exploration->sort(6);
00544                                 exploration->visualizeClustersConsole();
00545 
00546                                 if(cluster_initialize_flag == false)
00547                                 {
00548                                     ros::Duration(2).sleep();
00549                                 }
00550                                 cluster_initialize_flag = true;
00551 
00552                                 while(true)
00553                                 {             
00554                                     goal_determined = exploration->determine_goal(4, &final_goal, count, cluster_element, &robot_str);
00555 
00556                                     if(goal_determined == false)
00557                                     {
00558                                         break;
00559                                     }
00560                                     else
00561                                     {   
00562                                         /*
00563                                          * If negotiation is not needed, simply uncomment
00564                                          * and set the negotiation to TRUE.
00565                                          */
00566                                         negotiation = exploration->negotiate_Frontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),final_goal.at(3),final_goal.at(4));
00567 //                                        negotiation = true; 
00568 
00569                                         if(negotiation == true)
00570                                         {
00571                                             cluster_element = final_goal.at(4);
00572                                             break;
00573                                         }
00574                                         else
00575                                         {
00576                                             cluster_element = int(exploration->clusters.size()*rand()/(RAND_MAX));
00577                                             ROS_INFO("Random cluster_element: %d  from available %lu clusters", cluster_element, exploration->clusters.size());
00578                                         }
00579                                         count++;
00580                                     }
00581                                 }
00582                             } 
00583                             else if(frontier_selection == 1)
00584                             {     
00585                                 costmap_mutex.lock();
00586                                 if(cluster_initialize_flag == true)
00587                                 {
00588                                      exploration->clearVisitedAndSeenFrontiersFromClusters();
00589                                 }
00590                                 else
00591                                 {                              
00592                                     /*
00593                                      * This is only necessary for the first run 
00594                                      */
00595 
00596                                     exploration->sort(2); 
00597                                     cluster_initialize_flag = true;
00598                                 }
00599 
00600                                 exploration->clusterFrontiers();                 
00601                                 exploration->sort(4);      
00602                                 exploration->sort(5);
00603 
00604                                 costmap_mutex.unlock();
00605 
00606                                 exploration->visualizeClustersConsole();
00607 
00608                                 while(true)
00609                                 {                             
00610                                     final_goal.clear();
00611                                     robot_str.clear();
00612                                     goal_determined = exploration->determine_goal(5, &final_goal, 0, cluster_element, &robot_str);
00613                                     
00614                                     ROS_DEBUG("Cluster element: %d", cluster_element);
00615                                     
00616                                     int cluster_vector_position = -1;
00617  
00618                                     if(cluster_element != -1)
00619                                     {
00620                                         if(exploration->clusters.size() > 0)
00621                                         {                   
00622                                             for (int i = 0; i < exploration->clusters.size(); i++)
00623                                             {
00624                                                 if(exploration->clusters.at(i).id == cluster_element)
00625                                                 {
00626                                                     if(exploration->clusters.at(i).cluster_element.size() > 0)
00627                                                     {
00628                                                         cluster_vector_position = i; 
00629                                                     }
00630                                                     break;
00631                                                 }
00632                                             }
00633                                         }
00634                                     }
00635                                     ROS_DEBUG("Cluster vector position: %d", cluster_vector_position);
00636                                     
00637                                     if(cluster_vector_position >= 0)
00638                                     {
00639                                         if(exploration->clusters.at(cluster_vector_position).unreachable_frontier_count >= number_unreachable_frontiers_for_cluster)
00640                                         {
00641                                             goal_determined = false;
00642                                             ROS_WARN("Cluster inoperateable");
00643                                         }else
00644                                         {
00645                                             ROS_WARN("Cluster operateable");
00646                                         }
00647                                     }
00648                                     
00649                                     if(goal_determined == false)
00650                                     {  
00651                                         
00652                                         ROS_INFO("No goal was determined, cluster is empty. Bid for another one");
00653 
00654                                         final_goal.clear();
00655                                         robot_str.clear();
00656                                         clusters_available_in_pool.clear();
00657 
00658                                         bool auctioning = exploration->auctioning(&final_goal, &clusters_available_in_pool, &robot_str);
00659                                         if(auctioning == true)
00660                                         {
00661                                             goal_determined = true; 
00662                                             cluster_element = final_goal.at(4);
00663                                             counter_waiting_for_clusters = 0;
00664                                             break;
00665                                         }
00666                                         else
00667                                         {
00668                                             if(exploration->clusters.size() > 0) //clusters_available_in_pool.size() > 0)
00669                                             {
00670                                                 ROS_INFO("No cluster was selected but other robots are operating ... waiting for new clusters");
00671                                                 counter_waiting_for_clusters++;
00672                                                 break;
00673                                             }else
00674                                             {
00675                                                 /*
00676                                                  * If NO goals are selected at all, iterate over the global
00677                                                  * map to find some goals. 
00678                                                  */
00679                                                 ROS_WARN("No goals are available at all");
00680                                                 cluster_element = -1;
00681                                                 break;
00682                                             }
00683                                         }
00684                                     }
00685                                     else
00686                                     {
00687                                         ROS_INFO("Still some goals in current cluster, finish this job first");
00688                                         break;
00689                                     }
00690                                 }
00691 
00692                             } 
00693 
00694 
00695                              /*
00696                              * *****************************************************
00697                              * FRONTIER COORDINATION
00698                              * *****************************************************
00699                              */
00700 
00701 
00702     //                        exploration->visualize_Clusters();
00703                             exploration->visualize_Cluster_Cells();
00704                             exploration->visualize_Frontiers();
00705     //                        exploration->visualize_visited_Frontiers();
00706 
00707                        
00708 //                            if (Simulation == false)//cluster_element_size != 0) 
00709 //                            {
00710                                 ROS_INFO("Navigating to Goal");
00711 
00712                                 if(goal_determined == true)
00713                                 {
00714                                     if(OPERATE_WITH_GOAL_BACKOFF == true)
00715                                     {
00716                                         ROS_DEBUG("Doing smartGoalBackoff");
00717                                         backoff_sucessfull = exploration->smartGoalBackoff(final_goal.at(0),final_goal.at(1), costmap2d_global, &backoffGoal);   
00718                                     }
00719                                     else
00720                                     {
00721                                         backoff_sucessfull = true;
00722                                     }
00723                                 }
00724 
00725                                 if(backoff_sucessfull == true)
00726                                 {
00727                                     if(OPERATE_WITH_GOAL_BACKOFF == true)
00728                                     {
00729                                         ROS_INFO("Doing navigation to backoff goal");
00730                                         navigate_to_goal = navigate(backoffGoal);
00731                                     }else
00732                                     {
00733                                         ROS_INFO("Doing navigation to goal");
00734                                         navigate_to_goal = navigate(final_goal);
00735                                     }
00736                                 }
00737                                 else if(backoff_sucessfull == false && goal_determined == false)
00738                                 {
00739                                     navigate_to_goal = navigate(final_goal);
00740                                     goal_determined = false;
00741                                 }
00742 
00743 
00744                                 if(navigate_to_goal == true && goal_determined == true)
00745                                 {
00746                                     exploration->calculate_travel_path(exploration->visited_frontiers.at(exploration->visited_frontiers.size()-1).x_coordinate, exploration->visited_frontiers.at(exploration->visited_frontiers.size()-1).y_coordinate);
00747 
00748                                     ROS_DEBUG("Storeing visited...");
00749                                     exploration->storeVisitedFrontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),robot_str.at(0),final_goal.at(3)); 
00750                                     ROS_DEBUG("Stored Visited frontier");
00751                                    
00752                                 }   
00753                                 else if(navigate_to_goal == false && goal_determined == true)
00754                                 {
00755                                     ROS_DEBUG("Storeing unreachable...");
00756                                     exploration->storeUnreachableFrontier(final_goal.at(0),final_goal.at(1),final_goal.at(2),robot_str.at(0),final_goal.at(3));                            
00757                                     ROS_DEBUG("Stored unreachable frontier");
00758                                 } 
00759 
00760     //                            exploration->clearVisitedFrontiers();
00761     //                            exploration->clearUnreachableFrontiers();  
00762 
00763     //                            exploration->publish_frontier_list();  
00764     //                            exploration->publish_visited_frontier_list();                           
00765 //                            }
00766                         }
00767                         else
00768                         {
00769                                 ROS_WARN("No navigation performed");                            
00770                         }               
00771                         ROS_DEBUG("                                             ");
00772                         ROS_DEBUG("                                             ");
00773                 }
00774         }         
00775            
00776         void frontiers()
00777         {
00778             ros::Rate r(5);
00779             while(ros::ok())
00780             {
00781                 
00782                 costmap_mutex.lock();
00783                 
00784                 exploration->clearSeenFrontiers(costmap2d_global);
00785                 exploration->clearVisitedFrontiers();
00786                 exploration->clearUnreachableFrontiers(); 
00787                 
00788                 exploration->publish_frontier_list();  
00789                 exploration->publish_visited_frontier_list();  
00790                 
00791                 costmap_mutex.unlock();
00792                 
00793                 r.sleep();
00794             }
00795         }
00796         
00797         
00798         void map_info()
00799         {       
00800             fs_csv.open(csv_file.c_str(), std::fstream::in | std::fstream::app | std::fstream::out);
00801             fs_csv << "#time,exploration_travel_path_global,exploration_travel_path_average,global_map_progress,local_map_progress,number_of_completed_auctions, number_of_uncompleted_auctions, frontier_selection_strategy, costmap_size, unreachable_frontiers" << std::endl;
00802             fs_csv.close();
00803             
00804             while(ros::ok() && exploration_finished != true)
00805             {
00806                 costmap_mutex.lock();
00807 
00808                 ros::Duration time = ros::Time::now() - time_start;
00809                 double exploration_time = time.toSec();  
00810 
00811                 map_progress.global_freespace = global_costmap_size();
00812                 map_progress.local_freespace = local_costmap_size();
00813                 map_progress.time = exploration_time;               
00814                 map_progress_during_exploration.push_back(map_progress);
00815 
00816 
00817                 double exploration_travel_path_global = (double)exploration->exploration_travel_path_global * 0.02;
00818                 double exploration_travel_path_average = 0;
00819                 if(counter != 0)
00820                 {
00821                     exploration_travel_path_average = (exploration->exploration_travel_path_global) / counter;
00822                 }
00823 
00824 //                ROS_INFO("global map size: %f   at time: %f", map_progress.global_freespace, map_progress.time);
00825 //                ROS_INFO("local map size : %f   at time: %f", map_progress.local_freespace, map_progress.time);
00826 //                ROS_INFO("travel path glo: %d   at time: %f", exploration_travel_path_global, map_progress.time);
00827 //                ROS_INFO("travel path ave: %d   at time: %f", exploration_travel_path_average, map_progress.time);
00828 
00829                 fs_csv.open(csv_file.c_str(), std::fstream::in | std::fstream::app | std::fstream::out);
00830 
00831                 fs_csv << map_progress.time << "," << exploration_travel_path_global << "," << exploration_travel_path_average << "," << map_progress.global_freespace << "," << map_progress.local_freespace << "," << global_iterattions <<  "," << exploration->number_of_completed_auctions << "," << exploration->number_of_uncompleted_auctions << "," << frontier_selection << "," <<  costmap_width << "," << exploration->unreachable_frontiers.size() <<  std::endl;
00832 //                fs_csv << "travel_path_global   = " << exploration_travel_path_global << std::endl;
00833 //                fs_csv << "travel_path_average  = " << exploration_travel_path_average << std::endl;             
00834 //                fs_csv << "map_progress_global  = " << map_progress.global_freespace << std::endl;
00835 //                fs_csv << "map_progress_average = " << map_progress.local_freespace << std::endl;
00836 //                fs_csv << "time                 = " << map_progress.time << std::endl;
00837 //                fs_csv << "                       " << std::endl;
00838 
00839                 fs_csv.close();
00840 
00841                 costmap_mutex.unlock();    
00842 
00843                 // call map_merger to log data
00844                 map_merger::LogMaps log;
00845                 log.request.log = 12;    
00846                 ROS_DEBUG("Calling map_merger service logOutput");
00847                 if(!mm_log_client.call(log))
00848                     ROS_WARN("Could not call map_merger service to store log.");
00849                 ROS_DEBUG("Finished service call.");
00850 
00851                 save_progress();
00852                 
00853                 ros::Duration(10.0).sleep();
00854             }
00855         }
00856         
00857         int global_costmap_size()
00858         {
00859            occupancy_grid_global = costmap2d_global->getCostmap()->getCharMap();
00860            int num_map_cells_ = costmap2d_global->getCostmap()->getSizeInCellsX() * costmap2d_global->getCostmap()->getSizeInCellsY();
00861            int free = 0;
00862            
00863            for (unsigned int i = 0; i < num_map_cells_; i++) 
00864            {
00865                 if ((int) occupancy_grid_global[i] == costmap_2d::FREE_SPACE) 
00866                 {
00867                      free++;
00868                 }
00869             }   
00870            return free;
00871         }
00872         
00873         int local_costmap_size()
00874         {   
00875             if(OPERATE_ON_GLOBAL_MAP == false)
00876             {
00877                 occupancy_grid_local = costmap2d_local->getCostmap()->getCharMap();
00878                 int num_map_cells_ = costmap2d_local->getCostmap()->getSizeInCellsX() * costmap2d_local->getCostmap()->getSizeInCellsY();
00879                 int free = 0;
00880 
00881                 for (unsigned int i = 0; i < num_map_cells_; i++) 
00882                 {
00883                      if ((int) occupancy_grid_local[i] == costmap_2d::FREE_SPACE) 
00884                      {
00885                           free++;
00886                      }
00887                  }   
00888                 return free;
00889             }else
00890             {
00891                 occupancy_grid_local = costmap2d_local_size->getCostmap()->getCharMap();
00892                 int num_map_cells_ = costmap2d_local_size->getCostmap()->getSizeInCellsX() * costmap2d_local_size->getCostmap()->getSizeInCellsY();
00893                 int free = 0;
00894 
00895                 for (unsigned int i = 0; i < num_map_cells_; i++) 
00896                 {
00897                      if ((int) occupancy_grid_local[i] == costmap_2d::FREE_SPACE) 
00898                      {
00899                           free++;
00900                      }
00901                  }   
00902                 return free;
00903             }
00904         }
00905         
00906         void initLogPath()
00907         {
00908            /*
00909             *  CREATE LOG PATH
00910             * Following code enables to write the output to a file
00911             * which is localized at the log_path
00912             */
00913             
00914             nh.param<std::string>("log_path",log_path,"");           
00915 
00916             std::stringstream robot_number;
00917             robot_number << robot_id;
00918             std::string prefix = "/robot_";
00919             std::string robo_name = prefix.append(robot_number.str());    
00920 
00921             log_path = log_path.append("/explorer");
00922             log_path = log_path.append(robo_name);
00923             log_path = log_path.append("/");
00924             ROS_INFO("Logging files to %s", log_path.c_str());
00925 
00926             boost::filesystem::path boost_log_path(log_path.c_str());
00927             if(!boost::filesystem::exists(boost_log_path))
00928                 if(!boost::filesystem::create_directories(boost_log_path))
00929                     // directory not created; check if it is there anyways
00930                     if(!boost::filesystem::is_directory(boost_log_path))
00931                         ROS_ERROR("Cannot create directory %s.", log_path.c_str());
00932                     else
00933                         ROS_INFO("Successfully created directory %s.", log_path.c_str());
00934              
00935         }
00936         
00937         void save_progress(bool final=false)
00938         {
00939             ros::Duration ros_time = ros::Time::now() - time_start;
00940             
00941             double exploration_time = ros_time.toSec();           
00942             int navigation_goals_required = counter;        
00943             double exploration_travel_path = (double)exploration->exploration_travel_path_global * 0.02;
00944             double size_global_map = map_progress_during_exploration.at(map_progress_during_exploration.size()-1).global_freespace;
00945                     
00946             double efficiency_value = (exploration_time) / (number_of_robots * navigation_goals_required);
00947 
00948             std::string tmp_log;
00949             if(!final) 
00950             {
00951                 tmp_log = log_file + std::string(".tmp");
00952                 fs.open(tmp_log.c_str(), std::fstream::in | std::fstream::trunc | std::fstream::out);
00953             }
00954             else
00955             {
00956                 tmp_log = log_file;
00957                 fs.open(tmp_log.c_str(), std::fstream::in | std::fstream::trunc | std::fstream::out);
00958             }
00959 
00960             /*
00961              *  WRITE LOG FILE
00962              * Write all the output to the log file
00963              */
00964             time_t raw_time;
00965             struct tm* timeinfo;
00966             time (&raw_time);
00967             timeinfo = localtime (&raw_time);
00968 
00969             fs << "[Exploration]" << std::endl;
00970             fs << "time_file_written                            = " << asctime(timeinfo); // << std::endl;
00971             fs << "start_time                                   = " << time_start << std::endl;
00972             fs << "end_time                                     = " << ros::Time::now() << std::endl;
00973             fs << "exploration_time                             = " << exploration_time << std::endl;
00974             fs << "required_goals                               = " << navigation_goals_required << std::endl;  
00975             fs << "unreachable_goals                            = " << exploration->unreachable_frontiers.size() << std::endl;
00976             fs << "travel_path_overall                          = " << exploration_travel_path << std::endl;
00977             fs << "number_of_completed_auctions                 = " << exploration->number_of_completed_auctions << std::endl;
00978             fs << "number_of_uncompleted_auctions               = " << exploration->number_of_uncompleted_auctions << std::endl;
00979             fs << "frontier_selection_strategy                  = " << frontier_selection << std::endl;
00980             fs << "costmap_size                                 = " << costmap_width << std::endl;
00981             fs << "global costmap iterations                    = " << global_costmap_iteration << std::endl;
00982             
00983             double param_double;
00984         int param_int;
00985             std::string param;
00986         /*param = robot_prefix + "/explorer/local_costmap/height";
00987             ros::param::get(param,param_double);*/
00988         nh.param<int>("local_costmap/height",param_int,-1);
00989         fs << "explorer_local_costmap_height            = " << param_int << std::endl;
00990 
00991         /*param = robot_prefix + "/explorer/local_costmap/width";
00992             ros::param::get(param,param_double);*/
00993         nh.param<int>("local_costmap/width",param_int,-1);
00994         fs << "explorer_local_costmap_width             = " << param_int << std::endl;
00995 
00996             param = robot_prefix + "/move_base/local_costmap/height";
00997             ros::param::get(param,param_double);
00998             fs << "move_base_local_costmap_height               = " << param_double << std::endl;
00999 
01000             param = robot_prefix + "/move_base/local_costmap/width";
01001             ros::param::get(param,param_double);
01002             fs << "move_base_local_costmap_width                = " << param_double << std::endl;
01003 
01004             param = robot_prefix + "/move_base/global_costmap/obstacle_layer/raytrace_range";
01005             ros::param::get(param,param_double);
01006             fs << "move_base_raytrace_range             = " << param_double << std::endl;
01007 
01008             param = robot_prefix + "/move_base/global_costmap/obstacle_layer/obstacle_range";
01009             ros::param::get(param,param_double);
01010             fs << "move_base_obstacle_range             = " << param_double << std::endl;
01011 
01012 //          param = robot_prefix + "/navigation/global_costmap/obstacle_layer/raytrace_range";
01013         nh.getParam("/global_costmap/obstacle_layer/raytrace_range",param_double);
01014         ros::param::get(param,param_double);
01015             fs << "explorer_raytrace_range              = " << param_double << std::endl;
01016 
01017         //param = robot_prefix + "/navigation/global_costmap/obstacle_layer/obstacle_range";
01018         //    ros::param::get(param,param_double);
01019         nh.getParam("/global_costmap/obstacle_layer/obstacle_range",param_double);
01020             fs << "explorer_obstacle_range              = " << param_double << std::endl;
01021 
01022             if(final)
01023                 fs << "complete                                 = " << "1" << std::endl;
01024             else
01025                 fs << "complete                                 = " << "0" << std::endl;
01026             
01027             fs.close();
01028 //            ROS_INFO("Wrote file %s\n", log_file.c_str());                    
01029             
01030             
01031             /*
01032              * Inform map_merger to save maps
01033              */
01034             
01035            if(final)
01036             {
01037                     map_merger::LogMaps log;
01038                     log.request.log = 3;    
01039                     if(!mm_log_client.call(log))
01040                         ROS_WARN("Could not call map_merger service to store log.");
01041             }
01042     
01043         }
01044         
01045         void exploration_has_finished()
01046         {
01047             ros::Duration ros_time = ros::Time::now() - time_start;
01048             
01049             double exploration_time = ros_time.toSec();           
01050             int navigation_goals_required = counter;        
01051             double exploration_travel_path = (double)exploration->exploration_travel_path_global * 0.02;
01052             double size_global_map = map_progress_during_exploration.at(map_progress_during_exploration.size()-1).global_freespace;
01053             ROS_INFO("overall freespace in the global map: %f", size_global_map);
01054             
01055             for(int i = 0; i< map_progress_during_exploration.size(); i++)
01056             {
01057                 ROS_INFO("map progress: %f",(map_progress_during_exploration.at(i).global_freespace/size_global_map)*100);
01058             }
01059            
01060             ROS_DEBUG("******************************************");
01061             ROS_DEBUG("******************************************");
01062             ROS_DEBUG("TIME: %f sec  GOALS: %d  PATH: %f  COMPLETED AUCTIONS: %d  UNCOMPLETED AUCTIONS: %d", exploration_time, navigation_goals_required, exploration_travel_path, exploration->number_of_completed_auctions, exploration->number_of_uncompleted_auctions);
01063             ROS_DEBUG("******************************************");
01064             ROS_DEBUG("******************************************");
01065             
01066                   
01067             double efficiency_value = (exploration_time) / (number_of_robots * navigation_goals_required);
01068                     
01069             /*
01070              *  WRITE LOG FILE
01071              * Write all the output to the log file
01072              */
01073            save_progress(true);
01074             ROS_INFO("Wrote file %s\n", log_file.c_str());                    
01075             
01076             
01077             /*
01078              * Inform map_merger to save maps
01079              */
01080             
01081             map_merger::LogMaps log;
01082             log.request.log = 3;    
01083             if(!mm_log_client.call(log))
01084                 ROS_WARN("Could not call map_merger service to store log.");
01085 
01086                 
01087             // Indicte end of simulation for this robot
01088             this->indicateSimulationEnd();
01089    
01090 //#ifdef PROFILE
01091 //HeapProfilerStop();
01092 //ProfilerStop();
01093 //#endif
01094         }
01095         
01096         void indicateSimulationEnd()
01097         {
01099             std::stringstream robot_number;
01100             robot_number << robot_id;
01101             
01102             std::string prefix = "/robot_";
01103             std::string status_directory = "/simulation_status";
01104             std::string robo_name = prefix.append(robot_number.str());    
01105             std::string file_suffix(".finished");
01106 
01107             std::string ros_package_path = ros::package::getPath("multi_robot_analyzer");
01108             std::string status_path = ros_package_path + status_directory;
01109             std::string status_file = status_path + robo_name + file_suffix;
01110 
01112             boost::filesystem::path boost_status_path(status_path.c_str());
01113             if(!boost::filesystem::exists(boost_status_path))
01114                 if(!boost::filesystem::create_directories(boost_status_path))
01115                     ROS_ERROR("Cannot create directory %s.", status_path.c_str());
01116             std::ofstream outfile(status_file.c_str());
01117             outfile.close();
01118             ROS_INFO("Creating file %s to indicate end of exploration.", status_file.c_str());
01119 
01120 
01121         }
01122         
01123         
01124         bool iterate_global_costmap(std::vector<double> *global_goal, std::vector<std::string> *robot_str)
01125         {
01126             global_costmap_iteration++;
01127             int counter = 0;
01128             bool exploration_flag;
01129             
01130             costmap_mutex.lock();
01131 
01132             exploration->transformToOwnCoordinates_frontiers();
01133             exploration->transformToOwnCoordinates_visited_frontiers();
01134 
01135 //                          ros::Duration(1.0).sleep();
01136             exploration->initialize_planner("exploration planner", costmap2d_global, costmap2d_global);
01137             exploration->findFrontiers();
01138 //            exploration->printFrontiers();
01139 
01140             exploration->clearVisitedFrontiers();                       
01141             exploration->clearUnreachableFrontiers();
01142             exploration->clearSeenFrontiers(costmap2d_global);       
01143 
01144            costmap_mutex.unlock();
01145                             
01146            exploration->visualize_Frontiers();
01147            
01148                 if(frontier_selection < 5 && frontier_selection != 1)
01149                 {
01150                    exploration->sort(2);
01151                    
01152                     while(true)
01153                     {   
01154                         exploration_flag = exploration->determine_goal(2, global_goal, counter, -1, robot_str);
01155                         
01156                        
01157                         if(exploration_flag == false)
01158                         {
01159                            break;
01160                         }
01161                         else
01162                         {    
01163                             bool negotiation = true; 
01164                             negotiation = exploration->negotiate_Frontier(global_goal->at(0),global_goal->at(1),global_goal->at(2),global_goal->at(3), -1);
01165                             
01166                             if(negotiation == true)
01167                             {     
01168                                 return true;
01169                                 break;
01170                             }           
01171                             counter++;
01172                         }
01173                     }    
01174                 }else if(frontier_selection == 1 || frontier_selection == 6 || frontier_selection == 5)
01175                 {
01176                     costmap_mutex.lock();
01177                     exploration->clearVisitedAndSeenFrontiersFromClusters();
01178     
01179                     exploration->clusterFrontiers(); 
01180                     
01181                     exploration->sort(4);      
01182                     exploration->sort(5);
01183 
01184                     costmap_mutex.unlock(); 
01185                     
01186 //                    exploration->visualizeClustersConsole();
01187 
01188                     cluster_element = -1;
01189                     
01190                     while(true)
01191                     {                             
01192                         std::vector<double> goal_vector;
01193                         std::vector<std::string> robot_str_name;
01194                         std::vector<int> clusters_used_by_others; 
01195                         
01196                         goal_determined = exploration->determine_goal(5, &goal_vector, 0, cluster_element, robot_str);
01197 
01198                         if(goal_determined == false)
01199                         {  
01200                             ROS_INFO("No goal was determined, cluster is empty. Bid for another one");
01201 
01202                             goal_vector.clear();
01203                             bool auctioning = exploration->auctioning(&goal_vector, &clusters_used_by_others, &robot_str_name);
01204                             if(auctioning == true)
01205                             {                               
01206                                 goal_determined = true; 
01207                                 cluster_element = goal_vector.at(4);
01208                                 
01209                                 global_goal->push_back(goal_vector.at(0));
01210                                 global_goal->push_back(goal_vector.at(1));
01211                                 global_goal->push_back(goal_vector.at(2));
01212                                 global_goal->push_back(goal_vector.at(3));
01213                                 global_goal->push_back(goal_vector.at(4));
01214                                 
01215                                 robot_str->push_back(robot_str_name.at(0));
01216                                 return true; 
01217                             }
01218                             else
01219                             {
01220                                 /*
01221                                  * If NO goals are selected at all, iterate over the global
01222                                  * map to find some goals. 
01223                                  */
01224                                 ROS_WARN("No goals are available at all");
01225                                 cluster_element = -1;
01226                                 break;
01227                             }
01228                         }
01229                         else
01230                         {
01231                             ROS_INFO("Still some goals in current cluster, finish this job first");
01232                             break;
01233                         }
01234                     }
01235 //                    while(true)
01236 //                    {                            
01237 //                        std::vector<double> goal_vector;
01238 //                        goal_determined = exploration->determine_goal(4, &goal_vector, counter, cluster_element);
01239 //
01240 //                        if(goal_determined == false)
01241 //                        {
01242 //                            ROS_INFO("Another cluster is not available, no cluster determined");
01243 //                            return false;
01244 //                        }
01245 //                        else
01246 //                        {
01247 //                             /*
01248 //                             * If negotiation is not needed, simply uncomment
01249 //                             * and set the negotiation to TRUE.
01250 //                             */
01251 //                            bool negotiation = exploration->negotiate_Frontier(goal_vector.at(0),goal_vector.at(1),goal_vector.at(2),goal_vector.at(3),goal_vector.at(4));
01252 //                            if(negotiation == true)
01253 //                            {
01254 //                                ROS_INFO("Negotiation was successful");
01255 //                                cluster_element = goal_vector.at(4);
01256 //
01257 //                                global_goal->push_back(goal_vector.at(0));
01258 //                                global_goal->push_back(goal_vector.at(1));
01259 //                                global_goal->push_back(goal_vector.at(2));
01260 //                                global_goal->push_back(goal_vector.at(3));
01261 //                                global_goal->push_back(goal_vector.at(4));
01262 //                                                              
01263 //                                return true;
01264 //                            }
01265 //                            else
01266 //                            {              
01267 //                                cluster_element = goal_vector.at(4);
01268 //                                ROS_ERROR("Negotiation was not successful, try next cluster");
01269 //                            }
01270 //                            counter++;
01271 //                        }
01272 //                    }  
01273                     exploration->visualize_Cluster_Cells();
01274             }
01275               
01276 //            exploration->visualize_Clusters();
01277 //            exploration->visualize_visited_Frontiers();
01278 
01279             global_iterattions++;
01280             return false;              
01281         }
01282 
01283         
01284         bool navigate(std::vector<double> goal) {
01285                 
01286                 /*
01287                  * If received goal is not empty (x=0 y=0), drive the robot to this point
01288                  * and mark that goal as seen in the last_goal_position vector!!!
01289                  * Otherwise turn the robot by 90° to the right and search again for a better
01290                  * frontier.
01291                  */
01292                 bool completed_navigation = false;
01293                 if (goal_determined == true)
01294                 {
01295                         visualize_goal_point(goal.at(0), goal.at(1));
01296                         
01297                         counter++;
01298                         ROS_INFO("GOAL %d:  x: %f      y: %f", counter, goal.at(0), goal.at(1));
01299                         completed_navigation = move_robot(counter, goal.at(0), goal.at(1));
01300                         rotation_counter = 0;
01301                 }
01302                 else
01303                 {
01304                         rotation_counter++;
01305 //                        ROS_INFO("In navigation .... cluster_available: %lu     counter: %d", clusters_available_in_pool.size(), counter_waiting_for_clusters);
01306                          ROS_INFO("In navigation .... cluster_available: %lu     counter: %d", exploration->clusters.size(), counter_waiting_for_clusters);                     
01307 //                        if(clusters_available_in_pool.size() <= 0 || counter_waiting_for_clusters > 10) //(rotation_counter >= 2)
01308                         if(exploration->clusters.size() == 0 || counter_waiting_for_clusters > 10) //(rotation_counter >= 2)
01309                         {
01310                             ROS_INFO("Iterating over GLOBAL COSTMAP to find a goal!!!!");
01311                             std::vector<double> global_goal;
01312                             std::vector<std::string> robot_str;
01313                             bool global_costmap_goal = iterate_global_costmap(&global_goal, &robot_str);
01314                                  
01315                             if(global_costmap_goal == false)
01316                             {
01317 //                                global_iterations_counter++;
01318 //                                if(global_iterations_counter >= 5)
01319 //                                {
01320                                     counter++;
01321                                     ROS_INFO("GOAL %d: BACK TO HOME   x: %f    y: %f", counter, home_point_x, home_point_y);
01322                                     
01323                                     /*
01324                                      * If the robot should drive to the home position
01325                                      * exploration_has_finished() has to be uncommented. 
01326                                      * It creates a file which is the immediate trigger to 
01327                                      * shut down the ros_node. Therefore the navigation process
01328                                      * is canceled . 
01329                                      */
01330                                     exploration_has_finished();
01331                                     visualize_goal_point(home_point_x, home_point_y);
01332                                     completed_navigation = false;
01333                                     for(int i = 0; i< 5; i++)
01334                                     {
01335                                         if(completed_navigation == false)
01336                                         {
01337                                            completed_navigation = move_robot(counter, home_point_x, home_point_y); 
01338                                         }
01339                                         else
01340                                         {
01341                                             break;
01342                                         }                                       
01343                                     }
01344                                     
01345                                     exploration_finished = true;
01346 //                                }else
01347 //                                {
01348 //                                    ROS_INFO("No Goal determined on GLOBAL COSTMAP for the %d time. Still wait ...", global_iterations_counter);
01349 //                                }
01350                             }else
01351                             {
01352                                     counter++;
01353                                     ROS_INFO("GOAL %d:  x: %f      y: %f", counter, global_goal.at(0), global_goal.at(1));
01354                                     std::vector<double> backoffGoal;
01355                                     bool backoff_sucessfull = exploration->smartGoalBackoff(global_goal.at(0),global_goal.at(1), costmap2d_global, &backoffGoal);
01356                                     
01357                                     if(backoff_sucessfull == true)
01358                                     {
01359                                         ROS_DEBUG("doing navigation to back-off goal");
01360                                         visualize_goal_point(backoffGoal.at(0), backoffGoal.at(1));
01361                                         completed_navigation = move_robot(counter, backoffGoal.at(0), backoffGoal.at(1));
01362                                         rotation_counter = 0;
01363                                         if(completed_navigation == true)
01364                                         {
01365                                             exploration->calculate_travel_path(exploration->visited_frontiers.at(exploration->visited_frontiers.size()-1).x_coordinate, exploration->visited_frontiers.at(exploration->visited_frontiers.size()-1).y_coordinate);
01366                                 
01367                                             ROS_INFO("Storing visited...");
01368                                             exploration->storeVisitedFrontier(global_goal.at(0),global_goal.at(1),global_goal.at(2),robot_str.at(0), global_goal.at(3)); 
01369                                             ROS_INFO("Stored Visited frontier");
01370                                         }   
01371                                         else
01372                                         {
01373                                             ROS_INFO("Storing unreachable...");
01374                                             exploration->storeUnreachableFrontier(global_goal.at(0),global_goal.at(1),global_goal.at(2),robot_str.at(0), global_goal.at(3));
01375                                             ROS_INFO("Stored unreachable frontier");
01376                                         }                          
01377                                     }
01378                                     else if(backoff_sucessfull == false)
01379                                     {
01380                                         ROS_WARN("Navigation to global costmap back-off goal not possible"); 
01381                                         ROS_INFO("Storing as unreachable...");
01382                                         exploration->storeUnreachableFrontier(global_goal.at(0),global_goal.at(1),global_goal.at(2), robot_str.at(0), global_goal.at(3));
01383                                         ROS_INFO("Stored unreachable frontier");
01384                                     }
01385                             }
01386                         }else
01387                         {
01388                                 counter++;
01389                                 ROS_INFO("GOAL %d:  rotation", counter);
01390                                 completed_navigation = turn_robot(counter);
01391                         }
01392                 }      
01393                 return(completed_navigation);
01394         }
01395 
01396         void visualize_goal_point(double x, double y) {
01397 
01398                 goalPoint.header.seq = goal_point_message++;
01399                 goalPoint.header.stamp = ros::Time::now();
01400                 goalPoint.header.frame_id = move_base_frame; //"map"
01401                 goalPoint.point.x = x;// - robotPose.getOrigin().getX();
01402                 goalPoint.point.y = y;// - robotPose.getOrigin().getY();
01403 
01404                 ros::NodeHandle nh_Point("goalPoint");
01405                 pub_Point = nh_Point.advertise < geometry_msgs::PointStamped
01406                                 > ("goalPoint", 100, true);
01407                 pub_Point.publish < geometry_msgs::PointStamped > (goalPoint);
01408         }
01409 
01410         void visualize_home_point() {
01411 
01412                 homePoint.header.seq = home_point_message++;
01413                 homePoint.header.stamp = ros::Time::now();
01414                 homePoint.header.frame_id = move_base_frame; //"map";
01415                 home_point_x = robotPose.getOrigin().getX();
01416                 home_point_y = robotPose.getOrigin().getY();
01417                 homePoint.point.x = home_point_x;
01418                 homePoint.point.y = home_point_y;
01419 
01420                 ros::NodeHandle nh("homePoint");
01421                 pub_home_Point = nh.advertise < geometry_msgs::PointStamped
01422                                 > ("homePoint", 100, true);
01423                 pub_home_Point.publish < geometry_msgs::PointStamped > (homePoint);
01424 
01425         }
01426 
01427         bool move_robot(int seq, double position_x, double position_y) {
01428             
01429             exploration->next_auction_position_x = position_x;
01430             exploration->next_auction_position_y = position_y;
01431             
01432                 /*
01433                  * Move the robot with the help of an action client. Goal positions are
01434                  * transmitted to the robot and feedback is given about the actual
01435                  * driving state of the robot.
01436                  */
01437                 if (!costmap2d_local->getRobotPose(robotPose)) {
01438                         ROS_ERROR("Failed to get RobotPose");
01439                 }
01440                 
01441                 actionlib::SimpleActionClient < move_base_msgs::MoveBaseAction
01442                                 > ac("move_base", true);
01443 
01444                 while (!ac.waitForServer(ros::Duration(10.0)))
01445                         ;
01446            
01447                 move_base_msgs::MoveBaseGoal goal_msgs;
01448 
01449                 goal_msgs.target_pose.header.seq = seq; // increase the sequence number
01450 //              goal_msgs.target_pose.header.stamp = ros::Time::now();
01451                 goal_msgs.target_pose.header.frame_id = move_base_frame; //"map";
01452                 goal_msgs.target_pose.pose.position.x = position_x;// - robotPose.getOrigin().getX(); //goals[0].pose.position.x;
01453                 goal_msgs.target_pose.pose.position.y = position_y;// - robotPose.getOrigin().getY();
01454                 goal_msgs.target_pose.pose.position.z = 0;
01455                 goal_msgs.target_pose.pose.orientation.x = 0; //sin(angle/2); // goals[0].pose.orientation.x;
01456                 goal_msgs.target_pose.pose.orientation.y = 0;
01457                 goal_msgs.target_pose.pose.orientation.z = 0;
01458                 goal_msgs.target_pose.pose.orientation.w = 1;
01459 
01460                 ac.sendGoal(goal_msgs);
01461                
01462         //ac.waitForResult(ros::Duration(20)); EDIT Peter: Test if it also works with smaller value!
01463         ac.waitForResult(ros::Duration(waitForResult)); //here Parameter!
01464         while (ac.getState() == actionlib::SimpleClientGoalState::PENDING)
01465         {
01466             ros::Duration(0.5).sleep();
01467         }
01468                 ROS_INFO("Not longer PENDING");
01469 
01470         while (ac.getState() == actionlib::SimpleClientGoalState::ACTIVE)
01471         {
01472             ros::Duration(0.5).sleep();
01473         }
01474                 ROS_INFO("Not longer ACTIVE");
01475 
01476                 while (ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) {
01477                         if (ac.getState() == actionlib::SimpleClientGoalState::ABORTED) {
01478                                 ROS_INFO("ABORTED");
01479                                 
01480                                 exploration->next_auction_position_x = robotPose.getOrigin().getX();
01481                                 exploration->next_auction_position_y = robotPose.getOrigin().getY();
01482                                 return false;
01483                         }
01484                         /*
01485                          double my_time = ros::Time::now().toSec();
01486                          //ROS_ERROR("my_time: %f     timeout: %f",my_time,timeout);
01487                          if(my_time >= timeout)
01488                          {
01489                          ROS_ERROR("Timeout exceeded");
01490                          break;
01491                          }
01492                          if(ac.getState() == actionlib::SimpleClientGoalState::PREEMPTED){ROS_ERROR("PREEMPTED");}
01493                          if(ac.getState() == actionlib::SimpleClientGoalState::ABORTED){ROS_ERROR("ABORTED");}
01494                          if(ac.getState() == actionlib::SimpleClientGoalState::REJECTED){ROS_ERROR("REJECTED");}
01495                          if(ac.getState() == actionlib::SimpleClientGoalState::RECALLED){ROS_ERROR("RECALLED");}
01496                          */
01497                 }
01498                 ROS_INFO("TARGET REACHED");
01499                 
01500                 exploration->next_auction_position_x = robotPose.getOrigin().getX();
01501                 exploration->next_auction_position_y = robotPose.getOrigin().getY();
01502                 return true;
01503         }
01504 
01505         bool turn_robot(int seq) {
01506                 
01507                 double angle = 45;
01508 
01509                 if (!costmap2d_local->getRobotPose(robotPose)) {
01510                         ROS_ERROR("Failed to get RobotPose");
01511                 }
01512                 
01513                 actionlib::SimpleActionClient < move_base_msgs::MoveBaseAction
01514                                 > ac("move_base", true);
01515                 while (!ac.waitForServer(ros::Duration(10.0)))
01516                         ;
01517 
01518                 move_base_msgs::MoveBaseGoal goal_msgs;
01519 
01520                 goal_msgs.target_pose.header.seq = seq; // increase the sequence number
01521                 goal_msgs.target_pose.header.stamp = ros::Time::now();
01522 
01523                 goal_msgs.target_pose.header.frame_id = move_base_frame;//"map";
01524                 goal_msgs.target_pose.pose.position.x = robotPose.getOrigin().getX();
01525                 goal_msgs.target_pose.pose.position.y = robotPose.getOrigin().getY();
01526                 goal_msgs.target_pose.pose.position.z = 0;
01527                 goal_msgs.target_pose.pose.orientation.x = 0; //sin(angle/2); // goals[0].pose.orientation.x;
01528                 goal_msgs.target_pose.pose.orientation.y = 0;
01529                 goal_msgs.target_pose.pose.orientation.z = sin(angle / 2);
01530                 goal_msgs.target_pose.pose.orientation.w = cos(angle / 2);
01531 
01532                 ac.sendGoal(goal_msgs);
01533                 ac.waitForResult(ros::Duration(waitForResult));
01534 
01535                 while (ac.getState() == actionlib::SimpleClientGoalState::PENDING)
01536                 {
01537                     ros::Duration(0.5).sleep();
01538                 }
01539                 ROS_INFO("Not longer PENDING");
01540 
01541                 while (ac.getState() == actionlib::SimpleClientGoalState::ACTIVE)
01542                 {
01543                     ros::Duration(0.5).sleep();
01544                 }
01545                 ROS_INFO("Not longer ACTIVE");
01546 
01547                 while (ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) {
01548                         if (ac.getState() == actionlib::SimpleClientGoalState::ABORTED) {
01549                                 ROS_INFO("ABORTED");                                
01550                                 return false;
01551                         }
01552                 }
01553                 ROS_INFO("ROTATION ACCOMBLISHED");
01554                 return true;
01555         }
01556 
01557         void feedbackCallback(
01558                         const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg) {
01559                 feedback_value = msg.get()->status.status;
01560                 feedback_succeed_value =
01561                                 msg.get()->feedback.base_position.pose.orientation.w;
01562 
01563         }
01564 
01565         bool target_reached(void) {
01566 
01567                 ros::NodeHandle nh_sub_move_base;
01568                 sub_move_base = nh_sub_move_base.subscribe("feedback", 1000,
01569                                 &Explorer::feedbackCallback, this);
01570 
01571                 while (feedback_value != feedback_succeed_value)
01572                         ;
01573 
01574                 return true;
01575         }
01576 
01577 public:
01578 
01579         struct map_progress_t
01580         {
01581             double local_freespace;
01582             double global_freespace;
01583             double time;
01584         } map_progress;
01585         
01586         ros::Subscriber sub_move_base, sub_obstacle;    
01587         
01588         // create a costmap
01589         costmap_2d::Costmap2DROS* costmap2d_local;
01590         costmap_2d::Costmap2DROS* costmap2d_local_size;
01591         costmap_2d::Costmap2DROS* costmap2d_global;
01592         costmap_2d::Costmap2D costmap;
01593 
01594         std::vector<map_progress_t> map_progress_during_exploration;
01595         
01596         std::vector<int> clusters_available_in_pool;
01597         
01598         int home_position_x, home_position_y;
01599         int robot_id, number_of_robots;
01600         int frontier_selection, costmap_width, global_costmap_iteration, number_unreachable_frontiers_for_cluster;
01601         int counter_waiting_for_clusters;
01602         
01603         double robot_home_position_x, robot_home_position_y;
01604         bool Simulation, goal_determined;
01605         bool robot_prefix_empty;
01606         int accessing_cluster, cluster_element_size, cluster_element;
01607         int global_iterattions;
01608         bool cluster_flag, cluster_initialize_flag;
01609         int global_iterations_counter; 
01610         int waitForResult;
01611         std::string move_base_frame;
01612         std::string robot_prefix;               
01613         std::string robot_name;
01614         const unsigned char* occupancy_grid_global;
01615         const unsigned char* occupancy_grid_local;
01616         
01617         std::string csv_file, log_file;
01618         std::string log_path;
01619         std::fstream fs_csv, fs;
01620                 
01621 private:
01622         
01623         ros::Publisher pub_move_base;
01624         ros::Publisher pub_Point;
01625         ros::Publisher pub_home_Point;
01626         ros::Publisher pub_frontiers;
01627 
01628         ros::ServiceClient mm_log_client;
01629         
01630         ros::NodeHandle nh;
01631         ros::Time time_start;
01632 
01633         //Create a move_base_msgs to define a goal to steer the robot to
01634         move_base_msgs::MoveBaseActionGoal action_goal_msg;
01635         move_base_msgs::MoveBaseActionFeedback feedback_msgs;
01636 
01637         //move_base::MoveBase simple_move_base;
01638         geometry_msgs::PointStamped goalPoint;
01639         geometry_msgs::PointStamped homePoint;
01640 
01641         std::vector<geometry_msgs::PoseStamped> goals;
01642         tf::Stamped<tf::Pose> robotPose;
01643 
01644         explorationPlanner::ExplorationPlanner *exploration;       
01645         
01646         double x_val, y_val, home_point_x, home_point_y;
01647         int seq, feedback_value, feedback_succeed_value, rotation_counter,
01648                         home_point_message, goal_point_message;
01649         int counter;
01650         bool pioneer, exploration_finished;
01651 };
01652 
01653 int main(int argc, char **argv) {
01654         /*
01655          * ROS::init() function needs argc and argv to perform
01656          * any argument and remapping that is provided by the
01657          * command line. The third argument is the name of the node
01658          */
01659         ros::init(argc, argv, "simple_navigation");
01660         /*
01661          * Create instance of Simple Navigation
01662          */
01663         tf::TransformListener tf(ros::Duration(10));
01664         Explorer simple(tf);
01665 
01666         /*
01667          * The ros::spin command is needed to wait for any call-back. This could for
01668          * example be a subscription on another topic. Do this to be able to receive a
01669          * message.
01670          */
01671 
01672         boost::thread thr_explore(boost::bind(&Explorer::explore, &simple));    
01673 //        boost::thread thr_frontiers(boost::bind(&SimpleNavigation::frontiers, &simple));
01674         /*
01675          * The following thread is only necessary to log simulation results.
01676          * Otherwise it produces unused output.
01677          */
01678         boost::thread thr_map(boost::bind(&Explorer::map_info, &simple));
01679         
01680         /*
01681          * FIXME
01682          * Which rate is required in order not to oversee
01683          * any callback data (frontiers, negotiation ...)
01684          */
01685 //        ros::Rate r(5); // FIXME 30
01686         while (ros::ok()) {
01687            
01688             costmap_mutex.lock(); 
01689             ros::spinOnce();
01690             costmap_mutex.unlock();
01691            
01692 //            r.sleep();
01693             ros::Duration(0.1).sleep();
01694         }
01695 
01696         thr_explore.interrupt();
01697         thr_map.interrupt();
01698 //      thr_frontiers.interrupt();
01699         
01700         thr_explore.join();
01701         thr_map.join();
01702 //        thr_frontiers.interrupt();
01703         
01704         return 0;
01705 }


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