lsp_test_laser.cc

Go to the documentation of this file.

#include "lsp_test_laser.hh"

using namespace lspTest;

const int Laser::Samples = 361;

void Laser::setUp() {
        connect();
        laserProxy = playerc_laser_create( client, 0 );
        CPPUNIT_ASSERT( playerc_laser_subscribe( laserProxy, PLAYER_OPEN_MODE ) == 0 );
}


void Laser::tearDown() {
        CPPUNIT_ASSERT( playerc_laser_unsubscribe( laserProxy ) == 0 );
        playerc_laser_destroy( laserProxy );
        disconnect();
}

void Laser::testConfig() {
        double min = -M_PI/2;
        double max = +M_PI/2;
        double res = M_PI/Samples; // sick laser default
        double range_res = 1;  // not being used by stage 
        unsigned char intensity = 1; // not being used by stage
        double freq = 10; // 10Hz / 100ms (stage default)
        
        CPPUNIT_ASSERT( playerc_laser_set_config( laserProxy, min, max, res, range_res, intensity, freq ) == 0 );
        
        double min2, max2, res2, range_res2, freq2;
        unsigned char intensity2 = 1;
        CPPUNIT_ASSERT( playerc_laser_get_config( laserProxy, &min2, &max2, &res2, &range_res2, &intensity2, &freq2 ) == 0 );

        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", min, min2, Delta );     
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", max, max2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular resolution", res, res2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "range resolution", range_res, range_res2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "scan freq", freq, freq2, Delta );
        CPPUNIT_ASSERT_EQUAL_MESSAGE( "intensity", intensity, intensity2 );
}

void Laser::testGeom() {
        CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 );
        
        // values from lsp_test.world and sick.inc
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0.03, laserProxy->pose[0], Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, laserProxy->pose[1], Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, laserProxy->pose[2], Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (x)", 0.156, laserProxy->size[0], Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (y)", 0.155, laserProxy->size[1], Delta );
}

void Laser::testData() {
        playerc_client_read( client );  
        
        // verify that we're getting new data
        laserProxy->info.fresh = 0;
        playerc_client_read( client );
        CPPUNIT_ASSERT_MESSAGE( "laser updating", laserProxy->info.fresh == 1 );
        
        CPPUNIT_ASSERT( laserProxy->info.datatime > 0 );
        CPPUNIT_ASSERT( laserProxy->scan_count == Samples );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", -M_PI/2, laserProxy->scan[0][1], Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", M_PI/2, laserProxy->scan[Samples-1][1], Delta );

        for ( int i = 0; i < laserProxy->scan_count; i++ ) {
                double distance = laserProxy->scan[i][0];

                // check range of each sample is within max and min
                CPPUNIT_ASSERT( distance <= laserProxy->max_range );
                CPPUNIT_ASSERT( distance >= laserProxy->min_right );

                //        printf("[%6.3f, %6.3f ] \n", laserProxy->scan[i][0], laserProxy->scan[i][1]);
        }
}