Update of /cvsroot/playerstage/code/player/server/drivers/position/nav200
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv11204/server/drivers/position/nav200
Modified Files:
sicknav200.cc
Log Message:
added pose setting for nav200
added option to invert s300/s3000 laser data
Index: sicknav200.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/sicknav200.cc,v
retrieving revision 1.11
retrieving revision 1.12
diff -C2 -d -r1.11 -r1.12
*** sicknav200.cc 18 Dec 2007 05:05:09 -0000 1.11
--- sicknav200.cc 20 Dec 2007 23:28:27 -0000 1.12
***************
*** 518,525 ****
if(Laser.GetPositionAuto(Reading))
{
! data_packet.pos.px = static_cast<double> (Reading.pos.x)/1000;
! data_packet.pos.py = static_cast<double> (Reading.pos.y)/1000;
! double angle = M_PI + Reading.orientation/32768.0*M_PI;
! data_packet.pos.pa = atan2(sin(angle), cos(angle));
if(Reading.quality==0xFF || Reading.quality==0xFE ||
Reading.quality==0x00)
{
--- 518,529 ----
if(Laser.GetPositionAuto(Reading))
{
! double angle = M_PI + Reading.orientation/32768.0*M_PI - pose[2];
! double dx = sin(angle);
! double dy = cos(angle);
! double rx = sin(angle + M_PI/2.0);
! double ry = cos(angle + M_PI/2.0);
! data_packet.pos.pa = atan2(dx, dy);
! data_packet.pos.px = static_cast<double> (Reading.pos.x)/1000 - dx *
pose[1] - rx * pose[0];
! data_packet.pos.py = static_cast<double> (Reading.pos.y)/1000 - dy *
pose[1] - ry * pose[0];
if(Reading.quality==0xFF || Reading.quality==0xFE ||
Reading.quality==0x00)
{
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
