int main(int argc, char **argv)
{
PlayerClient client("localhost");
RangerProxy ranger(&client, 0);
// Perform a preliminary fetch from the robot.
client.Read();
// Pull configuration information from remote ranger to the local ranger
// proxy.
ranger.RequestConfigure();
ranger.RequestGeom();
// Check the number of elements in the ranger
// We only support laser-scanners (single-element, fan-of-readings devices)
if(ranger.GetElementCount() != 1)
{
cerr << "This is not a laser rangefinder." << endl;
}
// Extract the scan parameters
unsigned int num_readings = ranger.GetRangeCount();
double angle_min = ranger.GetMinAngle();
double angle_max = ranger.GetMaxAngle();
double angle_delta = (angle_max - angle_min) / num_readings;
cout << "Found laser ranger with properties: " << endl;
cout << "min angle: " << angle_min << endl;
cout << "max angle: " << angle_max << endl;
cout << "num readings: " << num_readings << endl;
cout << "angle delta: " << angle_delta << endl;
// Turn on the laser
ranger.SetPower(true);
// Storage for the point sequence
mat points(2, num_readings);
// Storage for the delta sequence
mat deltas(2, num_readings-1);
// Storage for the cross product and dot product sequences
mat crossps(1, num_readings-2);
mat dotps(1, num_readings-2);
while(true)
{
// Fetch updates from the robot
client.Read();
// Copy readings from the ranger proxy
double angle = angle_min;
for(int count = 0; count < num_readings; count++)
{
points(0, count) = ranger[count] * cos(angle);
points(1, count) = ranger[count] * sin(angle);
angle += angle_delta;
}
// Compute deltas
for(int count = 1; count < num_readings; count++)
{
deltas.col(count-1) = points.col(count) - points.col(count-1);
deltas.col(count-1) /= norm(deltas.col(count-1), 2);
}
// Compute cross products and dot products
for(int count = 2; count < num_readings; count++)
{
crossps(0, count-2) = deltas(0,count-2)*deltas(1, count-1)
- deltas(0, count-1)*deltas(1, count-2);
dotps(0, count-2) = dot(deltas.col(count-2), deltas.col(count-1));
}
// Bootstrap output
angle = angle_min;
for(int count = 2; count < num_readings; count++)
{
cout << angle << " " << crossps(0, count-2) << endl;
angle += angle_delta;
}
cout << endl;
break;
}
return 0;
}
