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; }