Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _laser_people_label_alg_node_h_
00026 #define _laser_people_label_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "laser_people_label_alg.h"
00030
00031 #include <sys/stat.h>
00032 #include <rosbag/bag.h>
00033 #include <rosbag/view.h>
00034 #include <rosbag/query.h>
00035
00036
00037 #include <visualization_msgs/MarkerArray.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <sensor_msgs/LaserScan.h>
00040
00041
00042
00043
00044
00045 typedef enum {idle, segment, next, skip, go, end, Apos, Aneg, Areset, play, stop} ButtonType;
00046 typedef enum {standby, ending, playing} ModeType;
00047
00052 class LaserPeopleLabelAlgNode : public algorithm_base::IriBaseAlgorithm<LaserPeopleLabelAlgorithm>
00053 {
00054 private:
00055
00056 bool debug;
00057
00058 ros::Publisher areas_array_publisher_;
00059 visualization_msgs::MarkerArray MarkerArrayAreas_msg_;
00060
00061 ros::Publisher buttons_array_publisher_;
00062 visualization_msgs::MarkerArray MarkerArrayButtons_msg_;
00063
00064 ros::Publisher scanLabel_publisher_;
00065 sensor_msgs::LaserScan LaserScan_msg_;
00066
00067
00068 ros::Subscriber goal_subscriber_;
00069 void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
00070 CMutex goal_mutex_;
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 rosbag::Bag bag;
00082 rosbag::View view;
00083 rosbag::View::iterator iter;
00084
00085 sensor_msgs::LaserScan currentScan;
00086 std::vector<sensor_msgs::LaserScan> bagScans;
00087
00088 string filePath;
00089 string bagFilePath;
00090 string labelsFilePath;
00091 string scanTopic;
00092
00093 int currentIteration;
00094 int currentBagScan;
00095 bool append;
00096 bool skipScan;
00097 int modeArea;
00098 int areaVertex;
00099
00100 std::vector< float > area;
00101 std::vector< std::vector< float > > areaListPos;
00102 std::vector< std::vector< float > > areaListNeg;
00103
00104
00105 vector<float> intensities;
00106 ButtonType button_type;
00107 ModeType mode;
00108
00109 vector<float> currentGoal;
00110
00111 void initialize();
00112 void loadBag(void);
00113 bool doFileExist(string filePath);
00114 void getButton(vector<float> goal);
00115 void doButton();
00116 void nextScan();
00117 void goToScan();
00118 void setArea();
00119 void labelAreas();
00120 void fillButtons(visualization_msgs::MarkerArray & buttons);
00121 void fillAreas(visualization_msgs::MarkerArray & areas);
00122
00123 public:
00130 LaserPeopleLabelAlgNode(void);
00131
00138 ~LaserPeopleLabelAlgNode(void);
00139
00140 protected:
00153 void mainNodeThread(void);
00154
00167 void node_config_update(Config &config, uint32_t level);
00168
00175 void addNodeDiagnostics(void);
00176
00177
00178
00179
00180 };
00181
00182 #endif