Revision: 8301
http://playerstage.svn.sourceforge.net/playerstage/?rev=8301&view=rev
Author: thjc
Date: 2009-10-18 14:20:48 +0000 (Sun, 18 Oct 2009)
Log Message:
-----------
Applied patch 2859422: Player SVN trunk: goto and globalize rq forwarding issues
Modified Paths:
--------------
code/player/trunk/server/drivers/position/globalize/globalize.cc
code/player/trunk/server/drivers/position/goto/goto.cc
Modified: code/player/trunk/server/drivers/position/globalize/globalize.cc
===================================================================
--- code/player/trunk/server/drivers/position/globalize/globalize.cc
2009-10-18 14:05:50 UTC (rev 8300)
+++ code/player/trunk/server/drivers/position/globalize/globalize.cc
2009-10-18 14:20:48 UTC (rev 8301)
@@ -67,6 +67,7 @@
/** @} */
#include <stddef.h>
+#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <libplayercore/playercore.h>
@@ -91,7 +92,8 @@
player_devaddr_t p_pos_addr;
player_msghdr_t rq_hdrs[RQ_QUEUE_LEN];
QueuePointer rq_ptrs[RQ_QUEUE_LEN];
- int rq;
+ void * payloads[RQ_QUEUE_LEN];
+ int last_rq;
double cmd_interval;
double cmd_time;
uint8_t stall;
@@ -99,13 +101,19 @@
Globalize::Globalize(ConfigFile* cf, int section) : Driver(cf, section, true,
PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
+ int i;
+
this->r_local_pos_dev = NULL;
this->r_global_pos_dev = NULL;
memset(&(this->r_local_pos_addr), 0, sizeof(player_devaddr_t));
memset(&(this->r_global_pos_addr), 0, sizeof(player_devaddr_t));
memset(&(this->p_pos_addr), 0, sizeof(player_devaddr_t));
memset(this->rq_hdrs, 0, sizeof this->rq_hdrs);
- this->rq = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++)
+ {
+ this->payloads[i] = NULL;
+ }
+ this->last_rq = -1;
this->cmd_interval = 0.0;
this->cmd_time = 0.0;
this->stall = 0;
@@ -135,16 +143,31 @@
this->cmd_interval = cf->ReadFloat(section, "cmd_interval", -1.0);
}
-Globalize::~Globalize() { }
+Globalize::~Globalize()
+{
+ int i;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i])
+ {
+ free(this->payloads[i]);
+ this->payloads[i] = NULL;
+ }
+}
+
int Globalize::Setup()
{
+ int i;
+
this->r_local_pos_dev = NULL;
this->r_global_pos_dev = NULL;
this->cmd_time = 0.0;
this->stall = 0;
memset(this->rq_hdrs, 0, sizeof this->rq_hdrs);
- this->rq = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++)
+ {
+ this->payloads[i] = NULL;
+ }
+ this->last_rq = -1;
if ((Device::MatchDeviceAddress(this->r_local_pos_addr, this->p_pos_addr))
|| (Device::MatchDeviceAddress(this->r_global_pos_addr,
this->p_pos_addr)))
{
@@ -184,10 +207,17 @@
int Globalize::Shutdown()
{
+ int i;
+
if (this->r_local_pos_dev) this->r_local_pos_dev->Unsubscribe(this->InQueue);
this->r_local_pos_dev = NULL;
if (this->r_global_pos_dev)
this->r_global_pos_dev->Unsubscribe(this->InQueue);
this->r_global_pos_dev = NULL;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i])
+ {
+ free(this->payloads[i]);
+ this->payloads[i] = NULL;
+ }
return 0;
}
@@ -199,6 +229,8 @@
player_position2d_data_t pos_data;
double d;
QueuePointer null;
+ int i;
+ int n;
if (!hdr)
{
@@ -223,31 +255,51 @@
}
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, this->p_pos_addr))
{
- if ((this->rq) >= RQ_QUEUE_LEN) return -1;
- this->rq++;
- assert((this->rq) >= 0);
- this->rq_hdrs[this->rq] = *hdr;
- this->rq_ptrs[this->rq] = resp_queue;
- if (!(this->rq))
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (!(this->payloads[i]))
{
- newhdr = this->rq_hdrs[this->rq];
+ this->rq_hdrs[i] = *hdr;
+ this->rq_ptrs[i] = resp_queue;
+ this->payloads[i] = malloc(hdr->size);
+ assert(this->payloads[i]);
+ memcpy(this->payloads[i], data, hdr->size);
+ break;
+ }
+ if (!(i < RQ_QUEUE_LEN)) return -1;
+ n = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i]) n = i;
+ assert(n >= 0);
+ if (!n)
+ {
+ newhdr = this->rq_hdrs[n];
newhdr.addr = this->r_local_pos_addr;
- this->r_local_pos_dev->PutMsg(this->InQueue, &newhdr, data);
+ assert(this->payloads[n]);
+ this->r_local_pos_dev->PutMsg(this->InQueue, &newhdr, this->payloads[n],
true); // copy = true
+ this->last_rq = n;
}
return 0;
}
if ((Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, -1,
this->r_local_pos_addr)) || (Message::MatchMessage(hdr,
PLAYER_MSGTYPE_RESP_NACK, -1, this->r_local_pos_addr)))
{
- assert((this->rq) >= 0);
- assert((hdr->subtype) == (this->rq_hdrs[this->rq].subtype));
- this->Publish(this->p_pos_addr, rq_ptrs[this->rq], hdr->type,
hdr->subtype, data, 0, &(hdr->timestamp));
- this->rq_ptrs[this->rq] = null;
- this->rq--;
- if ((this->rq) >= 0)
+ if ((this->last_rq) < 0)
{
- newhdr = this->rq_hdrs[this->rq];
+ PLAYER_ERROR("last_rq < 0");
+ return -1;
+ }
+ assert((hdr->subtype) == (this->rq_hdrs[this->last_rq].subtype));
+ this->Publish(this->p_pos_addr, this->rq_ptrs[this->last_rq], hdr->type,
hdr->subtype, data, 0, &(hdr->timestamp), true); // copy = true
+ this->rq_ptrs[this->last_rq] = null;
+ assert(this->payloads[this->last_rq]);
+ free(this->payloads[this->last_rq]);
+ this->payloads[this->last_rq] = NULL;
+ this->last_rq = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i])
+ {
+ newhdr = this->rq_hdrs[i];
newhdr.addr = this->r_local_pos_addr;
- this->r_local_pos_dev->PutMsg(this->InQueue, &newhdr, data);
+ assert(this->payloads[i]);
+ this->r_local_pos_dev->PutMsg(this->InQueue, &newhdr, this->payloads[i],
true); // copy = true;
+ this->last_rq = i;
+ break;
}
return 0;
}
Modified: code/player/trunk/server/drivers/position/goto/goto.cc
===================================================================
--- code/player/trunk/server/drivers/position/goto/goto.cc 2009-10-18
14:05:50 UTC (rev 8300)
+++ code/player/trunk/server/drivers/position/goto/goto.cc 2009-10-18
14:20:48 UTC (rev 8301)
@@ -180,7 +180,8 @@
int skip;
player_msghdr_t rq_hdrs[RQ_QUEUE_LEN];
QueuePointer rq_ptrs[RQ_QUEUE_LEN];
- int rq;
+ void * payloads[RQ_QUEUE_LEN];
+ int last_rq;
static double mod(double x, double y);
static double angle_map(double d);
static double angle_diff(double a, double b);
@@ -213,6 +214,8 @@
Goto::Goto(ConfigFile* cf, int section) : Driver(cf, section, true,
PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
+ int i;
+
srand(time(NULL));
this->stopping_time = 0.0;
this->stall_start_time = 0.0;
@@ -238,7 +241,11 @@
memset(&(this->prev_pos_data), 0, sizeof(this->prev_pos_data));
this->prev_pos_data_valid = false;
memset(&(this->rq_hdrs), 0, sizeof this->rq_hdrs);
- this->rq = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++)
+ {
+ this->payloads[i] = NULL;
+ }
+ this->last_rq = -1;
if (cf->ReadDeviceAddr(&(this->position2d_provided_addr), section,
"provides",
PLAYER_POSITION2D_CODE, -1, NULL))
{
@@ -334,15 +341,30 @@
}
-Goto::~Goto() { }
+Goto::~Goto()
+{
+ int i;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i])
+ {
+ free(this->payloads[i]);
+ this->payloads[i] = NULL;
+ }
+}
+
int Goto::Setup()
{
+ int i;
+
this->position2d_required_dev = NULL;
memset(&(this->prev_pos_data), 0, sizeof(this->prev_pos_data));
this->prev_pos_data_valid = false;
memset(&(this->rq_hdrs), 0, sizeof this->rq_hdrs);
- this->rq = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++)
+ {
+ this->payloads[i] = NULL;
+ }
+ this->last_rq = -1;
// Only for driver that provides the same interface as it requires
if (Device::MatchDeviceAddress(this->position2d_required_addr,
this->position2d_provided_addr))
{
@@ -375,8 +397,15 @@
int Goto::Shutdown()
{
+ int i;
+
if (this->position2d_required_dev)
this->position2d_required_dev->Unsubscribe(this->InQueue);
this->position2d_required_dev = NULL;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i])
+ {
+ free(this->payloads[i]);
+ this->payloads[i] = NULL;
+ }
return 0;
}
@@ -394,6 +423,7 @@
double dist;
double ad, av, tv;
int i;
+ int n;
if (!hdr)
{
@@ -448,31 +478,51 @@
}
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1,
this->position2d_provided_addr))
{
- if ((this->rq) >= RQ_QUEUE_LEN) return -1;
- this->rq++;
- assert((this->rq) >= 0);
- this->rq_hdrs[this->rq] = *hdr;
- this->rq_ptrs[this->rq] = resp_queue;
- if (!(this->rq))
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (!(this->payloads[i]))
{
- newhdr = this->rq_hdrs[this->rq];
+ this->rq_hdrs[i] = *hdr;
+ this->rq_ptrs[i] = resp_queue;
+ this->payloads[i] = malloc(hdr->size);
+ assert(this->payloads[i]);
+ memcpy(this->payloads[i], data, hdr->size);
+ break;
+ }
+ if (!(i < RQ_QUEUE_LEN)) return -1;
+ n = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i]) n = i;
+ assert(n >= 0);
+ if (!n)
+ {
+ newhdr = this->rq_hdrs[n];
newhdr.addr = this->position2d_required_addr;
- this->position2d_required_dev->PutMsg(this->InQueue, &newhdr, data);
+ assert(this->payloads[n]);
+ this->position2d_required_dev->PutMsg(this->InQueue, &newhdr,
this->payloads[n], true); // copy = true
+ this->last_rq = n;
}
return 0;
}
if ((Message::MatchMessage(hdr, PLAYER_MSGTYPE_RESP_ACK, -1,
this->position2d_required_addr)) || (Message::MatchMessage(hdr,
PLAYER_MSGTYPE_RESP_NACK, -1, this->position2d_required_addr)))
{
- assert((this->rq) >= 0);
- assert((hdr->subtype) == (this->rq_hdrs[this->rq].subtype));
- this->Publish(this->position2d_provided_addr, rq_ptrs[this->rq],
hdr->type, hdr->subtype, data, 0, &(hdr->timestamp));
- this->rq_ptrs[this->rq] = null;
- this->rq--;
- if ((this->rq) >= 0)
+ if ((this->last_rq) < 0)
{
- newhdr = this->rq_hdrs[this->rq];
+ PLAYER_ERROR("last_rq < 0");
+ return -1;
+ }
+ assert((hdr->subtype) == (this->rq_hdrs[this->last_rq].subtype));
+ this->Publish(this->position2d_provided_addr,
this->rq_ptrs[this->last_rq], hdr->type, hdr->subtype, data, 0,
&(hdr->timestamp), true); // copy = true
+ this->rq_ptrs[this->last_rq] = null;
+ assert(this->payloads[this->last_rq]);
+ free(this->payloads[this->last_rq]);
+ this->payloads[this->last_rq] = NULL;
+ this->last_rq = -1;
+ for (i = 0; i < RQ_QUEUE_LEN; i++) if (this->payloads[i])
+ {
+ newhdr = this->rq_hdrs[i];
newhdr.addr = this->position2d_required_addr;
- this->position2d_required_dev->PutMsg(this->InQueue, &newhdr, data);
+ assert(this->payloads[i]);
+ this->position2d_required_dev->PutMsg(this->InQueue, &newhdr,
this->payloads[i], true); // copy = true;
+ this->last_rq = i;
+ break;
}
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry(R) Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9 - 12, 2009. Register now!
http://p.sf.net/sfu/devconference
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
