Update of /cvsroot/playerstage/code/player/server/drivers/position/nav200
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22520/server/drivers/position/nav200
Modified Files:
sicknav200.cc
Log Message:
clean up debug output and some velocity output fixes
Index: sicknav200.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/position/nav200/sicknav200.cc,v
retrieving revision 1.15
retrieving revision 1.16
diff -C2 -d -r1.15 -r1.16
*** sicknav200.cc 19 Feb 2008 19:06:28 -0000 1.15
--- sicknav200.cc 25 Feb 2008 19:59:19 -0000 1.16
***************
*** 178,181 ****
--- 178,182 ----
double speed;
double steeringAngle;
+ double navAngle;
DoubleProperty wheelBase;
***************
*** 213,220 ****
// Vector map interface
player_devaddr_t vectormap_addr;
- // Position interface
- player_devaddr_t debug_addr;
- player_position2d_data_t debug_packet;
- struct timeval previous;
};
--- 214,217 ----
***************
*** 273,291 ****
return;
}
-
- // Create debug interface
- if (cf->ReadDeviceAddr(&(this->debug_addr), section,
- "provides", PLAYER_POSITION2D_CODE, 2, NULL) != 0)
- {
- PLAYER_ERROR("Could not read debug interface device address.");
- this->SetError(-1);
- return;
- }
- if (this->AddInterface(this->debug_addr))
- {
- PLAYER_ERROR("Could not add debug interface.");
- this->SetError(-1);
- return;
- }
// Laser geometry.
--- 270,273 ----
***************
*** 303,306 ****
--- 285,289 ----
this->steeringAngle = 0;
this->fetchOnStart = false;
+ this->navAngle = 0;
// Serial port - done in the opaque driver
***************
*** 337,342 ****
PLAYER_VECTORMAP_CODE, -1, NULL);
- gettimeofday(&previous, NULL);
-
return;
}
--- 320,323 ----
***************
*** 667,670 ****
--- 648,654 ----
ProcessMessages();
+ double vehicleVelX = 0;
+ double vehicleVelY = 0;
+ double angularVelocity = 0;
bool gotReading;
if (velocity)
***************
*** 677,694 ****
else
{
! // Convert vehicle movement data into NAV200 format and
coordinates.
! //double arcRadius = wheelBase / tan(steeringAngle);
//double angularVelocity = speed / arcRadius;
! double angularVelocity = speed * tan(steeringAngle) /
wheelBase.GetValue();
! double velX = speed - pose[1] * angularVelocity;
double velY = pose[0] * angularVelocity;
double navVelX = velX * cos(pose[2]) + velY * sin(pose[2]);
double navVelY = velY * cos(pose[2]) - velX * sin(pose[2]);
! debug_packet.vel.pa = angularVelocity;
! debug_packet.vel.px = navVelX;
! debug_packet.vel.py = navVelY;
! debug_packet.stall = 0;
! this->Publish(this->debug_addr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE, (void*)&debug_packet, sizeof(debug_packet), NULL);
! gotReading = Laser.GetPositionSpeedVelocity(short(navVelX *
1000), short(navVelY * 1000), short(angularVelocity * 32768.0 / M_PI), Reading);
}
}
--- 661,682 ----
else
{
! // Calculate the vehicles velocity
! double angle = navAngle - pose[2];
! vehicleVelX = speed * cos(angle); // Vehicle velocity in world
coordinates.
! vehicleVelY = speed * sin(angle);
! //double arcRadius = wheelBase / tan(steeringAngle);
//double angularVelocity = speed / arcRadius;
! angularVelocity = speed * tan(steeringAngle) /
wheelBase.GetValue(); // Angular velocity of NAV and vehicle.
!
! // Convert vehicle movement data into NAV200 coordinates.
! double velX = speed - pose[1] * angularVelocity; // NAV vel in
vehicle coordinates.
double velY = pose[0] * angularVelocity;
double navVelX = velX * cos(pose[2]) + velY * sin(pose[2]);
double navVelY = velY * cos(pose[2]) - velX * sin(pose[2]);
! //gotReading = Laser.GetPositionSpeedVelocity(short(navVelX *
1000), short(navVelY * 1000), short(angularVelocity * 32768.0 / M_PI), Reading);
! // Convert NAV200 velocity into world coordinates.
! double worldVelX = navVelX * cos(navAngle) - navVelY *
sin(navAngle);
! double worldVelY = navVelX * sin(navAngle) + navVelY *
cos(navAngle);
! gotReading =
Laser.GetPositionSpeedVelocityAbsolute(short(worldVelX * 1000), short(worldVelY
* 1000), short(angularVelocity * 32768.0 / M_PI), Reading);
}
}
***************
*** 700,704 ****
{
// Use NAV200 position and orientation data to determine vehicle
position and orientation.
! double angle = Reading.orientation/32768.0*M_PI - pose[2];
double forwardx = cos(angle);
double forwardy = sin(angle);
--- 688,693 ----
{
// Use NAV200 position and orientation data to determine vehicle
position and orientation.
! navAngle = Reading.orientation/32768.0*M_PI;
! double angle = navAngle - pose[2];
double forwardx = cos(angle);
double forwardy = sin(angle);
***************
*** 708,729 ****
double newX = static_cast<double> (Reading.pos.x)/1000 - forwardx *
pose[0] - leftx * pose[1];
double newY = static_cast<double> (Reading.pos.y)/1000 - forwardy *
pose[0] - lefty * pose[1];
-
- // Begin debug stuff
- struct timeval current;
- gettimeofday(¤t, NULL);
- double dt = static_cast<double> (current.tv_sec - previous.tv_sec) +
static_cast<double>(current.tv_usec - previous.tv_usec) / 1000000.0;
-
- double angVel = (newAngle - data_packet.pos.pa) / dt;
- double velX = (newX - data_packet.pos.px) / dt;
- double velY = (newY - data_packet.pos.py) / dt;
- PLAYER_MSG3(2, "Vx: %lf\tVy: %lf\tVa: %lf", velX, velY, angVel);
- data_packet.vel.pa = angVel;
- data_packet.vel.px = velX;
- data_packet.vel.py = velY;
- // End debug stuff
-
data_packet.pos.pa = newAngle;
data_packet.pos.px = newX;
data_packet.pos.py = newY;
if(Reading.quality==0xFF || Reading.quality==0xFE ||
Reading.quality==0x00)
{
--- 697,706 ----
double newX = static_cast<double> (Reading.pos.x)/1000 - forwardx *
pose[0] - leftx * pose[1];
double newY = static_cast<double> (Reading.pos.y)/1000 - forwardy *
pose[0] - lefty * pose[1];
data_packet.pos.pa = newAngle;
data_packet.pos.px = newX;
data_packet.pos.py = newY;
+ data_packet.vel.pa = angularVelocity;
+ data_packet.vel.px = vehicleVelX;
+ data_packet.vel.py = vehicleVelY;
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 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
