[Playerstage-commit] code/player/server/drivers/position/nav200 sicknav200.cc, 1.11, 1.12

[Toby C. <th...@us...>]

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)
        {