On Tue, 9 Dec 2003 parker@... 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.
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.
- 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
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 *
ly_rot = this->localize_x * sin(offset_a) + this->localize_y *
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;
p.s. hmm, i hope i got that transform right....