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
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #ifndef __PCD_FILTER_PA_ROS_FILTER_H
00047 #define __PCD_FILTER_PA_ROS_FILTER_H
00048
00049
00050 #include "pcdfilter_pa/pcdfilter_pa_filter.h"
00051
00052
00053 #include <tf/transform_datatypes.h>
00054 #include <tf/transform_listener.h>
00055
00056
00057 #include <opencv2/highgui/highgui.hpp>
00058
00059
00060 #include <string>
00061 #include <vector>
00062
00063
00064 class cPcdFilterPaRosFilter {
00065 public:
00066 enum eFiltertype {
00067 ftNONE = cPcdFilterPaFilter::ftNONE,
00068 ftCUBE = cPcdFilterPaFilter::ftCUBE,
00069 ftSPHERE = cPcdFilterPaFilter::ftSPHERE,
00070 ftBLOCK = cPcdFilterPaFilter::ftBLOCK,
00071 ftCYLINDER = cPcdFilterPaFilter::ftCYLINDER,
00072 ftCONE = cPcdFilterPaFilter::ftCONE,
00073 ftLINK
00074 };
00075 enum {
00076 COUNT_PARAMETER = 3,
00077 COUNT_FRAME = 2
00078 };
00079
00080 cPcdFilterPaRosFilter();
00081 cPcdFilterPaRosFilter(const cPcdFilterPaRosFilter &other);
00082 cPcdFilterPaRosFilter& operator = (
00083 const cPcdFilterPaRosFilter &other);
00084
00086 bool inverse_;
00087
00089 eFiltertype type_;
00090
00092 bool required_;
00093
00095 double parameter_[COUNT_PARAMETER];
00096
00098 std::string frame_[COUNT_FRAME];
00099
00100
00101 tf::Transform offset_[COUNT_FRAME];
00102
00104 std::string comment_;
00105
00107 bool fromString(const std::string &filter);
00109 std::string toString(void) const;
00110
00112 void reset(void);
00113
00115 bool isValid(void) const;
00116
00117 private:
00119 bool _skipWhitespace(const std::string &str, int &pos) const;
00120
00122 bool _checkSymbol(const std::string &str, int &pos,
00123 const char symbol) const;
00124
00126 std::string _getValue(const std::string &str, int &pos) const;
00127
00129 std::string _getComment(const std::string &str) const;
00130
00132 std::string _floatToStr(const double &value) const;
00133
00135 bool _StrToFloat(const std::string &str, double &value) const;
00136 };
00137
00138
00139 class cPcdFilterPaRosFilters {
00140 public:
00141 cPcdFilterPaRosFilters();
00142
00143
00144
00145 bool add(std::string filter);
00146
00147
00148 void clear(void);
00149
00150
00151 std::vector<std::string> get(void) const;
00152
00153
00154 bool update( const tf::TransformListener &tf_listener,
00155 const std::string base_frame, const ros::Time time,
00156 std::vector<cPcdFilterPaFilter> &result) const;
00157
00158
00159 std::string getLast(void) const;
00160
00161
00162 int size(void) const;
00163
00164
00165 cPcdFilterPaRosFilter& operator[](int i);
00166
00167 const cPcdFilterPaRosFilter& operator[](int i) const;
00168
00169
00170 double tf_lookup_time_;
00171
00172 protected:
00173
00174 double update_transform(const tf::Transform &start,
00175 const tf::Transform &goal, tf::Transform &result,
00176 const double relative_pos = 0.0) const;
00177
00178
00179 std::vector<cPcdFilterPaRosFilter> filters_;
00180
00181 cPcdFilterPaRosFilter last_filter_;
00182 };
00183
00184 #endif // __PCD_FILTER_PA_ROS_FILTER_H