amcl.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00035
00036 #ifndef AMCL_H
00037 #define AMCL_H
00038
00039 #include <pthread.h>
00040
00041 #ifdef INCLUDE_RTKGUI
00042 #include "rtk.h"
00043 #endif
00044
00045 #include <libplayercore/playercore.h>
00046
00047 #include "pf/pf.h"
00048
00049 class AMCLSensor;
00050 class AMCLSensorData;
00051
00052
00053 typedef struct
00054 {
00055
00056 double weight;
00057
00058
00059 pf_vector_t pf_pose_mean;
00060
00061
00062 pf_matrix_t pf_pose_cov;
00063
00064 } amcl_hyp_t;
00065
00066
00067
00068
00069 class AdaptiveMCL : public Driver
00070 {
00072
00073
00075
00076
00077 public: AdaptiveMCL(ConfigFile* cf, int section);
00078
00079
00080 public: virtual ~AdaptiveMCL(void);
00081
00082
00083 public: virtual int Setup(void);
00084 public: virtual int Shutdown(void);
00085
00087
00088
00090
00092
00094
00095
00096 public: void Push(AMCLSensorData *data);
00097
00098
00099 private: AMCLSensorData *Peek(void);
00100
00101
00102 private: AMCLSensorData *Pop(void);
00103
00104
00105 public: virtual int ProcessMessage(MessageQueue * resp_queue,
00106 player_msghdr * hdr,
00107 void * data);
00108
00109
00110 public: virtual void UpdateSensorData(void);
00111
00112
00113 private: virtual void Main(void);
00114
00115
00116 private: virtual void MainQuit();
00117
00118
00119 private: void InitFilter(void);
00120
00121
00122 private: bool UpdateFilter();
00123
00124
00125 private: void PutDataLocalize(double time);
00126
00127
00128 private: void PutDataPosition(pf_vector_t delta, double time);
00129
00130
00131 private: void ProcessGeom(MessageQueue* resp_queue, player_msghdr_t* hdr);
00132
00133 #ifdef INCLUDE_RTKGUI
00134
00135 private: int SetupGUI(void);
00136
00137
00138 private: int ShutdownGUI(void);
00139
00140
00141 private: void UpdateGUI(void);
00142
00143
00144 private: void DrawPoseEst();
00145 #endif
00146
00148
00150
00151
00152 private: player_devaddr_t position_addr;
00153 private: player_devaddr_t localize_addr;
00154
00155
00156 private: int sensor_count;
00157 private: AMCLSensor *sensors[16];
00158
00159
00160 private: int init_sensor;
00161
00162
00163 private: int action_sensor;
00164
00165
00166 private: pf_t *pf;
00167 private: int pf_min_samples, pf_max_samples;
00168 private: double pf_err, pf_z;
00169
00170
00171 private: int q_size, q_start, q_len;
00172 private: AMCLSensorData **q_data;
00173
00174
00175 private: int hyp_count;
00176 private: amcl_hyp_t hyps[PLAYER_LOCALIZE_MAX_HYPOTHS];
00177 private: pf_vector_t best_hyp;
00178 private: pthread_mutex_t best_hyp_lock;
00179
00180
00181 private: bool pf_init;
00182
00183
00184 private: pf_vector_t pf_init_pose_mean;
00185 private: pf_matrix_t pf_init_pose_cov;
00186
00187
00188 private: pf_vector_t pf_odom_pose;
00189
00190
00191 private: double min_dr, min_da;
00192
00193 #ifdef INCLUDE_RTKGUI
00194
00195 private: int enable_gui;
00196 private: rtk_app_t *app;
00197 private: rtk_canvas_t *canvas;
00198 private: rtk_fig_t *map_fig;
00199 private: rtk_fig_t *pf_fig;
00200 private: rtk_fig_t *robot_fig;
00201 #endif
00202
00203 #ifdef INCLUDE_OUTFILE
00204 private: FILE *outfile;
00205 #endif
00206 };
00207
00208 #endif
Last updated 12 September 2005 21:38:45
|