Revision: 6904
http://playerstage.svn.sourceforge.net/playerstage/?rev=6904&view=rev
Author: jeremy_asher
Date: 2008-07-23 00:03:34 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
libstageplugin: p_position updates, added position2d test
Modified Paths:
--------------
code/stage/trunk/libstageplugin/p_laser.cc
code/stage/trunk/libstageplugin/p_position.cc
code/stage/trunk/libstageplugin/test/CMakeLists.txt
code/stage/trunk/libstageplugin/test/lsp_test_laser.cc
code/stage/trunk/libstageplugin/test/lsp_test_laser.hh
code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc
code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh
code/stage/trunk/libstageplugin/test/lsp_test_speech.cc
code/stage/trunk/worlds/lsp_test.world
Added Paths:
-----------
code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh
Modified: code/stage/trunk/libstageplugin/p_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_laser.cc 2008-07-22 20:23:21 UTC (rev
6903)
+++ code/stage/trunk/libstageplugin/p_laser.cc 2008-07-23 00:03:34 UTC (rev
6904)
@@ -110,7 +110,7 @@
stg_laser_cfg_t cfg = mod->GetConfig();
- printf( "laser config was: resolution %d, fov %.6f, interval %d\n",
+ PRINT_DEBUG3( "laser config was: resolution %d, fov %.6f, interval
%d\n",
cfg.resolution, cfg.fov, cfg.interval );
cfg.fov = plc->max_angle - plc->min_angle;
@@ -119,8 +119,7 @@
cfg.resolution = 1;
cfg.interval = 1.0E6 / plc->scanning_frequency;
- //PRINT_DEBUG3
- printf( "setting laser config: resolution %d, fov %.6f, interval
%d\n",
+ PRINT_DEBUG3( "setting laser config: resolution %d, fov %.6f,
interval %d\n",
cfg.resolution, cfg.fov, cfg.interval );
// Range resolution is currently locked to the world setting
Modified: code/stage/trunk/libstageplugin/p_position.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_position.cc 2008-07-22 20:23:21 UTC
(rev 6903)
+++ code/stage/trunk/libstageplugin/p_position.cc 2008-07-23 00:03:34 UTC
(rev 6904)
@@ -86,7 +86,7 @@
// convert from Player to Stage format
player_position2d_cmd_pos_t* pcmd = (player_position2d_cmd_pos_t*)data;
- mod->GoTo( pcmd->vel.px, pcmd->vel.py, pcmd->vel.pa );
+ mod->GoTo( pcmd->pos.px, pcmd->pos.py, pcmd->pos.pa );
return 0;
}
@@ -140,12 +140,12 @@
{
if(hdr->size == 0)
{
- PRINT_WARN( "reset odom not implemented" );
PRINT_DEBUG( "resetting odometry" );
- stg_pose_t origin;
- memset(&origin,0,sizeof(origin));
- //stg_model_position_set_odom( this->mod, &origin );
+ mod->est_pose.x = 0;
+ mod->est_pose.y = 0;
+ mod->est_pose.z = 0;
+ mod->est_pose.a = 0;
this->driver->Publish( this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
@@ -166,18 +166,15 @@
{
if(hdr->size == sizeof(player_position2d_set_odom_req_t))
{
- PRINT_WARN( "set odom not implemented" );
-
player_position2d_set_odom_req_t* req =
(player_position2d_set_odom_req_t*)data;
- stg_pose_t pose;
- pose.x = req->pose.px;
- pose.y = req->pose.py;
- pose.a = req->pose.pa;
+
+ mod->est_pose.x = req->pose.px;
+ mod->est_pose.y = req->pose.py;
+ //mod->est_pose.z = req->pose.pz;
+ mod->est_pose.a = req->pose.pa;
- //stg_model_position_set_odom( this->mod, &pose );
-
PRINT_DEBUG3( "set odometry to (%.2f,%.2f,%.2f)",
pose.x,
pose.y,
Modified: code/stage/trunk/libstageplugin/test/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-22 20:23:21 UTC
(rev 6903)
+++ code/stage/trunk/libstageplugin/test/CMakeLists.txt 2008-07-23 00:03:34 UTC
(rev 6904)
@@ -19,6 +19,8 @@
lsp_test_laser.hh
lsp_test_speech.cc
lsp_test_speech.hh
+ lsp_test_position2d.cc
+ lsp_test_position2d.hh
)
add_executable( lsp_test ${lspTestSrcs} )
Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-22
20:23:21 UTC (rev 6903)
+++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-23
00:03:34 UTC (rev 6904)
@@ -2,7 +2,6 @@
using namespace lspTest;
-const double Laser::Delta = 0.01;
const int Laser::Samples = 361;
void Laser::setUp() {
@@ -46,7 +45,7 @@
// values for lsp_test.cfg/world (sicklaser)
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 (z)", 0,
laserProxy->pose[2], Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0,
laserProxy->pose[2], Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (x)", 0.156,
laserProxy->size[0], Delta );
CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (y)", 0.155,
laserProxy->size[1], Delta );
}
Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-22
20:23:21 UTC (rev 6903)
+++ code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-23
00:03:34 UTC (rev 6904)
@@ -22,7 +22,6 @@
void testGeom();
void testData();
- static const double Delta;
static const int Samples;
public:
void setUp();
Added: code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc
(rev 0)
+++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.cc 2008-07-23
00:03:34 UTC (rev 6904)
@@ -0,0 +1,114 @@
+#include "lsp_test_position2d.hh"
+
+
+using namespace lspTest;
+
+void Position2D::setUp() {
+ connect();
+ posProxy = playerc_position2d_create( client, 0 );
+ CPPUNIT_ASSERT( playerc_position2d_subscribe( posProxy,
PLAYER_OPEN_MODE ) == 0 );
+
+ // ignore initial messages on proxy subscription
+ for ( int i = 0; i < 10; i++ )
+ playerc_client_read( client );
+}
+
+void Position2D::tearDown() {
+ CPPUNIT_ASSERT( playerc_position2d_unsubscribe( posProxy ) == 0 );
+ playerc_position2d_destroy( posProxy );
+ disconnect();
+}
+
+void Position2D::testGeom() {
+ CPPUNIT_ASSERT( playerc_position2d_get_geom( posProxy ) == 0 );
+
+ // values for 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 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom size (x)", 0.44,
posProxy->size[0], Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "geom size (y)", 0.38,
posProxy->size[1], Delta );
+}
+
+void Position2D::testData() {
+ playerc_client_read( client );
+
+ // verify that we're getting new data
+ posProxy->info.fresh = 0;
+ playerc_client_read( client );
+ CPPUNIT_ASSERT_MESSAGE( "laser updating", posProxy->info.fresh == 1 );
+
+ CPPUNIT_ASSERT( posProxy->info.datatime > 0 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa,
Delta );
+ CPPUNIT_ASSERT( posProxy->stall == 0 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", 0,
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 );
+}
+
+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 );
+ playerc_client_read( client );
+ CPPUNIT_ASSERT( playerc_position2d_set_cmd_vel( posProxy, 0, 0, 0, 1 )
== 0 );
+
+ // Make sure it moved back
+ CPPUNIT_ASSERT( posProxy->px < 0 );
+ // Make sure it didn't turn or move sideways
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa,
Delta );
+ // 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( "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, 0, 1 )
== 0 );
+ sleep( 1 );
+ playerc_client_read( client );
+
+ // Make sure odom still reads negative
+ CPPUNIT_ASSERT( posProxy->px < 0 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py,
Delta );
+ // See if we turned in the right direction
+ CPPUNIT_ASSERT( posProxy > 0 );
+ // Shouldn't be stalled
+ CPPUNIT_ASSERT_EQUAL( 0, posProxy->stall );
+ // Should have stopped
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", 0,
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 );
+
+ // Save position and try position based movement toward start position
+ double oldx = posProxy->px;
+ double olda = posProxy->pa;
+ 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 );
+ playerc_client_read( client );
+
+ // Make sure we've made progress
+ CPPUNIT_ASSERT( posProxy->px > oldx );
+ CPPUNIT_ASSERT( posProxy->pa < olda );
+ CPPUNIT_ASSERT( posProxy->stall == 0 );
+ // Verify that we've stopped
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "forward velocity", 0,
posProxy->vx, 0.1 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "horizontal velocity", 0,
posProxy->vy, 0.1 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular velocity", 0,
posProxy->va, 0.1 );
+
+ // Reset odometer and check position
+ CPPUNIT_ASSERT( playerc_position2d_set_odom( posProxy, 0, 0, 0 ) == 0 );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (x)", 0, posProxy->px,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (y)", 0, posProxy->py,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "pose (angle)", 0, posProxy->pa,
Delta );
+}
\ No newline at end of file
Added: code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh
(rev 0)
+++ code/stage/trunk/libstageplugin/test/lsp_test_position2d.hh 2008-07-23
00:03:34 UTC (rev 6904)
@@ -0,0 +1,33 @@
+#ifndef _LSP_POSITION2D_TEST_H_
+#define _LSP_POSITION2D_TEST_H_
+
+#include <cppunit/extensions/HelperMacros.h>
+#include <libplayerc/playerc.h>
+
+#include "lsp_test_proxy.hh"
+
+namespace lspTest {
+ class Position2D : public Proxy
+ {
+ CPPUNIT_TEST_SUB_SUITE( Position2D, Proxy );
+ CPPUNIT_TEST( testGeom );
+ CPPUNIT_TEST( testData );
+ CPPUNIT_TEST( testMove );
+ CPPUNIT_TEST_SUITE_END();
+
+ protected:
+ playerc_position2d_t* posProxy;
+
+ void testGeom();
+ void testData();
+ void testMove();
+
+ public:
+ void setUp();
+ void tearDown();
+ };
+};
+
+CPPUNIT_TEST_SUITE_REGISTRATION( lspTest::Position2D );
+
+#endif
\ No newline at end of file
Modified: code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc 2008-07-22
20:23:21 UTC (rev 6903)
+++ code/stage/trunk/libstageplugin/test/lsp_test_proxy.cc 2008-07-23
00:03:34 UTC (rev 6904)
@@ -1,12 +1,14 @@
#include "lsp_test_proxy.hh"
-#include <stdio.h>
-
using namespace lspTest;
+const double Proxy::Delta = 0.01;
+
void Proxy::connect() {
client = playerc_client_create( NULL, "localhost", 6665 );
CPPUNIT_ASSERT( playerc_client_connect( client ) == 0 );
+ CPPUNIT_ASSERT( playerc_client_datamode (client, PLAYERC_DATAMODE_PULL)
== 0 );
+ CPPUNIT_ASSERT( playerc_client_set_replace_rule (client, -1, -1,
PLAYER_MSGTYPE_DATA, -1, 1) == 0 );
}
void Proxy::disconnect() {
Modified: code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh 2008-07-22
20:23:21 UTC (rev 6903)
+++ code/stage/trunk/libstageplugin/test/lsp_test_proxy.hh 2008-07-23
00:03:34 UTC (rev 6904)
@@ -15,6 +15,8 @@
void connect();
void disconnect();
+
+ static const double Delta;
};
};
Modified: code/stage/trunk/libstageplugin/test/lsp_test_speech.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_speech.cc 2008-07-22
20:23:21 UTC (rev 6903)
+++ code/stage/trunk/libstageplugin/test/lsp_test_speech.cc 2008-07-23
00:03:34 UTC (rev 6904)
@@ -1,7 +1,5 @@
#include "lsp_test_speech.hh"
-#include <stdio.h>
-
using namespace lspTest;
void Speech::setUp() {
Modified: code/stage/trunk/worlds/lsp_test.world
===================================================================
--- code/stage/trunk/worlds/lsp_test.world 2008-07-22 20:23:21 UTC (rev
6903)
+++ code/stage/trunk/worlds/lsp_test.world 2008-07-23 00:03:34 UTC (rev
6904)
@@ -1,9 +1,9 @@
# lsp_test.world - test world for use with lsp_test
-include "pioneer.inc" # defines Pioneer-like robots
-include "map.inc" # defines 'floorplan' object used for obstacle maps
-include "sick.inc" # defines sick laser
+include "pioneer.inc"
+include "map.inc"
+include "sick.inc"
interval_sim 100 # simulation timestep in milliseconds
interval_real 1 # real-time interval between simulation updates in
milliseconds
@@ -14,7 +14,7 @@
window
(
size [ 700.000 700.000 ] # in pixels
- scale 37 # pixels per meter
+ scale 37.000 # pixels per meter
show_data 1 # 1=on 0=off
)
@@ -22,7 +22,7 @@
floorplan
(
name "cave"
- size [16 16 0.6]
+ size [16.000 16.000 0.600]
bitmap "bitmaps/cave.png"
)
@@ -31,7 +31,7 @@
# can refer to the robot by this name
name "r0"
- pose [ -6.5 -6.5 0 45 ]
+ pose [ -7.490 -7.490 0 45.000 ]
sicklaser()
)
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
