Revision: 6899
http://playerstage.svn.sourceforge.net/playerstage/?rev=6899&view=rev
Author: jeremy_asher
Date: 2008-07-21 23:39:22 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
libstageplugin: fixed a few bugs with laser interface configuration setting,
speech interface should be working, updated tests
Modified Paths:
--------------
code/stage/trunk/libstage/model_laser.cc
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstageplugin/p_laser.cc
code/stage/trunk/libstageplugin/p_speech.cc
code/stage/trunk/libstageplugin/test/lsp_test_laser.cc
code/stage/trunk/libstageplugin/test/lsp_test_laser.hh
Added Paths:
-----------
code/stage/trunk/worlds/lsp_test.cfg
code/stage/trunk/worlds/lsp_test.world
Modified: code/stage/trunk/libstage/model_laser.cc
===================================================================
--- code/stage/trunk/libstage/model_laser.cc 2008-07-21 23:21:42 UTC (rev
6898)
+++ code/stage/trunk/libstage/model_laser.cc 2008-07-21 23:39:22 UTC (rev
6899)
@@ -141,6 +141,7 @@
cfg.range_bounds.max = range_max;
cfg.fov = fov;
cfg.resolution = resolution;
+ cfg.interval = interval;
return cfg;
}
@@ -150,6 +151,7 @@
range_max = cfg.range_bounds.max;
fov = cfg.fov;
resolution = cfg.resolution;
+ interval = cfg.interval;
}
static bool laser_raytrace_match( StgBlock* testblock,
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2008-07-21 23:21:42 UTC (rev 6898)
+++ code/stage/trunk/libstage/stage.hh 2008-07-21 23:39:22 UTC (rev 6899)
@@ -1196,8 +1196,8 @@
int subs; //< the number of subscriptions to this model
- stg_usec_t interval; //< time between updates in ms
- stg_usec_t last_update; //< time of last update in ms
+ stg_usec_t interval; //< time between updates in us
+ stg_usec_t last_update; //< time of last update in us
stg_bool_t disabled; //< if non-zero, the model is disabled
@@ -2162,6 +2162,7 @@
uint32_t resolution;
stg_range_bounds_t range_bounds;
stg_radians_t fov;
+ stg_usec_t interval;
} stg_laser_cfg_t;
/// %StgModelLaser class
Modified: code/stage/trunk/libstageplugin/p_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_laser.cc 2008-07-21 23:21:42 UTC (rev
6898)
+++ code/stage/trunk/libstageplugin/p_laser.cc 2008-07-21 23:39:22 UTC (rev
6899)
@@ -62,11 +62,10 @@
memset( &pdata, 0, sizeof(pdata) );
stg_laser_cfg_t cfg = mod->GetConfig();
-
pdata.min_angle = -cfg.fov/2.0;
pdata.max_angle = +cfg.fov/2.0;
+ pdata.resolution = cfg.fov / cfg.sample_count;
pdata.max_range = cfg.range_bounds.max;
- pdata.resolution = cfg.fov / (double)(cfg.sample_count-1);
pdata.ranges_count = pdata.intensity_count = cfg.sample_count;
pdata.id = this->scan_id++;
@@ -114,18 +113,22 @@
plc->resolution/1e2);
stg_laser_cfg_t cfg = mod->GetConfig();
+
+ cfg.resolution = 1.0 / plc->resolution;
+ cfg.range_bounds.max = plc->max_range;
+ cfg.fov = plc->max_angle - plc->min_angle;
+ cfg.interval = 1E6 / plc->scanning_frequency;
- // tweak the parts that player knows about
- cfg.fov = plc->max_angle - plc->min_angle;
+ PRINT_DEBUG4( "setting laser config: fov %.2f max_range %.2f " /
+ "resolution %.2f interval %.2f",
+ cfg.fov, cfg.range_bounds.max,
+ cfg.resolution, cfg.interval );
- // todo - test this
- cfg.sample_count = (int)(cfg.fov / DTOR(plc->resolution/1e2));
-
- PRINT_DEBUG2( "setting laser config: fov %.2f samples %d",
- slc.fov, slc.samples );
-
+ // Range resolution is currently locked to the world setting
+ // and intensity values are always read. The relevant settings
+ // are ignored.
+
mod->SetConfig( cfg );
-
this->driver->Publish(this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
@@ -153,9 +156,11 @@
memset(&plc,0,sizeof(plc));
plc.min_angle = -cfg.fov/2.0;
plc.max_angle = +cfg.fov/2.0;
+ plc.resolution = cfg.fov / cfg.sample_count;
plc.max_range = cfg.range_bounds.max;
- plc.resolution = (uint8_t)(RTOD(cfg.fov / (cfg.sample_count-1)) * 100);
+ plc.range_res = 1.0; // todo
plc.intensity = 1; // todo
+ plc.scanning_frequency = 1E6 / cfg.interval;
this->driver->Publish(this->addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
Modified: code/stage/trunk/libstageplugin/p_speech.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_speech.cc 2008-07-21 23:21:42 UTC (rev
6898)
+++ code/stage/trunk/libstageplugin/p_speech.cc 2008-07-21 23:39:22 UTC (rev
6899)
@@ -73,8 +73,8 @@
PLAYER_SPEECH_CMD_SAY,
this->addr))
{
- if( hdr->size == sizeof(player_speech_cmd_t) )
- {
+// if( hdr->size == sizeof(player_speech_cmd_t) )
+// {
player_speech_cmd_t* pcmd = (player_speech_cmd_t*)data;
// Pass it to stage:
@@ -82,14 +82,14 @@
mod->Say( pcmd->string );
return( 0 );
- }
- else
- {
- PRINT_ERR2( "wrong size speech command packet (%d/%d
bytes)",
- (int)hdr->size,
(int)sizeof(player_speech_cmd_t) );
-
- return( -1 );
- }
+// }
+// else
+// {
+// PRINT_ERR2( "wrong size speech command packet (%d/%d
bytes)",
+// (int)hdr->size,
(int)sizeof(player_speech_cmd_t) );
+//
+// return( -1 );
+// }
}
PRINT_WARN2( "stage speech doesn't support message id:%d/%d",
Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-21
23:21:42 UTC (rev 6898)
+++ code/stage/trunk/libstageplugin/test/lsp_test_laser.cc 2008-07-21
23:39:22 UTC (rev 6899)
@@ -2,6 +2,8 @@
using namespace lspTest;
+const double Laser::Delta = 0.01;
+
void Laser::setUp() {
connect();
laserProxy = playerc_laser_create( client, 0 );
@@ -16,14 +18,12 @@
}
void Laser::testConfig() {
- const double DELTA = 0.01;
-
double min = -M_PI/2;
double max = +M_PI/2;
- double res = 100;
- double range_res = 1;
- unsigned char intensity = 1;
- double freq = 10;
+ double res = M_PI/361; // sick laser default
+ double range_res = 1; // not being used by stage
+ unsigned char intensity = 1; // not being used by stage
+ double freq = 1.0/1E4; // stage default
CPPUNIT_ASSERT( playerc_laser_set_config( laserProxy, min, max, res,
range_res, intensity, freq ) == 0 );
@@ -31,14 +31,34 @@
unsigned char intensity2 = 1;
CPPUNIT_ASSERT( playerc_laser_get_config( laserProxy, &min2, &max2,
&res2, &range_res2, &intensity2, &freq2 ) == 0 );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", min, min2,
DELTA );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", max, max2,
DELTA );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular resolution", res, res2,
DELTA );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "range resolution", range_res,
range_res2, DELTA );
- CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "scan freq", freq, freq2, DELTA );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "min scan angle", min, min2,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "max scan angle", max, max2,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "angular resolution", res, res2,
Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "range resolution", range_res,
range_res2, Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "scan freq", freq, freq2, Delta );
CPPUNIT_ASSERT_EQUAL_MESSAGE( "intensity", intensity, intensity2 );
}
+void Laser::testGeom() {
+ CPPUNIT_ASSERT( playerc_laser_get_geom( laserProxy ) == 0 );
+
+ // 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( "size (x)", 0.156,
laserProxy->size[0], Delta );
+ CPPUNIT_ASSERT_DOUBLES_EQUAL_MESSAGE( "size (y)", 0.155,
laserProxy->size[1], Delta );
+}
+
void Laser::testData() {
+ playerc_client_read( client );
+ printf("\nlaser: [%14.3f ] [%d]\n ", laserProxy->info.datatime,
laserProxy->scan_count);
+ for ( int i = 0; i < laserProxy->scan_count; i++ ) {
+ printf("[%6.3f, %6.3f ] \n", laserProxy->scan[i][0],
laserProxy->scan[i][1]);
+ }
+
+ // check first and last sample's angle
+ // check number of samples
+ // check range of each is within max and min
}
\ No newline at end of file
Modified: code/stage/trunk/libstageplugin/test/lsp_test_laser.hh
===================================================================
--- code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-21
23:21:42 UTC (rev 6898)
+++ code/stage/trunk/libstageplugin/test/lsp_test_laser.hh 2008-07-21
23:39:22 UTC (rev 6899)
@@ -11,6 +11,7 @@
{
CPPUNIT_TEST_SUITE( Laser );
CPPUNIT_TEST( testConfig );
+ CPPUNIT_TEST( testGeom );
CPPUNIT_TEST( testData );
CPPUNIT_TEST_SUITE_END();
@@ -18,8 +19,10 @@
playerc_laser_t* laserProxy;
void testConfig();
+ void testGeom();
void testData();
+ static const double Delta;
public:
void setUp();
void tearDown();
Added: code/stage/trunk/worlds/lsp_test.cfg
===================================================================
--- code/stage/trunk/worlds/lsp_test.cfg (rev 0)
+++ code/stage/trunk/worlds/lsp_test.cfg 2008-07-21 23:39:22 UTC (rev
6899)
@@ -0,0 +1,22 @@
+# Desc: Player configuration file for use with lsp_test utility
+
+# load the Stage plugin simulation driver
+driver
+(
+ name "stage"
+ provides [ "simulation:0" ]
+ plugin "libstageplugin"
+
+ # load the named file into the simulator
+ worldfile "lsp_test.world"
+)
+
+# Create a Stage driver and attach position2d and laser interfaces
+# to the model "r0"
+driver
+(
+ name "stage"
+ provides [ "position2d:0" "laser:0" "speech:0" ]
+ model "r0"
+)
+
Added: code/stage/trunk/worlds/lsp_test.world
===================================================================
--- code/stage/trunk/worlds/lsp_test.world (rev 0)
+++ code/stage/trunk/worlds/lsp_test.world 2008-07-21 23:39:22 UTC (rev
6899)
@@ -0,0 +1,37 @@
+# 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
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 1 # real-time interval between simulation updates in
milliseconds
+
+paused 0
+
+# configure the GUI window
+window
+(
+ size [ 700.000 700.000 ] # in pixels
+ scale 37 # pixels per meter
+ show_data 1 # 1=on 0=off
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "cave"
+ size [16 16 0.6]
+ bitmap "bitmaps/cave.png"
+)
+
+pioneer2dx
+(
+ # can refer to the robot by this name
+ name "r0"
+
+ pose [ -6.5 -6.5 0 45 ]
+ 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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
