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 #include <playerconfig.h>
00042 #ifdef INCLUDE_RTKGUI
00043 #include "rtk.h"
00044 #endif
00045
00046 #include <libplayercore/playercore.h>
00047
00048 #include "pf/pf.h"
00049
00050 class AMCLSensor;
00051 class AMCLSensorData;
00052
00053
00054 typedef struct
00055 {
00056
00057 double weight;
00058
00059
00060 pf_vector_t pf_pose_mean;
00061
00062
00063 pf_matrix_t pf_pose_cov;
00064
00065 } amcl_hyp_t;
00066
00067
00068
00069
00070 class AdaptiveMCL : public ThreadedDriver
00071 {
00073
00074
00076
00077
00078 public: AdaptiveMCL(ConfigFile* cf, int section);
00079
00080
00081 public: virtual ~AdaptiveMCL(void);
00082
00083
00084 public: virtual int MainSetup(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(QueuePointer &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(QueuePointer &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: int hyp_alloc;
00177 private: amcl_hyp_t *hyps;
00178 private: pf_vector_t best_hyp;
00179 private: pthread_mutex_t best_hyp_lock;
00180
00181
00182 private: bool pf_init;
00183 private: bool pf_init_internal;
00184
00185
00186 private: pf_vector_t pf_init_pose_mean;
00187 private: pf_matrix_t pf_init_pose_cov;
00188
00189
00190 private: pf_vector_t pf_odom_pose;
00191
00192
00193 private: double min_dr, min_da;
00194
00195 #ifdef INCLUDE_RTKGUI
00196
00197 private: int enable_gui;
00198 private: rtk_app_t *app;
00199 private: rtk_canvas_t *canvas;
00200 private: rtk_fig_t *map_fig;
00201 private: rtk_fig_t *pf_fig;
00202 private: rtk_fig_t *robot_fig;
00203 #endif
00204
00205 #ifdef INCLUDE_OUTFILE
00206 private: FILE *outfile;
00207 #endif
00208 };
00209
00210 #endif