Revision: 6917
http://playerstage.svn.sourceforge.net/playerstage/?rev=6917&view=rev
Author: jeremy_asher
Date: 2008-07-23 21:50:03 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
libstageplugin: added sonar test
Modified Paths:
--------------
code/stage/trunk/libstageplugin/test/CMakeLists.txt
code/stage/trunk/libstageplugin/test/lsp_test_laser.cc
code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
code/stage/trunk/worlds/lsp_test.cfg
Added Paths:
-----------
code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc
code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh
Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-23 21:03:09 UTC
(rev 6916)
+++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-23 21:50:03 UTC
(rev 6917)
@@ -19,6 +19,8 @@
lsp_test_laser.hh
lsp_test_speech.cc
lsp_test_speech.hh
+ lsp_test_sonar.cc
+ lsp_test_sonar.hh
lsp_test_position2d.cc
lsp_test_position2d.hh
)
Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-23
21:03:09 UTC (rev 6916)
+++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-23
21:50:03 UTC (rev 6917)
@@ -42,7 +42,7 @@
void Laser::testGeom() {
CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 );
- // values for lsp_test.cfg/world (sicklaser)
+ // values from lsp_test.cfg/world and sick.inc
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0,
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 );
Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23
21:03:09 UTC (rev 6916)
+++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23
21:50:03 UTC (rev 6917)
@@ -22,7 +22,7 @@
void Position2D::testGeom() {
CPPUNIT_ASSERT( playerc_position2d_get_geom( posProxy ) == 0 );
- // values for lsp_test.cfg/world (pioneer2dx)
+ // values from lsp_test.cfg/world (pioneer2dx)
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (x)", -0.04,
posProxy->pose[0], Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (y)", 0,
posProxy->pose[1], Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom pose (angle)", 0,
posProxy->pose[2], Delta );
@@ -50,9 +50,9 @@
void Position2D::testMove() {
// Back the robot into the wall
- CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, -0.5, 0, 0, 1
) == 0 );
- // for 2 seconds, read and stop
- sleep( 2 );
+ CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, -3, 0, 0, 1 )
== 0 );
+ // for 500ms, read and stop
+ usleep( 500000 );
playerc_client_read( client );
CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 )
== 0 );
@@ -64,16 +64,16 @@
// Make sure it's hitting the wall
CPPUNIT_ASSERT( posProxy->stall == 1 );
// Make sure the velocity is as specified
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", -0.5,
posProxy->vx, Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", -3,
posProxy->vx, Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0,
posProxy->vy, Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0,
posProxy->va, Delta );
// Start turning left at 45 deg. / sec
- CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, M_PI/4,
1 ) == 0 );
- // for 1 second, stop, and read
- sleep( 1 );
+ CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, M_PI/2,
1 ) == 0 );
+ // for 500ms, stop, and read
+ usleep( 500000 );
CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 )
== 0 );
- sleep( 1 );
+ usleep( 500000 );
playerc_client_read( client );
// Make sure odom still reads negative
@@ -94,7 +94,7 @@
CPPUNIT_ASSERT( playerc_position2d_set_cmd_pose( posProxy, 0, 0, 0, 1 )
== 0 );
sleep( 5 );
CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 )
== 0 );
- sleep( 1 );
+ usleep( 500000 );
playerc_client_read( client );
// Make sure we've made progress
@@ -109,7 +109,7 @@
// Reset odometer and check position
playerc_client_read( client );
CPPUNIT_ASSERT( playerc_position2d_set_odom( posProxy, 0, 0, 0 ) == 0 );
- sleep( 1 );
+ usleep( 500000 );
playerc_client_read( client );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px,
Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py,
Delta );
Added: code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc
(rev 0)
+++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.cc 2008-07-23
21:50:03 UTC (rev 6917)
@@ -0,0 +1,47 @@
+#include "lsp_test_sonar.hh"
+
+using namespace lspTest;
+
+const int Sonar::Samples = 361;
+
+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]);
+ }
+}
\ No newline at end of file
Added: code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh
(rev 0)
+++ code/stage/trunk/libstageplugin/test/lsp_test_sonar.hh 2008-07-23
21:50:03 UTC (rev 6917)
@@ -0,0 +1,33 @@
+#ifndef _LSP_SONAR_TEST_H_
+#define _LSP_SONAR_TEST_H_
+
+#include <cppunit/extensions/HelperMacros.h>
+#include <libplayerc/playerc.h>
+
+#include "lsp_test_proxy.hh"
+
+namespace lspTest {
+ class Sonar : public Proxy
+ {
+ CPPUNIT_TEST_SUITE( Sonar );
+ CPPUNIT_TEST( testGeom );
+ CPPUNIT_TEST( testData );
+ CPPUNIT_TEST_SUITE_END();
+
+ protected:
+ playerc_sonar_t* sonarProxy;
+
+ void testConfig();
+ void testGeom();
+ void testData();
+
+ static const int Samples;
+ public:
+ void setUp();
+ void tearDown();
+ };
+};
+
+CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Sonar );
+
+#endif
\ No newline at end of file
Modified: code/stage/trunk/worlds/lsp_test.cfg
===================================================================
--- code/stage/trunk/worlds/lsp_test.cfg 2008-07-23 21:03:09 UTC (rev
6916)
+++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-23 21:50:03 UTC (rev
6917)
@@ -16,7 +16,7 @@
driver
(
name "stage"
- provides [ "position2d:0" "laser:0" "speech:0" ]
+ provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" ]
model "r0"
)
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
