00001 #include "lsp_test_laser.hh"
00002
00003 using namespace lspTest;
00004
00005 const int Laser::Samples = 361;
00006
00007 void Laser::setUp() {
00008 connect();
00009 laserProxy = playerc_laser_create( client, 0 );
00010 CPPUNIT_ASSERT( playerc_laser_subscribe( laserProxy, PLAYER_OPEN_MODE ) == 0 );
00011 }
00012
00013
00014 void Laser::tearDown() {
00015 CPPUNIT_ASSERT( playerc_laser_unsubscribe( laserProxy ) == 0 );
00016 playerc_laser_destroy( laserProxy );
00017 disconnect();
00018 }
00019
00020 void Laser::testConfig() {
00021 double min = -M_PI/2;
00022 double max = +M_PI/2;
00023 double res = M_PI/Samples;
00024 double range_res = 1;
00025 unsigned char intensity = 1;
00026 double freq = 10;
00027
00028 CPPUNIT_ASSERT( playerc_laser_set_config( laserProxy, min, max, res, range_res, intensity, freq ) == 0 );
00029
00030 double min2, max2, res2, range_res2, freq2;
00031 unsigned char intensity2 = 1;
00032 CPPUNIT_ASSERT( playerc_laser_get_config( laserProxy, &min2, &max2, &res2, &range_res2, &intensity2, &freq2 ) == 0 );
00033
00034 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", min, min2, Delta );
00035 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", max, max2, Delta );
00036 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular resolution", res, res2, Delta );
00037 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "range resolution", range_res, range_res2, Delta );
00038 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "scan freq", freq, freq2, Delta );
00039 CPPUNIT_ASSERT_EQUAL_MESSAGE( "intensity", intensity, intensity2 );
00040 }
00041
00042 void Laser::testGeom() {
00043 CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 );
00044
00045
00046 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0.03, laserProxy->pose[0], Delta );
00047 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, laserProxy->pose[1], Delta );
00048 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, laserProxy->pose[2], Delta );
00049 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (x)", 0.156, laserProxy->size[0], Delta );
00050 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (y)", 0.155, laserProxy->size[1], Delta );
00051 }
00052
00053 void Laser::testData() {
00054 playerc_client_read( client );
00055
00056
00057 laserProxy->info.fresh = 0;
00058 playerc_client_read( client );
00059 CPPUNIT_ASSERT_MESSAGE( "laser updating", laserProxy->info.fresh == 1 );
00060
00061 CPPUNIT_ASSERT( laserProxy->info.datatime > 0 );
00062 CPPUNIT_ASSERT( laserProxy->scan_count == Samples );
00063 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", -M_PI/2, laserProxy->scan[0][1], Delta );
00064 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", M_PI/2, laserProxy->scan[Samples-1][1], Delta );
00065
00066 for ( int i = 0; i < laserProxy->scan_count; i++ ) {
00067 double distance = laserProxy->scan[i][0];
00068
00069
00070 CPPUNIT_ASSERT( distance <= laserProxy->max_range );
00071 CPPUNIT_ASSERT( distance >= laserProxy->min_right );
00072
00073
00074 }
00075 }