00001 00011 #include <carl_navigation/BlockedCells.h> 00012 #include <nav_msgs/GetMap.h> 00013 #include <ros/ros.h> 00014 00015 class MapRepublisher 00016 { 00017 public: 00021 MapRepublisher(); 00022 00026 void publishMap(); 00027 00028 private: 00029 //ROS publishers, subscribers, and action servers 00030 ros::NodeHandle n; 00031 00032 ros::Subscriber mapSubscriber; 00033 ros::Subscriber obstacleGridSubscriber; 00034 00035 ros::Publisher mapPublisher; 00036 ros::Publisher mapMetaDataPublisher; 00037 00038 ros::ServiceServer getMapServer; 00039 00040 int mapSubscribers; //number of subscribers for the map topic 00041 int metaDataSubscribers; //number of subscribers for the map metadata topic 00042 00043 //Map 00044 nav_msgs::OccupancyGrid combinedMap; //combined static map and obstacles 00045 nav_msgs::OccupancyGrid staticMap; //static map forming the base of the combined map 00046 std::vector<geometry_msgs::Point> obstacleCells; //obstacle information to be added to the map 00047 std::vector<geometry_msgs::Point> obstacleEdges; //edge of obstacles to be added to the map 00048 bool staticMapInitialized; //true if the static map has been read successfully 00049 00056 bool mapServiceCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); 00057 00063 void staticMapCallback(const nav_msgs::OccupancyGrid& map); 00064 00069 void obstacleCallback(const carl_navigation::BlockedCells& grid); 00070 };