ExplorationPlanner.h
Go to the documentation of this file.
00001 #ifndef PLANNER_H___
00002 #define PLANNER_H___
00003 
00004 #include <ExplorationPlanner.h>
00005 #include <navfn/navfn_ros.h>
00006 #include "ros/ros.h"
00007 #include "hungarian.h"
00008 #include <ros/console.h>
00009 #include <costmap_2d/costmap_2d_ros.h>
00010 #include <costmap_2d/costmap_2d.h>
00011 #include <visualization_msgs/Marker.h>
00012 #include <visualization_msgs/MarkerArray.h>
00013 #include <adhoc_communication/ExpFrontier.h> //<simple_navigation/Frontier.h>
00014 #include <adhoc_communication/ExpCluster.h>
00015 #include <adhoc_communication/ExpAuction.h>
00016 #include <adhoc_communication/MmPoint.h>
00017 #include <adhoc_communication/MmListOfPoints.h>
00018 #include <map_merger/TransformPoint.h>
00019 //#include <dynamic_reconfigure/server.h>
00020 
00021 namespace explorationPlanner
00022 {
00023         class ExplorationPlanner
00024         {
00025         public:
00026  
00027             struct frontier_t
00028             {
00029                 int id;
00030                 int detected_by_robot;  
00031                 std::string detected_by_robot_str;
00032                 double robot_home_x;
00033                 double robot_home_y;
00034                 double x_coordinate;
00035                 double y_coordinate;
00036                 int distance_to_robot;
00037                 int dist_to_robot;
00038             } frontier, unreachable_frontier; 
00039 
00040             struct responded_t
00041             {
00042                 int robot_number; 
00043                 std::string robot_str;
00044                 int auction_number; 
00045             };
00046             
00047             struct cluster_t
00048             {
00049                 std::vector<frontier_t> cluster_element;
00050                 int id;
00051                 int unreachable_frontier_count;
00052             } cluster;
00053             
00054             struct transform_point_t
00055             {
00056                 int id; 
00057                 std::string robot_str;
00058             };
00059             
00060             struct requested_cluster_t
00061             {
00062                 std::vector<transform_point_t> requested_ids;
00063                 int own_cluster_id; 
00064             };
00065             
00066             struct auction_pair_t
00067             {
00068                 int bid_value;
00069                 int cluster_id;
00070             };
00071             
00072             struct auction_element_t
00073             {
00074                 int robot_id; 
00075                 std::string detected_by_robot_str;
00076                 std::vector<auction_pair_t> auction_element;
00077             };
00078             
00079             
00080             
00081             struct compare_pair_t
00082             {
00083                 int cluster_id;
00084                 int identical_ids;
00085             };
00086             
00087             boost::mutex store_frontier_mutex, store_visited_mutex, store_negotiation_mutex;
00088             boost::mutex publish_subscribe_mutex, callback_mutex, negotiation_mutex, negotiation_callback_mutex; 
00089             boost::mutex position_mutex, auction_mutex;
00090             boost::mutex cluster_mutex;
00091             boost::thread thr_auction_status, thr_auction_pid;
00092             
00093             
00094             ros::ServiceClient client;
00095 
00096             ros::Publisher pub_frontiers, pub_visited_frontiers, pub_negotion, pub_negotion_first, pub_Point;
00097             ros::Publisher pub_frontiers_points, pub_visited_frontiers_points;
00098             ros::Publisher pub_auctioning, pub_auctioning_status; 
00099             ros::Subscriber sub_frontiers, sub_visited_frontiers, sub_negotioation, sub_negotioation_first;
00100             ros::Subscriber sub_control;
00101             ros::Subscriber sub_auctioning, sub_auctioning_status;
00102             ros::Subscriber sub_position, sub_robot;        
00103             
00104             ros::Publisher pub_auctioning_first; 
00105             ros::Subscriber sub_auctioning_first; 
00106             
00107             ros::Publisher pub_clusters, pub_cluster_grid_0, pub_cluster_grid_1, pub_cluster_grid_2, pub_cluster_grid_3, pub_cluster_grid_4, pub_cluster_grid_5, pub_cluster_grid_6, pub_cluster_grid_7, pub_cluster_grid_8, pub_cluster_grid_9,
00108             pub_cluster_grid_10,pub_cluster_grid_11,pub_cluster_grid_12,pub_cluster_grid_13,pub_cluster_grid_14,pub_cluster_grid_15,pub_cluster_grid_16,pub_cluster_grid_17,pub_cluster_grid_18,pub_cluster_grid_19;
00109             
00110             ros::NodeHandle nh_frontier, nh_visited_frontier;
00111             ros::NodeHandle nh_Point, nh_visited_Point, nh_frontiers_points;
00112             ros::NodeHandle nh_transform, nh_negotiation, nh_negotiation_first;
00113             ros::NodeHandle nh_auction, nh_auction_status; 
00114             ros::NodeHandle nh_cluster, nh_cluster_grid;
00115             ros::NodeHandle nh_position, nh_robot;
00116             ros::NodeHandle nh_control; 
00117             ros::NodeHandle *nh_service;
00118             ros::NodeHandle nh; 
00119             ros::NodeHandle nh_auction_first;
00120             
00121             ros::MultiThreadedSpinner spinner;
00122             
00123             std::vector<int> allFrontiers;
00124            
00125             std::vector<auction_element_t> auction;
00126             std::vector<frontier_t> frontiers;
00127             std::vector<frontier_t> visited_frontiers;
00128             std::vector<frontier_t> unreachable_frontiers;
00129             
00130             std::vector<frontier_t> seen_frontier_list;
00131             std::vector<frontier_t> negotiation_list, my_negotiation_list;
00132             
00133             std::vector <responded_t> robots_already_responded;
00134             
00135             std::vector <double> goal_buffer_x;
00136             std::vector <double> goal_buffer_y;
00137 
00138             std::vector<double> last_goal_position_x;
00139             std::vector<double> last_goal_position_y;
00140 
00141             std::vector<cluster_t> clusters;
00142             std::vector<adhoc_communication::ExpCluster> unrecognized_occupied_clusters;
00143             
00144             std::vector<transform_point_t> transformedPointsFromOtherRobot_frontiers, transformedPointsFromOtherRobot_visited_frontiers;
00145             
00146             std::vector<int> already_used_ids, id_previously_detected;
00147             
00148             adhoc_communication::MmListOfPoints other_robots_positions; 
00149             
00150             tf::TransformListener listener;
00151             tf::StampedTransform transform;
00152             tf::Stamped < tf::Pose > robotPose;
00153 
00154             map_merger::TransformPoint service_message;
00155 
00156             navfn::NavfnROS nav;
00157 
00158             ros::ServiceClient ssendFrontier, ssendAuction;
00159             
00160             std::string trajectory_strategy;
00161             bool first_run, first_negotiation_run;
00162             bool start_thr_auction;
00163 
00164             int number_of_auction_runs;
00165             int cluster_id, cluster_cells_seq_number;
00166             int number_of_completed_auctions, number_of_uncompleted_auctions;
00167             bool initialized_planner;
00168             bool auction_is_running, auction_start, auction_finished;
00169             bool robot_prefix_empty_param;
00170             int number_of_auction_bids_received, number_of_robots;
00171             double other_robots_position_x, other_robots_position_y;
00172             int auction_id_number; 
00173             int auction_pid;
00174             int frontier_seq_number;
00175             double exploration_travel_path_global;
00176             double x_value,y_value,w_value,x_distance,y_distance,robot_pose_x,robot_pose_y;
00177             int goal_buffer_counter,goal_buffer_length,random_number, random_value;
00178             int inflated,free,lethal,unknown;
00179             double free_space;
00180             int frontier_point_message;
00181             int frontier_id_count, visited_frontier_id_count,unreachable_frontier_id_count;
00182             int goal_point_simulated_message, start_point_simulated_message;
00183             bool simulation;
00184             int threshold_free, threshold_inflated, threshold_lethal;
00185             int robot_name;
00186             double robot_home_x, robot_home_y;
00187             std::string move_base_frame, robots_in_simulation;
00188             std::string robot_str;
00189             std::vector<std::string> new_robots;
00190             int next_auction_position_x, next_auction_position_y;
00191             
00192             ExplorationPlanner(int robot_id, bool robot_prefix_empty, std::string robot_name_parameter);
00193             void printFrontiers();
00194             bool respondToAuction(std::vector<requested_cluster_t> requested_cluster_ids, int auction_id_number);
00195             bool clusterIdToElementIds(int cluster_id, std::vector<transform_point_t>* occupied_ids);
00196             bool initialize_auctioning(std::vector<double> *final_goal);
00197             bool auctioning(std::vector<double> *final_goal, std::vector<int> *clusters_available_in_pool, std::vector<std::string> *robot_str_name);
00198             bool selectClusterBasedOnAuction(std::vector<double> *goal, std::vector<int> *cluster_in_use_already_count, std::vector<std::string> *robot_str_name_to_return);
00199             bool InitSelectClusterBasedOnAuction(std::vector<double> *goal);
00200             int calculateAuctionBID(int cluster_number, std::string strategy);
00201             std::string lookupRobotName(int robot_name_int);
00202 //            void auctionStatusCallback(const adhoc_communication::AuctionStatus::ConstPtr& msg);
00203 //            void controlCallback(const bla& msg);
00204             int calculate_travel_path(double x, double y);
00205             int estimate_trajectory_plan(double start_x, double start_y, double target_x, double target_y);
00206             void Callbacks();
00207 //            void new_robot_callback(const std_msgs::StringConstPtr &msg);
00208             int checkClustersID(adhoc_communication::ExpCluster cluster_to_check);
00209             bool determine_goal(int strategy, std::vector<double> *final_goal, int count, int actual_cluster_id, std::vector<std::string> *robot_str_name);
00210             void sort(int strategy);
00211             void simulate();
00212             void visualize_Frontiers();
00213             void visualize_visited_Frontiers();
00214             void visualizeClustersConsole();
00215             void clear_Visualized_Cluster_Cells(std::vector<int> ids);
00216             void initialize_planner(std::string name, costmap_2d::Costmap2DROS *costmap, costmap_2d::Costmap2DROS *costmap_global);
00217             void findFrontiers();
00218             bool check_efficiency_of_goal(double x, double y);
00219             void clearVisitedAndSeenFrontiersFromClusters();
00220             void clearSeenFrontiers(costmap_2d::Costmap2DROS *global_costmap);
00221             void clearVisitedFrontiers();
00222             void clearUnreachableFrontiers();
00223             bool storeFrontier(double x, double y, int detected_by_robot, std::string detected_by_robot_str, int id);
00224             bool storeVisitedFrontier(double x, double y, int detected_by_robot, std::string detected_by_robot_str, int id);
00225             bool storeUnreachableFrontier(double x, double y, int detected_by_robot, std::string detected_by_robot_str, int id);
00226             bool removeStoredFrontier(int id, std::string detected_by_robot_str);
00227             bool removeVisitedFrontier(int id, std::string detected_by_robot_str);
00228             bool removeUnreachableFrontier(int id, std::string detected_by_robot_str);
00229             bool publish_frontier_list();
00230             bool publish_visited_frontier_list();
00231             bool publish_negotiation_list(frontier_t negotiation_frontier, int cluster);
00232             bool subscribe_negotiation_list();
00233             bool subscribe_frontier_list();
00234             bool subscribe_visited_frontier_list();
00235             void negotiationCallback(const adhoc_communication::ExpFrontier::ConstPtr& msg);
00236             void visited_frontierCallback(const adhoc_communication::ExpFrontier::ConstPtr& msg);
00237             void frontierCallback(const adhoc_communication::ExpFrontier::ConstPtr& msg);
00238             void auctionCallback(const adhoc_communication::ExpAuction::ConstPtr& msg);
00239             void positionCallback(const adhoc_communication::MmListOfPoints::ConstPtr& msg);
00240             bool smartGoalBackoff(double x, double y, costmap_2d::Costmap2DROS *global_costmap, std::vector<double> *new_goal);
00241             void setRobotConfig(int name, double robot_home_position_x, double robot_home_position_y, std::string frame);
00242             bool check_trajectory_plan();
00243             int check_trajectory_plan(double x, double y);
00244             bool negotiate_Frontier(double x, double y, int detected_by, int id, int cluster);
00245             bool clusterFrontiers();
00246             void visualize_Clusters();
00247             void visualize_Cluster_Cells();
00248             
00249             bool transformToOwnCoordinates_frontiers();
00250             bool transformToOwnCoordinates_visited_frontiers();
00251 
00252             bool sendToMulticast(std::string multi_cast_group, adhoc_communication::ExpFrontier frontier_to_send, std::string topic);
00253             bool sendToMulticastAuction(std::string multi_cast_group, adhoc_communication::ExpAuction auction_to_send, std::string topic);
00254             
00255         private:
00256 
00257             //Edit Peter
00258             bool auction_running;
00259 
00260             void home_position_(const geometry_msgs::PointStamped::ConstPtr& msg);
00261             void clearFrontiers();
00262             int isFrontier(int point);
00263             bool isFree(int point);
00264             inline bool isValid(int point);
00265             inline void getAdjacentPoints(int point, int points[]);
00266             std::vector<int> getMapNeighbours(unsigned int point_x, unsigned int point_y, int distance);
00267             bool isFrontierReached(int point);
00268             int backoff (int point);
00269             double getYawToUnknown(int point);
00270             bool isSameFrontier(int frontier_point1, int frontier_point2);
00271             void setupMapData();
00272             void deleteMapData();
00273             void resetMaps();
00274             bool countCostMapBlocks(int point);
00275 
00276             int left(int point);
00277             int upleft(int point);
00278             int up(int point);
00279             int upright(int point);
00280             int right(int point);
00281             int downright(int point);
00282             int down(int point);
00283             int downleft(int point);
00284 
00285             enum LastMode
00286             {
00287                 FRONTIER_EXPLORE,
00288                 INNER_EXPLORE
00289               } last_mode_;
00290 
00291             costmap_2d::Costmap2DROS *costmap_ros_;
00292             costmap_2d::Costmap2DROS *costmap_global_ros_;
00293             costmap_2d::Costmap2D costmap_;
00294             costmap_2d::Costmap2D costmap_global_;
00295 
00296             ros::Publisher visualization_pub_;
00297 
00298             const unsigned char* occupancy_grid_array_;
00299             unsigned int* exploration_trans_array_;
00300             unsigned int* obstacle_trans_array_;
00301             int* frontier_map_array_;
00302             int home_position_x_,home_position_y_;
00303 
00304             bool* is_goal_array_;
00305             bool initialized_;
00306             int previous_goal_;
00307 
00308             std::string name;
00309             unsigned int map_width_;
00310             unsigned int map_height_;
00311             unsigned int num_map_cells_;
00312 
00313             // Parameters
00314             bool p_plan_in_unknown_;
00315             bool p_use_inflated_obs_;
00316             bool p_security_constant_;  //FIXME
00317             int p_goal_angle_penalty_;
00318             int p_min_obstacle_dist_;
00319             int p_min_frontier_size_;
00320             double p_alpha_;
00321             double p_dist_for_goal_reached_;
00322             double p_same_frontier_dist_;
00323 
00324             //bool p_plan_in_unknown_;
00325             //bool p_use_inflated_obs_;
00326 
00327             //boost::shared_ptr<dynamic_reconfigure::Server<explorationPlanner::ExplorationPlanner> > dyn_rec_server_;
00328 
00329         };
00330 }
00331 #endif


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