00001
00002
00003
00004
00005
00006
00007 #ifndef CAT_CREATOR_H_
00008 #define CAT_CREATOR_H_
00009
00010 #include "ros/ros.h"
00011 #include "burst_calc/burst.h"
00012 #include "burst_calc/ranges.h"
00013 #include "burst_calc/cat.h"
00014 #include "burst_calc/ca.h"
00015 #include "neuro_recv/dish_state.h"
00016 #include <fstream>
00017 #include <string>
00018
00019 const int X_COORD_[60] = { 2, 3, 4, 5, 6, 7,
00020 1, 2, 3, 4, 5, 6, 7, 8,
00021 1, 2, 3, 4, 5, 6, 7, 8,
00022 1, 2, 3, 4, 5, 6, 7, 8,
00023 1, 2, 3, 4, 5, 6, 7, 8,
00024 1, 2, 3, 4, 5, 6, 7, 8,
00025 1, 2, 3, 4, 5, 6, 7, 8,
00026 2, 3, 4, 5, 6, 7 };
00027 const int Y_COORD_[60] = { 1, 1, 1, 1, 1, 1,
00028 2, 2, 2, 2, 2, 2, 2, 2,
00029 3, 3, 3, 3, 3, 3, 3, 3,
00030 4, 4, 4, 4, 4, 4, 4, 4,
00031 5, 5, 5, 5, 5, 5, 5, 5,
00032 6, 6, 6, 6, 6, 6, 6, 6,
00033 7, 7, 7, 7, 7, 7, 7, 7,
00034 8, 8, 8, 8, 8, 8 };
00035
00052 class CatCreator
00053 {
00054 public:
00055 CatCreator() { init(); }
00056 ~CatCreator();
00057
00058 private:
00059 void init();
00060 void getParams();
00061 void updateOffsets(const neuro_recv::dish_state& d);
00062 void callback(const burst_calc::burst::ConstPtr& b);
00063 void rangesCallback(const burst_calc::ranges::ConstPtr& r);
00064 bool caExists(const neuro_recv::dish_state& d);
00065 const burst_calc::ca getCa(const neuro_recv::dish_state& d);
00066 void initFile(const char* cat_file);
00067 void headerToFile(const burst_calc::burst& b);
00068 void toFile(int i, const neuro_recv::dish_state& d, const burst_calc::ca& c);
00069 void toFile(int i, const neuro_recv::dish_state& d);
00070
00071
00072 ros::NodeHandle n_;
00073 ros::Subscriber burst_sub_;
00074 ros::Subscriber ranges_sub_;
00075 ros::Publisher cat_pub_;
00076 ros::Publisher ca_pub_;
00077 std::ofstream cat_file_;
00078 std::string file_name_;
00079 double offsets_[60];
00080 double thresholds_[60];
00081 bool save_to_file_;
00082 bool is_init_;
00083 };
00084
00085 #endif