lsp_test_sonar.cc

Go to the documentation of this file.

#include "lsp_test_sonar.hh"

using namespace lspTest;

void Sonar::setUp() {
        connect();
        sonarProxy = playerc_sonar_create( client, 0 );
        CPPUNIT_ASSERT( playerc_sonar_subscribe( sonarProxy, PLAYER_OPEN_MODE ) == 0 );
}


void Sonar::tearDown() {
        CPPUNIT_ASSERT( playerc_sonar_unsubscribe( sonarProxy ) == 0 );
        playerc_sonar_destroy( sonarProxy );
        disconnect();
}

void Sonar::testGeom() {
        CPPUNIT_ASSERT( playerc_sonar_get_geom( sonarProxy ) == 0 );
        
        // values from pioneer.inc
        CPPUNIT_ASSERT( sonarProxy->pose_count == 16 );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (x)", 0.075, sonarProxy->poses[0].px, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (y)", 0.130, sonarProxy->poses[0].py, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (z)", 0, sonarProxy->poses[0].pz, Delta ); // currently unused
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose[0] (angle)", M_PI/2, sonarProxy->poses[0].pyaw, Delta );
}

void Sonar::testData() {
        playerc_client_read( client );  
        
        // verify that we're getting new data
        sonarProxy->info.fresh = 0;
        playerc_client_read( client );
        CPPUNIT_ASSERT_MESSAGE( "sonar updating", sonarProxy->info.fresh == 1 );
        
        CPPUNIT_ASSERT( sonarProxy->info.datatime > 0 );
        CPPUNIT_ASSERT( sonarProxy->scan_count == 16 ); // from pioneer.inc, actually sensor_count

        // sanity check
        for ( int i = 0; i < sonarProxy->scan_count; i++ ) {
                CPPUNIT_ASSERT( sonarProxy->scan[i] >= 0 );             
//              printf(" range[%d]: %6.3f ]\n", i, sonarProxy->scan[i]);
        }
}