From: Brian G. <ge...@ro...> - 2003-12-09 22:25:57
|
On Tue, 9 Dec 2003 pa...@cs... wrote: > The question is how to give a relative goal to vfh if the odometry origin > is drifting over time. If we can't reset the odometry without making the > laser localization go crazy, then it is unclear how to keep vfh working > over long distances. > > Any advice would be appreciated. hi Lynne, What I do in the 'wavefront' driver, and what should work for you, is to periodically (e.g., every time you get new localization data) re-compute the transform from localization coordinates to odometric coordinates, using the latest localization and position data. This will remove the odometric drift. Use the resulting transformation to re-transform your current global goal into local coordinates, and send this "new" goal to VFH. I do this transformation in the 'wavefront' driver using the method below, which you should be able to adapt for your needs. Notes: - the macro NORMALIZE is defined if you #include <player.h> - the latest localize pose is stored in this->localize_* - the latest odometric pose is stored in this->position_* - the global target is passed in as (lx,ly,la) - the local target is returned as (px,py,pa) - EVERYTHING'S MEASURED IN METERS AND RADIANS void Wavefront::LocalizeToPosition(double* px, double* py, double* pa, double lx, double ly, double la) { double offset_x, offset_y, offset_a; double lx_rot, ly_rot; offset_a = NORMALIZE(this->position_a - this->localize_a); lx_rot = this->localize_x * cos(offset_a) - this->localize_y * sin(offset_a); ly_rot = this->localize_x * sin(offset_a) + this->localize_y * cos(offset_a); offset_x = this->position_x - lx_rot; offset_y = this->position_y - ly_rot; //printf("offset: %f, %f, %f\n", offset_x, offset_y, RTOD(offset_a)); *px = lx * cos(offset_a) - ly * sin(offset_a) + offset_x; *py = lx * sin(offset_a) + ly * cos(offset_a) + offset_y; *pa = la + offset_a; } brian. p.s. hmm, i hope i got that transform right.... |