00001 #ifndef THREADGRID_H_ 00002 #define THREADGRID_H_ 00003 00004 #include "ThreadSLAM.h" 00005 00006 #include "obvision/reconstruct/grid/TsdGrid.h" 00007 00008 #include <ros/ros.h> 00009 #include <nav_msgs/OccupancyGrid.h> 00010 #include <nav_msgs/GetMap.h> 00011 00012 namespace ohm_tsd_slam 00013 { 00014 00020 class ThreadGrid : public ThreadSLAM 00021 { 00022 public: 00023 00030 ThreadGrid(obvious::TsdGrid* grid, ros::NodeHandle* const nh, const double xOffset, const double yOffset); 00031 00035 virtual ~ThreadGrid(); 00036 00037 protected: 00038 00043 virtual void eventLoop(void); 00044 00045 private: 00046 00054 bool getMapServCallBack(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res); 00055 00059 nav_msgs::OccupancyGrid* _occGrid; 00060 00064 ros::ServiceServer _getMapServ; 00065 00069 char* _occGridContent; 00070 00074 double* _gridCoords; 00075 00079 unsigned int _width; 00080 00084 unsigned int _height; 00085 00089 double _cellSize; 00090 00094 ros::Publisher _gridPub; 00095 00099 unsigned int _objInflateFactor; 00100 00104 bool _objectInflation; 00105 }; 00106 00107 } /* namespace ohm_tsd_slam */ 00108 00109 #endif /* THREADGRID_H_ */