Update of /cvsroot/playerstage/code/player/server/drivers/position/vfh
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4552/server/drivers/position/vfh
Modified Files:
Tag: release-2-0-patches
vfh.cc
Log Message:
backported lots of stuff from HEAD
Index: vfh.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/position/vfh/vfh.cc,v
retrieving revision 1.71
retrieving revision 1.71.2.1
diff -C2 -d -r1.71 -r1.71.2.1
*** vfh.cc 1 Mar 2006 02:37:10 -0000 1.71
--- vfh.cc 7 Jun 2006 16:12:53 -0000 1.71.2.1
***************
*** 531,535 ****
db = RTOD(data.resolution);
! this->laser_count = data.ranges_count;
assert(this->laser_count <
(int)sizeof(this->laser_ranges) /
(int)sizeof(this->laser_ranges[0]));
--- 531,535 ----
db = RTOD(data.resolution);
! this->laser_count = 181;
assert(this->laser_count <
(int)sizeof(this->laser_ranges) /
(int)sizeof(this->laser_ranges[0]));
***************
*** 538,547 ****
this->laser_ranges[i][0] = -1;
! b += 90.0;
! for(i = 0; i < this->laser_count; i++)
{
! this->laser_ranges[(int)rint(b * 2)][0] = data.ranges[i] * 1e3;
! this->laser_ranges[(int)rint(b * 2)][1] = b;
! b += db;
}
--- 538,550 ----
this->laser_ranges[i][0] = -1;
! // vfh seems to be very oriented around 180 degree scans so interpolate to
get 180 degrees
! // b += 90.0;
! for(i = 0; i < 181; i++)
{
! unsigned int index = rint(i/db);
! assert(index >= 0 && index < data.ranges_count);
! this->laser_ranges[i*2][0] = data.ranges[index] * 1e3;
! // this->laser_ranges[i*2][1] = index;
! // b += db;
}
***************
*** 566,569 ****
--- 569,573 ----
double cone_width = 30.0;
int count = 361;
+ float sonarDistToCenter = 0.0;
this->laser_count = count;
***************
*** 583,587 ****
if((b < 0) || (rint(b*2) >= count))
continue;
! this->laser_ranges[(int)rint(b * 2)][0] = data.ranges[i] * 1e3;
this->laser_ranges[(int)rint(b * 2)][1] = b;
}
--- 587,598 ----
if((b < 0) || (rint(b*2) >= count))
continue;
! // Sonars give distance readings from the perimeter of the robot while
lasers give distance
! // from the laser; hence, typically the distance from a single point,
like the center.
! // Since this version of the VFH+ algorithm was written for lasers and
we pass the algorithm
! // laser ranges, we must make the sonar ranges appear like laser
ranges. To do this, we take
! // into account the offset of a sonar's geometry from the center.
Simply add the distance from
! // the center of the robot to a sonar to the sonar's range reading.
! sonarDistToCenter = sqrt(pow(this->sonar_poses[i].px,2) +
pow(this->sonar_poses[i].py,2));
! this->laser_ranges[(int)rint(b * 2)][0] = (sonarDistToCenter +
data.ranges[i]) * 1e3;
this->laser_ranges[(int)rint(b * 2)][1] = b;
}
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
