lsp_test_simulation.cc

Go to the documentation of this file.

#include "lsp_test_simulation.hh"

using namespace lspTest;

void Simulation::setUp() {
        connect();
        simProxy = playerc_simulation_create( client, 0 );
        CPPUNIT_ASSERT( playerc_simulation_subscribe( simProxy, PLAYER_OPEN_MODE ) == 0 );
}

void Simulation::tearDown() {
        CPPUNIT_ASSERT( playerc_simulation_unsubscribe( simProxy ) == 0 );
        playerc_simulation_destroy( simProxy );
        disconnect();
}

void Simulation::testPose2D() {
        double x, y, a;
        double x2, y2, a2;
        
        // See if the robot "r1" is where it should be according to lsp_test.world
        CPPUNIT_ASSERT( playerc_simulation_get_pose2d( simProxy, "r1", &x, &y, &a ) == 0 );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -4.19, x, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -5.71, y, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (a)", 3*M_PI/4, a, Delta );

        // Set pose to [ 0, 0, 0 ] and verify
        CPPUNIT_ASSERT( playerc_simulation_set_pose2d( simProxy, "r1", 0, 0, 0 ) == 0 );
        CPPUNIT_ASSERT( playerc_simulation_get_pose2d( simProxy, "r1", &x2, &y2, &a2 ) == 0 );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, x2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, y2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (a)", 0, a2, Delta );
        
        // Return the robot to its starting point
        CPPUNIT_ASSERT( playerc_simulation_set_pose2d( simProxy, "r1", x, y, a ) == 0 );
}

void Simulation::testPose3D() {
        double x, y, z, roll, pitch, yaw, time;
        double x2, y2, z2, roll2, pitch2, yaw2, time2;
        
        // See if the robot "r1" is where it should be according to lsp_test.world
        CPPUNIT_ASSERT( playerc_simulation_get_pose3d( simProxy, "r1", &x, &y, &z, &roll, &pitch, &yaw, &time ) == 0 );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", -4.19, x, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -5.71, y, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0, z, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (roll)", 0, roll, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (pitch)", 0, pitch, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (yaw)", 3*M_PI/4, yaw, Delta );
        CPPUNIT_ASSERT( time > 0 );
        
        // Set pose to [ 0, 0, 0.5, M_PI/4, M_PI/4, M_PI/4 ] and verify
        CPPUNIT_ASSERT( playerc_simulation_set_pose3d( simProxy, "r1", 0, 0, 0.5, M_PI/4, M_PI/4, M_PI/4 ) == 0 );
        CPPUNIT_ASSERT( playerc_simulation_get_pose3d( simProxy, "r1", &x2, &y2, &z2, &roll2, &pitch2, &yaw2, &time2 ) == 0 );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, x2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, y2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (z)", 0.5, z2, Delta );
        // roll and pitch are currently unused in stage ( returns set to 0 )
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (roll)", 0, roll2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (pitch)", 0, pitch2, Delta );
        CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (yaw)", M_PI/4, yaw2, Delta );
        CPPUNIT_ASSERT( time2 > time );
        
        // Return the robot to its starting point
        CPPUNIT_ASSERT( playerc_simulation_set_pose3d( simProxy, "r1", x, y, z, roll, pitch, yaw ) == 0 );      
}

void Simulation::testProperties() {     
        int r0Agg = 5;
        int r1Agg = 1;
        int r1Pow = 125;
        
        // Set some properties
        CPPUNIT_ASSERT( playerc_simulation_set_property( simProxy, "r0", "aggression", &r0Agg, sizeof(void*) ) == 0 );
        CPPUNIT_ASSERT( playerc_simulation_set_property( simProxy, "r1", "aggression", &r1Agg, sizeof(void*) ) == 0 );
        CPPUNIT_ASSERT( playerc_simulation_set_property( simProxy, "r1", "power", &r1Pow, sizeof(void*) ) == 0 );

        // Get the properties back
//      int r0Agg2, r1Agg2, r1Pow2;
//      CPPUNIT_ASSERT( playerc_simulation_get_property( simProxy, "r0", "aggression", &r0Agg2, sizeof(void*) ) == 0 );
//      CPPUNIT_ASSERT( playerc_simulation_get_property( simProxy, "r1", "aggression", &r1Agg2, sizeof(void*) ) == 0 );
//      CPPUNIT_ASSERT( playerc_simulation_get_property( simProxy, "r1", "power", &r1Pow2, sizeof(void*) ) == 0 );
        
        // Make sure they're the same
//      CPPUNIT_ASSERT_EQUAL_MESSAGE( "r0Agg", r0Agg, r0Agg2 );
//      CPPUNIT_ASSERT_EQUAL_MESSAGE( "r1Agg", r1Agg, r1Agg2 );
//      CPPUNIT_ASSERT_EQUAL_MESSAGE( "r1Pow", r1Pow, r1Pow2 );
}