00001 #ifndef SLAMNODE_H_ 00002 #define SLAMNODE_H_ 00003 00004 #include <ros/ros.h> 00005 00006 #include <vector> 00007 00008 #include "obvision/reconstruct/grid/TsdGrid.h" 00009 #include "obvision/reconstruct/grid/SensorPolar2D.h" 00010 #include "obcore/base/Logger.h" 00011 00012 #define INIT_PSHS 1 //number of initial pushes into the grid 00013 #define THREAD_TERM_MS 1 //time in ms waiting for thread to terminate 00014 00015 namespace ohm_tsd_slam 00016 { 00017 class ThreadSLAM; 00018 class ThreadMapping; 00019 class ThreadGrid; 00020 class ThreadLocalize; 00021 00027 class SlamNode 00028 { 00029 public: 00030 00034 SlamNode(void); 00035 00039 virtual ~SlamNode(); 00040 00045 void start(void){this->run();} 00046 private: 00047 00052 void run(void); 00053 00058 void timedGridPub(void); 00059 00063 ros::NodeHandle _nh; 00064 00068 obvious::TsdGrid* _grid; 00069 00073 ThreadMapping* _threadMapping; 00074 00078 ThreadGrid* _threadGrid; 00079 00083 double _xOffFactor; 00084 00088 double _yOffFactor; 00089 00093 ros::Duration* _gridInterval; 00094 00098 ros::Rate* _loopRate; 00099 00103 std::vector<ros::Subscriber> _subsLaser; 00104 00105 00109 std::vector<ThreadLocalize*> _localizers; 00110 }; 00111 00112 } /* namespace ohm_tsd_slam */ 00113 00114 #endif /* SLAMNODE_H_ */