9 CPPUNIT_ASSERT( playerc_position2d_subscribe(
posProxy, PLAYER_OPEN_MODE ) == 0 );
12 for (
int i = 0; i < 10; i++ )
13 playerc_client_read(
client );
17 CPPUNIT_ASSERT( playerc_position2d_unsubscribe(
posProxy ) == 0 );
18 playerc_position2d_destroy(
posProxy );
23 CPPUNIT_ASSERT( playerc_position2d_get_geom(
posProxy ) == 0 );
26 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"geom pose (x)", -0.04,
posProxy->pose[0],
Delta );
27 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"geom pose (y)", 0,
posProxy->pose[1],
Delta );
28 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"geom pose (angle)", 0,
posProxy->pose[2],
Delta );
29 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"geom size (x)", 0.44,
posProxy->size[0],
Delta );
30 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"geom size (y)", 0.38,
posProxy->size[1],
Delta );
34 playerc_client_read(
client );
38 playerc_client_read(
client );
39 CPPUNIT_ASSERT_MESSAGE(
"laser updating",
posProxy->info.fresh == 1 );
41 CPPUNIT_ASSERT(
posProxy->info.datatime > 0 );
42 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (x)", 0,
posProxy->px,
Delta );
43 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (y)", 0,
posProxy->py,
Delta );
44 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (angle)", 0,
posProxy->pa,
Delta );
45 CPPUNIT_ASSERT(
posProxy->stall == 0 );
46 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"forward velocity", 0,
posProxy->vx,
Delta );
47 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"horizontal velocity", 0,
posProxy->vy,
Delta );
48 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"angular velocity", 0,
posProxy->va,
Delta );
53 CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel(
posProxy, -3, 0, 0, 1 ) == 0 );
56 playerc_client_read(
client );
57 CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel(
posProxy, 0, 0, 0, 1 ) == 0 );
62 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (y)", 0,
posProxy->py,
Delta );
63 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (angle)", 0,
posProxy->pa,
Delta );
65 CPPUNIT_ASSERT(
posProxy->stall == 1 );
67 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"forward velocity", -3,
posProxy->vx,
Delta );
68 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"horizontal velocity", 0,
posProxy->vy,
Delta );
69 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"angular velocity", 0,
posProxy->va,
Delta );
72 CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel(
posProxy, 0, 0, M_PI/2, 1 ) == 0 );
75 CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel(
posProxy, 0, 0, 0, 1 ) == 0 );
77 playerc_client_read(
client );
81 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (y)", 0,
posProxy->py,
Delta );
85 CPPUNIT_ASSERT_EQUAL( 0,
posProxy->stall );
87 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"forward velocity", 0,
posProxy->vx,
Delta );
88 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"horizontal velocity", 0,
posProxy->vy,
Delta );
89 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"angular velocity", 0,
posProxy->va,
Delta );
94 CPPUNIT_ASSERT( playerc_position2d_set_cmd_pose(
posProxy, 0, 0, 0, 1 ) == 0 );
96 CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel(
posProxy, 0, 0, 0, 1 ) == 0 );
98 playerc_client_read(
client );
101 CPPUNIT_ASSERT(
posProxy->px > oldx );
102 CPPUNIT_ASSERT(
posProxy->pa < olda );
103 CPPUNIT_ASSERT(
posProxy->stall == 0 );
105 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"forward velocity", 0,
posProxy->vx, 0.1 );
106 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"horizontal velocity", 0,
posProxy->vy, 0.1 );
107 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"angular velocity", 0,
posProxy->va, 0.1 );
110 playerc_client_read(
client );
111 CPPUNIT_ASSERT( playerc_position2d_set_odom(
posProxy, 0, 0, 0 ) == 0 );
113 playerc_client_read(
client );
114 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (x)", 0,
posProxy->px,
Delta );
115 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (y)", 0,
posProxy->py,
Delta );
116 CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE(
"pose (angle)", 0,
posProxy->pa,
Delta );
static const double Delta
playerc_client_t * client
playerc_position2d_t * posProxy