Revision: 6871
http://playerstage.svn.sourceforge.net/playerstage/?rev=6871&view=rev
Author: jeremy_asher
Date: 2008-07-15 15:51:29 -0700 (Tue, 15 Jul 2008)
Log Message:
-----------
libstageplugin: fixed allocation bug in BlobFinder and Fiducial interfaces,
updated a few example player cfgs
Modified Paths:
--------------
code/stage/trunk/libstageplugin/CMakeLists.txt
code/stage/trunk/libstageplugin/p_blobfinder.cc
code/stage/trunk/libstageplugin/p_driver.cc
code/stage/trunk/libstageplugin/p_fiducial.cc
code/stage/trunk/libstageplugin/p_laser.cc
code/stage/trunk/libstageplugin/p_speech.cc
code/stage/trunk/worlds/everything.cfg
code/stage/trunk/worlds/everything.world
code/stage/trunk/worlds/simple.cfg
code/stage/trunk/worlds/vfh.cfg
Removed Paths:
-------------
code/stage/trunk/libstageplugin/player_driver.h
Modified: code/stage/trunk/libstageplugin/CMakeLists.txt
===================================================================
--- code/stage/trunk/libstageplugin/CMakeLists.txt 2008-07-15 16:48:50 UTC
(rev 6870)
+++ code/stage/trunk/libstageplugin/CMakeLists.txt 2008-07-15 22:51:29 UTC
(rev 6871)
@@ -3,6 +3,7 @@
include_directories( ${PLAYER_INCLUDE_DIRS})
add_library( stageplugin MODULE
+ p_driver.h
p_driver.cc
p_blobfinder.cc
p_simulation.cc
@@ -10,6 +11,7 @@
p_fiducial.cc
p_position.cc
p_sonar.cc
+ p_speech.cc
stg_time.cc
)
Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-15 16:48:50 UTC
(rev 6870)
+++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-15 22:51:29 UTC
(rev 6871)
@@ -49,52 +49,58 @@
void InterfaceBlobfinder::Publish( void )
{
+ player_blobfinder_data_t bfd;
+ bzero( &bfd, sizeof(bfd) );
+
StgModelBlobfinder* blobmod = (StgModelBlobfinder*)this->mod;
+
uint32_t bcount = 0;
stg_blobfinder_blob_t* blobs = blobmod->GetBlobs( &bcount );
-
- player_blobfinder_data_t bfd;
- bzero( &bfd, sizeof(bfd) );
-
- // and set the image width * height
- bfd.width = blobmod->scan_width;
- bfd.height = blobmod->scan_height;
- bfd.blobs_count = bcount;
-
- // now run through the blobs, packing them into the player buffer
- // counting the number of blobs in each channel and making entries
- // in the acts header
- unsigned int b;
- 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
- << endl;
- */
-
- int dx = blobs[b].right - blobs[b].left;
- int dy = blobs[b].top - blobs[b].bottom;
+
+ if ( bcount > 0 )
+ {
+ // and set the image width * height
+ bfd.width = blobmod->scan_width;
+ bfd.height = blobmod->scan_height;
+ bfd.blobs_count = bcount;
- bfd.blobs[b].x = blobs[b].left + dx/2;
- bfd.blobs[b].y = blobs[b].bottom + dy/2;
+ bfd.blobs = new player_blobfinder_blob_t[ bcount ];
+
+ // now run through the blobs, packing them into the player buffer
+ // counting the number of blobs in each channel and making entries
+ // in the acts header
+ unsigned int b;
+ 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
+ << 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;
- bfd.blobs[b].left = blobs[b].left;
- bfd.blobs[b].right = blobs[b].right;
- bfd.blobs[b].top = blobs[b].top;
- bfd.blobs[b].bottom = blobs[b].bottom;
-
- bfd.blobs[b].color = blobs[b].color;
- bfd.blobs[b].area = dx * dy;
-
- bfd.blobs[b].range = blobs[b].range;
- }
+ bfd.blobs[b].left = blobs[b].left;
+ bfd.blobs[b].right = blobs[b].right;
+ bfd.blobs[b].top = blobs[b].top;
+ bfd.blobs[b].bottom = blobs[b].bottom;
+
+ bfd.blobs[b].color = blobs[b].color;
+ bfd.blobs[b].area = dx * dy;
+
+ bfd.blobs[b].range = blobs[b].range;
+ }
+ }
// should change player interface to support variable-lenght blob data
// size_t size = sizeof(bfd) - sizeof(bfd.blobs) + bcount *
sizeof(bfd.blobs[0]);
@@ -103,6 +109,8 @@
PLAYER_MSGTYPE_DATA,
PLAYER_BLOBFINDER_DATA_BLOBS,
&bfd,
sizeof(bfd), NULL);
+ if ( bfd.blobs )
+ delete [] bfd.blobs;
}
int InterfaceBlobfinder::ProcessMessage( QueuePointer& resp_queue,
Modified: code/stage/trunk/libstageplugin/p_driver.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_driver.cc 2008-07-15 16:48:50 UTC (rev
6870)
+++ code/stage/trunk/libstageplugin/p_driver.cc 2008-07-15 22:51:29 UTC (rev
6871)
@@ -310,6 +310,11 @@
case PLAYER_BLOBFINDER_CODE:
ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section );
break;
+
+// case PLAYER_CAMERA_CODE:
+// ifsrc = new InterfaceCamera( player_addr, this, cf, section );
+// break;
+
case PLAYER_SIMULATION_CODE:
ifsrc = new InterfaceSimulation( player_addr, this, cf, section );
@@ -326,7 +331,7 @@
case PLAYER_SONAR_CODE:
ifsrc = new InterfaceSonar( player_addr, this, cf, section );
break;
-
+
// case PLAYER_POWER_CODE:
// ifsrc = new InterfacePower( player_addr, this, cf, section );
// break;
@@ -363,9 +368,9 @@
// ifsrc = new InterfaceWifi( player_addr, this, cf, section );
// break;
-// case PLAYER_SPEECH_CODE:
-// ifsrc = new InterfaceSpeech( player_addr, this, cf, section );
-// break;
+ case PLAYER_SPEECH_CODE:
+ ifsrc = new InterfaceSpeech( player_addr, this, cf, section );
+ break;
// case PLAYER_BUMPER_CODE:
// ifsrc = new InterfaceBumper( player_addr, this, cf, section );
Modified: code/stage/trunk/libstageplugin/p_fiducial.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_fiducial.cc 2008-07-15 16:48:50 UTC
(rev 6870)
+++ code/stage/trunk/libstageplugin/p_fiducial.cc 2008-07-15 22:51:29 UTC
(rev 6871)
@@ -71,6 +71,8 @@
//printf( "reporting %d fiducials\n",
// fcount );
+
+ pdata.fiducials = new player_fiducial_item_t[pdata.fiducials_count];
for( int i=0; i<(int)pdata.fiducials_count; i++ )
{
@@ -80,22 +82,9 @@
double xpos = fids[i].range * cos(fids[i].bearing);
double ypos = fids[i].range * sin(fids[i].bearing);
- /*
- pdata.fiducials[i].pos[0] = xpos;
- pdata.fiducials[i].pos[1] = ypos;
-
- // yaw only
- pdata.fiducials[i].rot[2] = fids[i].geom.a;
- // player can't handle per-fiducial size.
- // we leave uncertainty (upose) at zero
- */
-
- // I'm guessing at this, but the above doesn't compile, because
- // there's no 'pos' or 'rot' fields - BPG
-
- pdata.fiducials[i].pose.px = fids[i].pose.x;
- pdata.fiducials[i].pose.py = fids[i].pose.y;
- pdata.fiducials[i].pose.pz = fids[i].pose.z;
+ pdata.fiducials[i].pose.px = xpos;
+ pdata.fiducials[i].pose.py = ypos;
+ pdata.fiducials[i].pose.pz = 0.0;
pdata.fiducials[i].pose.proll = 0.0;
pdata.fiducials[i].pose.ppitch = 0.0;
pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
@@ -107,6 +96,9 @@
PLAYER_MSGTYPE_DATA,
PLAYER_FIDUCIAL_DATA_SCAN,
&pdata,
sizeof(pdata), NULL);
+
+ if ( pdata.fiducials )
+ delete [] pdata.fiducials;
}
int InterfaceFiducial::ProcessMessage(QueuePointer& resp_queue,
Modified: code/stage/trunk/libstageplugin/p_laser.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_laser.cc 2008-07-15 16:48:50 UTC (rev
6870)
+++ code/stage/trunk/libstageplugin/p_laser.cc 2008-07-15 22:51:29 UTC (rev
6871)
@@ -73,7 +73,7 @@
pdata.ranges = new float[pdata.ranges_count];
pdata.intensity = new uint8_t[pdata.ranges_count];
- for( int i=0; i<cfg.sample_count; i++ )
+ for( unsigned int i=0; i<cfg.sample_count; i++ )
{
//printf( "range %d %d\n", i, samples[i].range);
pdata.ranges[i] = samples[i].range;
Modified: code/stage/trunk/libstageplugin/p_speech.cc
===================================================================
--- code/stage/trunk/libstageplugin/p_speech.cc 2008-07-15 16:48:50 UTC (rev
6870)
+++ code/stage/trunk/libstageplugin/p_speech.cc 2008-07-15 22:51:29 UTC (rev
6871)
@@ -34,16 +34,13 @@
- PLAYER_SPEECH_CMD_SAY
*/
-extern "C" {
-int speech_init( stg_model_t* mod );
-}
InterfaceSpeech::InterfaceSpeech( player_devaddr_t addr,
StgDriver* driver,
ConfigFile* cf,
int section )
- : InterfaceModel( addr, driver, cf, section, speech_init )
+ : InterfaceModel( addr, driver, cf, section, MODEL_TYPE_PLAIN )
{
// nothing to do
}
@@ -67,68 +64,38 @@
*/
}
-int InterfaceSpeech::ProcessMessage(MessageQueue* resp_queue,
- player_msghdr_t* hdr,
- void* data)
+int InterfaceSpeech::ProcessMessage( QueuePointer & resp_queue,
+
player_msghdr_t* hdr,
+ void*
data )
{
- // PROCESS INCOMING REQUESTS HERE
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
- PLAYER_SPEECH_CMD_SAY,
- this->addr))
- {
- if( hdr->size == sizeof(player_speech_cmd_t) )
+// PROCESS INCOMING REQUESTS HERE
+ if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_SPEECH_CMD_SAY,
+ this->addr))
{
- player_speech_cmd_t* pcmd = (player_speech_cmd_t*)data;
+ if( hdr->size == sizeof(player_speech_cmd_t) )
+ {
+ player_speech_cmd_t* pcmd = (player_speech_cmd_t*)data;
- // Pass it to stage:
- stg_speech_cmd_t cmd;
-// cmd.cmd = STG_SPEECH_CMD_NOP;
-// cmd.string[0] = 0;
+ // Pass it to stage:
- cmd.cmd = STG_SPEECH_CMD_SAY;
- strncpy(cmd.string, pcmd->string, STG_SPEECH_MAX_STRING_LEN);
- cmd.string[STG_SPEECH_MAX_STRING_LEN-1]=0;
-
- stg_model_set_cmd( this->mod, &cmd, sizeof(cmd) );
- }
- else
- PRINT_ERR2( "wrong size speech command packet (%d/%d bytes)",
- (int)hdr->size, (int)sizeof(player_speech_cmd_t) );
+ mod->Say( pcmd->string );
- return 0;
- }
-/*
- // is it a geometry request?
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_GRIPPER_REQ_GET_GEOM,
- this->addr))
- {
- // TODO: get pose in top-level model's CS instead.
-
- stg_geom_t geom;
- stg_model_get_geom( this->mod, &geom );
-
- stg_pose_t pose;
- stg_model_get_pose( this->mod, &pose);
-
- player_gripper_geom_t pgeom;
- pgeom.pose.px = pose.x;
- pgeom.pose.py = pose.y;
- pgeom.pose.pa = pose.a;
- pgeom.size.sw = geom.size.y;
- pgeom.size.sl = geom.size.x;
-
- this->driver->Publish(this->addr, resp_queue,
- PLAYER_MSGTYPE_RESP_ACK,
- PLAYER_GRIPPER_REQ_GET_GEOM,
- (void*)&pgeom, sizeof(pgeom), NULL);
- return(0);
+ this->driver->Publish(this->addr, resp_queue,
+
PLAYER_MSGTYPE_RESP_ACK,
+
PLAYER_SPEECH_CMD_SAY);
+ return( 0 );
+ }
+ else
+ {
+ PRINT_ERR2( "wrong size speech command packet (%d/%d
bytes)",
+ (int)hdr->size,
(int)sizeof(player_speech_cmd_t) );
-
- }
-*/
- PRINT_WARN2( "stage speech doesn't support message id:%d/%d",
- hdr->type, hdr->subtype );
- return -1;
-
+ return( -1 );
+ }
+ }
+
+ PRINT_WARN2( "stage speech doesn't support message id:%d/%d",
+ hdr->type, hdr->subtype );
+ return ( -1 );
}
Deleted: code/stage/trunk/libstageplugin/player_driver.h
===================================================================
--- code/stage/trunk/libstageplugin/player_driver.h 2008-07-15 16:48:50 UTC
(rev 6870)
+++ code/stage/trunk/libstageplugin/player_driver.h 2008-07-15 22:51:29 UTC
(rev 6871)
@@ -1,69 +0,0 @@
-#ifndef _STAGE_PLAYER_DRIVER_H
-#define _STAGE_PLAYER_DRIVER_H
-
-#include <unistd.h>
-#include <string.h>
-#include <netinet/in.h>
-#include <math.h>
-
-#include "player.h"
-#include "player/device.h"
-#include "player/driver.h"
-#include "player/configfile.h"
-#include "player/drivertable.h"
-#include <player/drivertable.h>
-#include <player/driver.h>
-#include <player/error.h>
-#include "playercommon.h"
-
-#include "stage_internal.h"
-#include "stg_time.h"
-
-// foward declare;
-class Interface;
-
-class StgDriver : public Driver
-{
- public:
- // Constructor; need that
- StgDriver(ConfigFile* cf, int section);
-
- // Destructor
- ~StgDriver(void);
-
- // Must implement the following methods.
- int Setup();
- int Shutdown();
-
- int Subscribe(player_device_id_t id);
- int Unsubscribe(player_device_id_t id);
-
- /// Main function for device thread.
- void Main();
-
- /// The server thread calls this method frequently. We use it to
- /// check for new commands and configs
- void Update();
-
- // override the Driver method to grab configs inside the server thread
- //int PutConfig(player_device_id_t id, void *client,
- // void* src, size_t len,
- // struct timeval* timestamp);
-
- /// all player devices share the same Stage world
- static stg_world_t* world;
-
- /// find the device record with this Player id
- Interface* LookupDevice( player_device_id_t id );
-
- stg_model_t* LocateModel( const char* basename,
- stg_model_type_t mod_type );
-
- protected:
-
- /// an array of pointers to device_record_t structs, defined below
- GPtrArray* devices;
-};
-
-
-#endif
Modified: code/stage/trunk/worlds/everything.cfg
===================================================================
--- code/stage/trunk/worlds/everything.cfg 2008-07-15 16:48:50 UTC (rev
6870)
+++ code/stage/trunk/worlds/everything.cfg 2008-07-15 22:51:29 UTC (rev
6871)
@@ -64,10 +64,3 @@
provides ["6671:position2d:0" "6671:sonar:0" "6671:laser:0"
"6671:blobfinder:0" "6671:fiducial:0"]
model "p6"
)
-
-# robot 7
-driver(
- name "stage"
- provides ["6672:position2d:0" "6672:bumper:0"]
- model "roomba1"
-)
Modified: code/stage/trunk/worlds/everything.world
===================================================================
--- code/stage/trunk/worlds/everything.world 2008-07-15 16:48:50 UTC (rev
6870)
+++ code/stage/trunk/worlds/everything.world 2008-07-15 22:51:29 UTC (rev
6871)
@@ -55,7 +55,7 @@
#
define trickedoutpioneer pioneer2dx
(
- ranger( alwayson 1 )
+ ranger( alwayson 0 )
sicklaser( pose [0.030 0 0 0 ] )
@@ -64,7 +64,7 @@
blobfinder(
channel_count 6
channels [ "red" "blue" "green" "cyan" "yellow" "magenta" ]
- alwayson 1
+ alwayson 0
)
fiducial_return 17
@@ -84,12 +84,20 @@
#speech()
)
+#trickedoutpioneer
+#(
+# color "red"
+# name "redrobot_w_camera"
+# pose [-5.645 3.034 0 -162.098]
+# camera( pose [ 0 0 0 0 ] range [ 0.2 8.0 ] resolution [ 100 100 ] fov #[
70 40 ] pantilt [ 0 0 ] )
+#)
+
trickedoutpioneer
(
- color "red"
- name "redrobot_w_camera"
- pose [-5.645 3.034 0 -162.098]
- camera( pose [ 0 0 0 0 ] range [ 0.2 8.0 ] resolution [ 100 100 ] fov [ 70
40 ] pantilt [ 0 0 ] )
+ fiducial_return 18
+ color "NavyBlue"
+ name "p1"
+ pose [-7.009 2.481 0 165.780]
)
trickedoutpioneer
Modified: code/stage/trunk/worlds/simple.cfg
===================================================================
--- code/stage/trunk/worlds/simple.cfg 2008-07-15 16:48:50 UTC (rev 6870)
+++ code/stage/trunk/worlds/simple.cfg 2008-07-15 22:51:29 UTC (rev 6871)
@@ -22,7 +22,7 @@
driver
(
name "stage"
- provides [ "position2d:0" "laser:0" ]
+ provides [ "position2d:0" "laser:0" "speech:0" ]
model "r0"
)
Modified: code/stage/trunk/worlds/vfh.cfg
===================================================================
--- code/stage/trunk/worlds/vfh.cfg 2008-07-15 16:48:50 UTC (rev 6870)
+++ code/stage/trunk/worlds/vfh.cfg 2008-07-15 22:51:29 UTC (rev 6871)
@@ -20,7 +20,7 @@
(
name "stage"
provides ["position2d:0" "laser:0"]
- model "robot1"
+ model "r0"
#alwayson 1
)
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
