mapmerger.h
Go to the documentation of this file.
00001 #ifndef MAPMERGER_H
00002 #define MAPMERGER_H
00003 
00004 #include "ros/ros.h"
00005 #include "stdio.h"
00006 #include "updatemanager.h"
00007 #include "nav_msgs/OccupancyGrid.h"
00008 #include "nav_msgs/Odometry.h"
00009 #include "std_msgs/String.h"
00010 #include "geometry_msgs/Pose.h"
00011 #include "geometry_msgs/PoseStamped.h"
00012 #include "nav_msgs/Odometry.h"
00013 #include "opencv2/imgproc/imgproc.hpp"
00014 #include "opencv2/highgui/highgui.hpp"
00015 #include "opencv2/video/tracking.hpp"
00016 
00017 #include "mapstitch.h"
00018 #include "adhoc_communication/SendOccupancyGrid.h"
00019 #include "adhoc_communication/SendMmRobotPosition.h"
00020 #include "adhoc_communication/MmRobotPosition.h"
00021 #include "adhoc_communication/MmListOfPoints.h"
00022 //#include "adhoc_communication/PointFromOtherRobot.h"
00023 #include "adhoc_communication/MmMapUpdate.h"
00024 #include "adhoc_communication/MmControl.h"
00025 #include "adhoc_communication/SendMmControl.h"
00026 #include "adhoc_communication/SendMmMapUpdate.h"
00027 #include "adhoc_communication/GetNeighbors.h"
00028 
00029 #include "updateentry.h"
00030 
00031 #include "map_merger/TransformPoint.h"
00032 #include "map_merger/LogMaps.h"
00033 
00034 #include "updateentry.h"
00035 
00036 #include "adhoc_communication/ExpFrontier.h"
00037 #include "adhoc_communication/ExpFrontierElement.h"
00038 #include "visualization_msgs/MarkerArray.h"
00039 
00040 
00041 enum MapMergerLog{
00042     LOG_GLOBAL_MAP          = 0x01,
00043     LOG_LOCAL_MAP           = 0x02,
00044     LOG_LOCAL_MAP_PROGRESS  = 0x04,
00045     LOG_GLOBAL_MAP_PROGRESS = 0x08
00046 };
00047 
00048 
00049 class MapMerger
00050 {
00051 public:
00052     MapMerger();
00053     void start();
00054     void waitForLocalMetaData();
00055     void waitForRobotInformation();
00056     bool transformPointSRV(map_merger::TransformPoint::Request &req,
00057                            map_merger::TransformPoint::Response &res);
00058     bool log_output_srv(map_merger::LogMaps::Request &req,
00059                         map_merger::LogMaps::Response &res);
00060     bool getHasLocalMap();
00061 private:
00062     //Private Methods
00063     //callback methods
00064     void callback_map(const nav_msgs::OccupancyGrid::ConstPtr& msg);
00065     void callback_map_other(const adhoc_communication::MmMapUpdateConstPtr &msg);
00066     void callback_control(const adhoc_communication::MmControlConstPtr &msg);
00067     void callback_map_meta_data_local(const nav_msgs::MapMetaData::ConstPtr &msg);
00068     void callback_robot_status(const nav_msgs::MapMetaData::ConstPtr &msg);
00069     void callback_global_pub(const ros::TimerEvent &e);
00070     void callback_send_map(const ros::TimerEvent &e);
00071     void callback_send_position(const ros::TimerEvent &e);
00072     void callback_recompute_transform(const ros::TimerEvent &e);
00073     void callback_got_position(const nav_msgs::Odometry::ConstPtr &msg);
00074     void callback_got_position_network(const adhoc_communication::MmRobotPosition::ConstPtr &msg);
00075     void callback_new_robot(const std_msgs::StringConstPtr &msg);
00076     void callback_ask_other_robots(const ros::TimerEvent &e);
00077     void callback_remove_robot(const std_msgs::StringConstPtr &msg);
00078 
00079     void callback_write_maps(const ros::TimerEvent &e);
00080 
00081     //int since_last_trans_try;
00082     void computeOffsets();
00083     void computeTransform(int mapDataIndex);
00084     bool recomputeTransform(int mapDataIndex);
00085     void makeEmptyMapData(string robot_name,int height,int width,float resolution);
00086     void mergeMaps(nav_msgs::OccupancyGrid *mapToMerge,int min_x = 0,int min_y = 0,int max_x = -1,int max_y = -1);
00087     void callback_got_robot_for_data(const std_msgs::StringConstPtr &msg);
00088     void sendControlMessage(std::vector<int>* updateNumbers,std::string dest);
00089 
00090     void processMap(nav_msgs::OccupancyGrid *map,int index_in_mapdata);
00091     void processLocalMap(nav_msgs::OccupancyGrid * toInsert,int index);
00092     void processPosition(geometry_msgs::PoseStamped * pose);
00093 
00094     void updateMap(nav_msgs::OccupancyGrid *mapToUpdate,int index_of_transform);
00095     void updateMapArea(int map_index,nav_msgs::OccupancyGrid *newData,bool clear = false);
00096     int findTransformIndex(int robot_index);
00097     int findRobotIndex(int transform_index);
00098     void sendMapOverNetwork(string destination,std::vector<int>* containedUpdates,int start_row = 0,int start_collum = 0,int end_row = -1,int end_collum = -1);
00099     void sendMetaData(float res = 0.05);
00100     void sendBackAskedMapData(string robotName, std::vector<int> missingUpdates );
00101     nav_msgs::OccupancyGrid* getMapPart(int map_index,int start_x,int start_y,int width,int height);
00102     nav_msgs::OccupancyGrid* matToMap(const cv::Mat mat, nav_msgs::OccupancyGrid *forInfo);
00103     bool createLogPath();
00104 
00105     cv::Mat mapToMat(const nav_msgs::OccupancyGrid *map);
00106     cv::Mat transformImage(Mat img1,Mat trans);
00107     updateManager * updateMan;
00108     //Methots accsesing opencv
00109     ros::NodeHandle * nodeHandle;
00110     //Private Lists
00111     std::vector<std::string>* robots;
00112     std::vector<std::string>* robots_in_transform;
00113     //std::vector<geometry_msgs::PoseStamped> _position_otherrobots;
00114     std::vector<cv::Mat>* transforms;
00115     std::vector<bool>* new_data_maps;
00116     std::vector<UpdateEntry>* formerUpdates;
00117     std::vector<ros::Publisher> *robots_position_publisher;
00118     ros::Publisher  list_of_positions_publisher;
00119     //std::vector<adhoc_communication::
00120     adhoc_communication::MmListOfPoints * positions;
00121     int map_width,map_height,size,
00122         seconds_meta_timer,seconds_publish_timer,
00123         seconds_send_timer,seconds_recompute_transform,
00124         seconds_send_position,
00125         laser_range,updateCount;
00126     std::vector<nav_msgs::OccupancyGrid*>* map_data;
00127     std::vector<UpdateEntry*>* update_list;
00128     nav_msgs::OccupancyGrid * global_map, * local_map, * local_map_old;
00129     //Private variables
00130     bool    debug,
00131             splitted,
00132             has_local_map,
00133             exchange_position,
00134             local_map_new_data,
00135             force_recompute_all,
00136             global_map_ready;
00137 //            convergente;
00138     cv::Mat lastTrans;
00139     ros::Publisher pub;
00140     geometry_msgs::PoseStamped * cur_position;
00141     float g_start_x,g_start_y;
00142     double max_trans_robot,max_rotation_robot;
00143     int update_seq;
00144     std::string topic_over_network,
00145                 local_map_frame_id,
00146                 local_map_topic,
00147                 local_map_metadata_topic,
00148                 position_local_robot_topic,
00149                 map_meta_topic,
00150                 position_other_robots_topic,
00151                 robot_prefix,
00152                 robot_name,
00153                 control_topic,
00154                 log_path,                   
00155                 full_log_path,
00156                 robot_hostname;              
00157     ros::Time time_start;
00158     double first_trans_x,first_trans_y;
00159     float old_middle_distance;
00160     ros::Timer send_position,recompute_transform_timer,global_timer_pub,send_map,ask_other_timer;
00161 
00162     std::vector<ros::Publisher> * pos_pub_other;
00163     ros::Publisher my_pos_pub;
00164     std::vector<visualization_msgs::MarkerArray> *  pos_array_other;
00165     visualization_msgs::MarkerArray pos_array_my;
00166     std::vector<int> * pos_seq_other;
00167     int pos_seq_my;
00168     bool turning;
00169 };
00170 
00171 #endif // MAPMERGER_Hd


map_merger
Author(s): Peter Kohout , Torsten Andre
autogenerated on Thu Aug 27 2015 11:56:48