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 Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit