Revision: 7066
http://playerstage.svn.sourceforge.net/playerstage/?rev=7066&view=rev
Author: gbiggs
Date: 2008-10-06 01:32:59 +0000 (Mon, 06 Oct 2008)
Log Message:
-----------
Applied patch 2147885
Modified Paths:
--------------
code/player/branches/release-2-1-patches/server/drivers/localization/fakelocalize.cc
Modified:
code/player/branches/release-2-1-patches/server/drivers/localization/fakelocalize.cc
===================================================================
---
code/player/branches/release-2-1-patches/server/drivers/localization/fakelocalize.cc
2008-10-06 01:31:46 UTC (rev 7065)
+++
code/player/branches/release-2-1-patches/server/drivers/localization/fakelocalize.cc
2008-10-06 01:32:59 UTC (rev 7066)
@@ -36,7 +36,7 @@
The fakelocalize driver polls a simulation device for 2D ground truth pose
data, then reports this data as if it were generated by a localization
-system. This driver is useful for running software (e.g., @ref
+system. This driver is useful for running software (e.g., @ref
util_playernav, @ref driver_wavefront) that needs a @ref
interface_localize device without incurring the computational
cost of actually running a localization algorithm.
@@ -57,15 +57,15 @@
@par Configuration requests
- PLAYER_LOCALIZE_REQ_SET_POSE : acknowledged, but ignored
-
+
@par Configuration file options
- model (string)
- Default: NULL
- Name of simulation model for which we're faking localization.
-
[EMAIL PROTECTED] Example
[EMAIL PROTECTED] Example
+
@verbatim
driver
(
@@ -135,7 +135,7 @@
}
// a driver registration function
-void FakeLocalize_Register(DriverTable* table)
+void fakelocalize_Register(DriverTable* table)
{
table->AddDriver("fakelocalize", FakeLocalize_Init);
}
@@ -143,13 +143,14 @@
////////////////////////////////////////////////////////////////////////////////
// Constructor
FakeLocalize::FakeLocalize( ConfigFile* cf, int section)
- : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
+ : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN),
+ model (NULL)
{
// Must have an input sim
if (cf->ReadDeviceAddr(&this->sim_id, section, "requires",
PLAYER_SIMULATION_CODE, -1, NULL) != 0)
{
- this->SetError(-1);
+ this->SetError(-1);
return;
}
this->sim = NULL;
@@ -182,19 +183,28 @@
return;
}
- if(!(this->model = strdup(cf->ReadString(section, "model", NULL))))
+ const char * str = cf->ReadString(section, "model", NULL);
+ if(str == NULL)
{
PLAYER_ERROR("must specify non-NULL model name");
this->SetError(-1);
return;
}
+ this->model = strdup(str);
+ if (this->model == NULL)
+ {
+ PLAYER_ERROR("failed to duplicate model string");
+ this->SetError(-1);
+ return;
+ }
return;
}
FakeLocalize::~FakeLocalize()
{
- free(this->model);
+ if(this->model != NULL)
+ free(this->model);
}
////////////////////////////////////////////////////////////////////////////////
@@ -235,7 +245,7 @@
player_localize_data_t loc_data;
player_position2d_data_t pos_data;
player_simulation_pose2d_req_t cfg;
-
+
// Request pose
memset(&cfg, 0, sizeof cfg);
cfg.name = this->model;
@@ -243,7 +253,7 @@
Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D,
reinterpret_cast<void *>(&cfg), sizeof cfg, NULL);
-
+
if (!Reply) return -1;
if (Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
{
@@ -313,14 +323,14 @@
int FakeLocalize::ProcessMessage(QueuePointer &resp_queue,
player_msghdr * hdr,
- void * data)
+ void * data)
{
// Is it a request to set the filter's pose?
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LOCALIZE_REQ_SET_POSE,
this->localize_addr))
{
- player_simulation_pose2d_req_t req;
+ player_simulation_pose2d_req_t req;
player_localize_set_pose_t * setposereq =
(player_localize_set_pose_t*)data;
assert(setposereq);
req.pose.px = setposereq->mean.px;
@@ -331,7 +341,7 @@
req.name_count = strlen(req.name) + 1;
// look up the named model
- Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ,
+ Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_SET_POSE2D,
reinterpret_cast<void *>(&req), sizeof
req, NULL);
if (!Reply) return -1;
@@ -373,7 +383,7 @@
resp.mean.py = ans->pose.py;
resp.mean.pa = ans->pose.pa;
resp.variance = 0;
-
+
resp.particles_count = 1;
for(uint32_t i=0;i<resp.particles_count;i++)
{
@@ -382,21 +392,21 @@
resp.particles[i].pose.pa = ans->pose.pa;
resp.particles[i].alpha = 1;
}
-
+
this->Publish(this->localize_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LOCALIZE_REQ_GET_PARTICLES,
(void*)&resp, sizeof(resp), NULL);
delete Reply;
Reply = NULL;
-
+
return 0;
}
- else
+ else
{
delete Reply;
return -1;
}
- }
+ }
return -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
