Revision: 6927
http://playerstage.svn.sourceforge.net/playerstage/?rev=6927&view=rev
Author: jeremy_asher
Date: 2008-07-25 21:27:59 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
libstageplugin: fixed blobfinder returning pixels in reverse x order,
implemented blobfinder test, added readme for tests
Modified Paths:
--------------
code/stage/trunk/libstage/model_blobfinder.cc
code/stage/trunk/libstageplugin/p_blobfinder.cc
code/stage/trunk/libstageplugin/test/CMakeLists.txt
code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh
code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc
code/stage/trunk/worlds/lsp_test.cfg
code/stage/trunk/worlds/lsp_test.world
Added Paths:
-----------
code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc
code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh
code/stage/trunk/libstageplugin/test/readme.txt
Modified: code/stage/trunk/libstage/model_blobfinder.cc
===================================================================
--- code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 17:35:38 UTC
(rev 6926)
+++ code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 21:27:59 UTC
(rev 6927)
@@ -197,7 +197,7 @@
//if( samples[s].block->Color() != 0xFFFF0000 )
//continue; // we saw nothing
- unsigned int blobleft = s;
+ unsigned int right = s;
stg_color_t blobcol = samples[s].block->Color();
//printf( "blob start %d color %X\n", blobleft, blobcol );
@@ -212,7 +212,7 @@
s++;
}
- unsigned int blobright = s-1;
+ unsigned int left = s - 1;
//if we have color filters in place, check to see if we're
looking for this color
if( colors->len )
@@ -235,25 +235,25 @@
// find the average range to the blob;
stg_meters_t range = 0;
- for( unsigned int t=blobleft; t<=blobright; t++ )
+ for( unsigned int t=right; t<=left; t++ )
range += samples[t].range;
- range /= blobright-blobleft + 1;
+ range /= left-right + 1;
double startyangle = atan2( robotHeight/2.0, range );
double endyangle = -startyangle;
int blobtop = scan_height/2 - (int)(startyangle/yRadsPerPixel);
int blobbottom = scan_height/2 -(int)(endyangle/yRadsPerPixel);
- blobtop = MIN( blobtop, (int)scan_height );
+ blobtop = MAX( blobtop, 0 );
blobbottom = MIN( blobbottom, (int)scan_height );
// fill in an array entry for this blob
stg_blobfinder_blob_t blob;
memset( &blob, 0, sizeof(blob) );
blob.color = blobcol;
- blob.left = blobleft;
+ blob.left = scan_width - left - 1;
blob.top = blobtop;
- blob.right = blobright;
+ blob.right = scan_width - right - 1;;
blob.bottom = blobbottom;
blob.range = range;
@@ -346,6 +346,7 @@
PushColor( 0xFF000000 );
glRectf( 0,0, scan_width, scan_height );
PopColor();
+ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
// draw the blobs on the screen
for( unsigned int s=0; s<blobs->len; s++ )
@@ -360,7 +361,6 @@
PopColor();
}
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
glPopMatrix();
}
Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-25 17:35:38 UTC
(rev 6926)
+++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-25 21:27:59 UTC
(rev 6927)
@@ -73,20 +73,19 @@
for( b=0; b<bcount; b++ )
{
// useful debug - leave in
- /*
+
cout << "blob "
- << " channel: " << (int)blobs[b].channel
- << " area: " << blobs[b].area
<< " left: " << blobs[b].left
<< " right: " << blobs[b].right
<< " top: " << blobs[b].top
<< " bottom: " << blobs[b].bottom
+ << " color: " << hex << blobs[b].color << dec
<< endl;
- */
+
int dx = blobs[b].right - blobs[b].left;
int dy = blobs[b].top - blobs[b].bottom;
-
+
bfd.blobs[b].x = blobs[b].left + dx/2;
bfd.blobs[b].y = blobs[b].bottom + dy/2;
Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-25 17:35:38 UTC
(rev 6926)
+++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-25 21:27:59 UTC
(rev 6927)
@@ -15,6 +15,8 @@
lsp_test.cc
lsp_test_proxy.cc
lsp_test_proxy.hh
+ lsp_test_blobfinder.cc
+ lsp_test_blobfinder.hh
lsp_test_simulation.cc
lsp_test_simulation.hh
lsp_test_speech.cc
Added: code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc
(rev 0)
+++ code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.cc 2008-07-25
21:27:59 UTC (rev 6927)
@@ -0,0 +1,59 @@
+#include "lsp_test_blobfinder.hh"
+
+using namespace lspTest;
+
+void Blobfinder::setUp() {
+ connect();
+ blobProxy = playerc_blobfinder_create( client, 0 );
+ CPPUNIT_ASSERT( playerc_blobfinder_subscribe( blobProxy,
PLAYER_OPEN_MODE ) == 0 );
+}
+
+
+void Blobfinder::tearDown() {
+ CPPUNIT_ASSERT( playerc_blobfinder_unsubscribe( blobProxy ) == 0 );
+ playerc_blobfinder_destroy( blobProxy );
+ disconnect();
+}
+
+void Blobfinder::testData() {
+ playerc_client_read( client );
+
+ // verify that we're getting new data
+ blobProxy->info.fresh = 0;
+ playerc_client_read( client );
+ CPPUNIT_ASSERT_MESSAGE( "blobfinder updating", blobProxy->info.fresh ==
1 );
+
+ CPPUNIT_ASSERT( blobProxy->info.datatime > 0 );
+
+ //printf("\ncolor0: 0x%X, color1: 0x%X\n", blobProxy->blobs[0].color,
blobProxy->blobs[1].color);
+
+ // check stage defaults
+ CPPUNIT_ASSERT( blobProxy->width == 80 );
+ CPPUNIT_ASSERT( blobProxy->height == 60 );
+
+ // Make sure we see three blobs: wall, robot, wall
+ CPPUNIT_ASSERT_EQUAL_MESSAGE( "blobs_count", (unsigned int)3,
blobProxy->blobs_count ); // lsp_test.world
+ // sanity checks
+ CPPUNIT_ASSERT( blobProxy->blobs[1].range > 0 );
+ CPPUNIT_ASSERT( blobProxy->blobs[1].area > 0 );
+ CPPUNIT_ASSERT( blobProxy->blobs[1].y > 0 );
+ CPPUNIT_ASSERT( blobProxy->blobs[1].left < blobProxy->blobs[1].right );
+ CPPUNIT_ASSERT( blobProxy->blobs[1].top < blobProxy->blobs[1].bottom );
+
+ // robot should be red
+ CPPUNIT_ASSERT( blobProxy->blobs[1].color == 0xFFFF0000 );
+
+ // robot should be closer
+ CPPUNIT_ASSERT( blobProxy->blobs[0].range > blobProxy->blobs[1].range );
+
+ // robot should be towards the right
+ uint32_t robotCenter = blobProxy->blobs[1].x;
+ uint32_t imageCenter = blobProxy->width / 2;
+ //printf(" robotCenter: %d, imageCenter: %d\n", robotCenter,
imageCenter);
+ CPPUNIT_ASSERT( robotCenter > imageCenter );
+
+
+
+// CPPUNIT_ASSERT_EQUAL_MESSAGE( "id", (unsigned int)2,
blobProxy->blobs[0].id );
+
+}
\ No newline at end of file
Added: code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh
(rev 0)
+++ code/stage/trunk/libstageplugin/test/lsp_test_blobfinder.hh 2008-07-25
21:27:59 UTC (rev 6927)
@@ -0,0 +1,31 @@
+#ifndef _LSP_BLOBFINDER_TEST_H_
+#define _LSP_BLOBFINDER_TEST_H_
+
+#include <cppunit/extensions/HelperMacros.h>
+#include <libplayerc/playerc.h>
+
+#include "lsp_test_proxy.hh"
+
+namespace lspTest {
+ class Blobfinder : public Proxy
+ {
+ CPPUNIT_TEST_SUITE( Blobfinder );
+ CPPUNIT_TEST( testData );
+ CPPUNIT_TEST_SUITE_END();
+
+ protected:
+ playerc_blobfinder_t* blobProxy;
+
+ void testConfig();
+ void testGeom();
+ void testData();
+
+ public:
+ void setUp();
+ void tearDown();
+ };
+};
+
+CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Blobfinder );
+
+#endif
\ No newline at end of file
Modified: code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-25
17:35:38 UTC (rev 6926)
+++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-25
21:27:59 UTC (rev 6927)
@@ -12,7 +12,7 @@
CPPUNIT_TEST_SUB_SUITE( Position2D, Proxy );
CPPUNIT_TEST( testGeom );
CPPUNIT_TEST( testData );
-// CPPUNIT_TEST( testMove );
+ CPPUNIT_TEST( testMove );
CPPUNIT_TEST_SUITE_END();
protected:
Modified: code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc 2008-07-25
17:35:38 UTC (rev 6926)
+++ code/stage/trunk/libstageplugin/test/lsp_test_simulation.cc 2008-07-25
21:27:59 UTC (rev 6927)
@@ -20,8 +20,8 @@
// 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)", -5.085, x, Delta );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -7.193, y, Delta );
+ 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
@@ -41,8 +41,8 @@
// 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)", -5.085, x, Delta );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", -7.193, y, Delta );
+ 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 );
Added: code/stage/trunk/libstageplugin/test/readme.txt
===================================================================
--- code/stage/trunk/libstageplugin/test/readme.txt
(rev 0)
+++ code/stage/trunk/libstageplugin/test/readme.txt 2008-07-25 21:27:59 UTC
(rev 6927)
@@ -0,0 +1,9 @@
+Building
+--------
+CPPUNIT is required. Build by enabling the BUILD_LSPTEST option either with a
cmake command line arg or through ccmake on the root of the stage source.
+
+Execution
+---------
+Run player on lsp_test.cfg, included in the worlds section of the stage source.
+
+Execute lsp_test on the same machine and the test suites will connect to
localhost:6665 to perform testing.
\ No newline at end of file
Modified: code/stage/trunk/worlds/lsp_test.cfg
===================================================================
--- code/stage/trunk/worlds/lsp_test.cfg 2008-07-25 17:35:38 UTC (rev
6926)
+++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-25 21:27:59 UTC (rev
6927)
@@ -16,7 +16,7 @@
driver
(
name "stage"
- provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" "fiducial:0" ]
+ provides [ "position2d:0" "speech:0" "laser:0" "sonar:0" "fiducial:0"
"blobfinder:0" ]
model "r0"
)
Modified: code/stage/trunk/worlds/lsp_test.world
===================================================================
--- code/stage/trunk/worlds/lsp_test.world 2008-07-25 17:35:38 UTC (rev
6926)
+++ code/stage/trunk/worlds/lsp_test.world 2008-07-25 21:27:59 UTC (rev
6927)
@@ -32,11 +32,11 @@
(
ranger()
- sicklaser( pose [ 0.03 0 0 0 ] )
+ sicklaser( pose [ 0.030 0 0 0 ] )
fiducial
(
- pose [ -0.15 0 0 0 ]
+ pose [ -0.150 0 0 0 ]
range_max 8
range_max_id 5
)
@@ -56,6 +56,7 @@
name "r0"
fiducial_return 1
+ blobfinder_return 1
pose [ -7.490 -7.490 0 45.000 ]
)
@@ -64,5 +65,6 @@
name "r1"
fiducial_return 2
- pose [ -5.085 -7.193 0 135 ]
+ blobfinder_return 2
+ pose [ -4.19 -5.71 0 135.000 ]
)
\ No newline at end of file
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
