Update of /cvsroot/playerstage/code/player/server/drivers/fiducial
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10946/server/drivers/fiducial
Modified Files:
.cvsignore laserbar.cc laserbarcode.cc laservisualbarcode.cc
laservisualbw.cc
Log Message:
applied Toby's patch to replace fixed-size arrays
Index: laservisualbw.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/fiducial/laservisualbw.cc,v
retrieving revision 1.19
retrieving revision 1.20
diff -C2 -d -r1.19 -r1.20
*** laservisualbw.cc 23 Aug 2007 19:58:43 -0000 1.19
--- laservisualbw.cc 1 Nov 2007 22:16:18 -0000 1.20
***************
*** 246,249 ****
--- 246,250 ----
// Local copy of the current fiducial data.
private: player_fiducial_data_t fdata;
+ int fdata_allocated;
};
***************
*** 331,334 ****
--- 332,338 ----
int LaserVisualBW::Setup()
{
+ fdata_allocated = 0;
+ fdata.fiducials = NULL;
+
// Subscribe to the laser.
if (!(laser = deviceTable->GetDevice (laser_id)))
***************
*** 380,383 ****
--- 384,389 ----
camera->Unsubscribe(InQueue);
+ free(fdata.fiducials);
+
return 0;
}
***************
*** 415,445 ****
}
- /*
-
- if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_REQ_GET_GEOM,
device_id))
- {
- assert(*resp_len>sizeof(player_fiducial_geom_t));
- assert(*resp_len>sizeof(player_laser_geom_t));
-
- int ret = laser->ProcessMessage( PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_GEOM,
- laser_id, 0, resp_data, resp_data, resp_len);
- if (ret != PLAYER_MSGTYPE_RESP_ACK)
- return ret;
- assert(*resp_len == sizeof(player_laser_geom_t));
- player_laser_geom_t lgeom = * reinterpret_cast<player_laser_geom_t * >
(resp_data);
- player_fiducial_geom_t * fgeom =
reinterpret_cast<player_fiducial_geom_t * > (resp_data);
-
- fgeom->pose[0] = lgeom.pose[0];
- fgeom->pose[1] = lgeom.pose[1];
- fgeom->pose[2] = lgeom.pose[2];
- fgeom->size[0] = lgeom.size[0];
- fgeom->size[1] = lgeom.size[1];
- fgeom->fiducial_size[0] = ntohs((int) (this->barwidth * 1000));
- fgeom->fiducial_size[1] = ntohs((int) (this->barwidth * 1000));
-
- *resp_len=sizeof(player_fiducial_geom_t);
-
- return ret;
- }*/
return -1;
}
--- 421,424 ----
***************
*** 607,622 ****
if (minfiducial == NULL)
{
! if (this->fiducial_count < ARRAYSIZE(this->fiducials))
{
! minfiducial = this->fiducials + this->fiducial_count++;
! minfiducial->id = -1;
! minfiducial->pose[0] = pose[0];
! minfiducial->pose[1] = pose[1];
! minfiducial->pose[2] = pose[2];
! minfiducial->laser_time = time;
! minfiducial->ptz_select_time = -1;
! minfiducial->ptz_lockon_time = -1;
! minfiducial->id_time = -1;
}
}
--- 586,604 ----
if (minfiducial == NULL)
{
! this->fiducial_count++;
! if (this->fiducial_count+1 > this->fdata_allocated)
{
! this->fdata_allocated = this->fiducial_count+1;
! this->fdata.fiducials =
(player_fiducial_item_t*)realloc(this->fdata.fiducials,
sizeof(this->fdata.fiducials[0])*this->fdata_allocated);
}
+ minfiducial = &this->fiducials[this->fiducial_count-1];
+ minfiducial->id = -1;
+ minfiducial->pose[0] = pose[0];
+ minfiducial->pose[1] = pose[1];
+ minfiducial->pose[2] = pose[2];
+ minfiducial->laser_time = time;
+ minfiducial->ptz_select_time = -1;
+ minfiducial->ptz_lockon_time = -1;
+ minfiducial->id_time = -1;
}
Index: laservisualbarcode.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/fiducial/laservisualbarcode.cc,v
retrieving revision 1.15
retrieving revision 1.16
diff -C2 -d -r1.15 -r1.16
*** laservisualbarcode.cc 23 Aug 2007 19:58:43 -0000 1.15
--- laservisualbarcode.cc 1 Nov 2007 22:16:18 -0000 1.16
***************
*** 267,270 ****
--- 267,271 ----
// Local copy of the current fiducial data.
private: player_fiducial_data_t fdata;
+ int fdata_allocated;
};
***************
*** 345,348 ****
--- 346,351 ----
int LaserVisualBarcode::Setup()
{
+ fdata_allocated = 0;
+ fdata.fiducials = NULL;
// Subscribe to the laser.
***************
*** 398,401 ****
--- 401,406 ----
blobfinder->Unsubscribe(InQueue);
+ free(fdata.fiducials);
+
return 0;
}
***************
*** 434,459 ****
}
- /*
- if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_REQ_GET_GEOM,
device_id))
- {
- int ret = laser->ProcessMessage(PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_GEOM, laser_id, 0, resp_data, resp_data, resp_len);
- if (ret != PLAYER_MSGTYPE_RESP_ACK)
- return ret;
- assert(*resp_len == sizeof(player_laser_geom_t));
- player_laser_geom_t lgeom = * reinterpret_cast<player_laser_geom_t * >
(resp_data);
- player_fiducial_geom_t * fgeom =
reinterpret_cast<player_fiducial_geom_t * > (resp_data);
- fgeom->pose[0] = lgeom.pose[0];
- fgeom->pose[1] = lgeom.pose[1];
- fgeom->pose[2] = lgeom.pose[2];
- fgeom->size[0] = lgeom.size[0];
- fgeom->size[1] = lgeom.size[1];
- fgeom->fiducial_size[0] = ntohs((int) (this->barwidth * 1000));
- fgeom->fiducial_size[1] = ntohs((int) (this->barwidth * 1000));
-
- *resp_len=sizeof(player_fiducial_geom_t);
-
- return ret;
- }*/
return -1;
}
--- 439,443 ----
***************
*** 623,638 ****
if (minfiducial == NULL)
{
! if (this->fiducial_count < ARRAYSIZE(this->fiducials))
{
! minfiducial = this->fiducials + this->fiducial_count++;
! minfiducial->id = -1;
! minfiducial->pose[0] = pose[0];
! minfiducial->pose[1] = pose[1];
! minfiducial->pose[2] = pose[2];
! minfiducial->laser_time = time;
! minfiducial->ptz_select_time = -1;
! minfiducial->ptz_lockon_time = -1;
! minfiducial->id_time = -1;
}
}
--- 607,625 ----
if (minfiducial == NULL)
{
! this->fiducial_count++;
! if (this->fiducial_count+1 > this->fdata_allocated)
{
! this->fdata_allocated = this->fiducial_count+1;
! this->fdata.fiducials =
(player_fiducial_item_t*)realloc(this->fdata.fiducials,
sizeof(this->fdata.fiducials[0])*this->fdata_allocated);
}
+ minfiducial = &this->fiducials[this->fiducial_count-1];
+ minfiducial->id = -1;
+ minfiducial->pose[0] = pose[0];
+ minfiducial->pose[1] = pose[1];
+ minfiducial->pose[2] = pose[2];
+ minfiducial->laser_time = time;
+ minfiducial->ptz_select_time = -1;
+ minfiducial->ptz_lockon_time = -1;
+ minfiducial->id_time = -1;
}
***************
*** 987,991 ****
// Copy data to server.
! Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN,
(void*) &data, sizeof(data), ×tamp);
}
--- 974,978 ----
// Copy data to server.
! Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN,
(void*) &data, 0, ×tamp);
}
Index: .cvsignore
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/fiducial/.cvsignore,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** .cvsignore 17 Sep 2007 02:18:50 -0000 1.2
--- .cvsignore 1 Nov 2007 22:16:18 -0000 1.3
***************
*** 4,5 ****
--- 4,7 ----
*.la
*.a
+ .libs
+ *.lo
Index: laserbarcode.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/fiducial/laserbarcode.cc,v
retrieving revision 1.19
retrieving revision 1.20
diff -C2 -d -r1.19 -r1.20
*** laserbarcode.cc 23 Aug 2007 19:58:43 -0000 1.19
--- laserbarcode.cc 1 Nov 2007 22:16:18 -0000 1.20
***************
*** 181,184 ****
--- 181,185 ----
// Current fiducial data
private: player_fiducial_data_t data;
+ unsigned int fdata_allocated;
};
***************
*** 230,233 ****
--- 231,237 ----
int LaserBarcode::Setup()
{
+ fdata_allocated = 0;
+ data.fiducials = NULL;
+
// Subscribe to the laser.
if (Device::MatchDeviceAddress (laser_id, device_addr))
***************
*** 262,265 ****
--- 266,271 ----
laser->Unsubscribe(InQueue);
+ free(data.fiducials);
+
PLAYER_MSG0(2, "laserbarcode device: shutdown");
return 0;
***************
*** 289,346 ****
}
- /* if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_REQ_GET_GEOM,
device_id))
- {
- hdr->device_index = laser_id.index;
- hdr->subtype = PLAYER_LASER_REQ_GET_GEOM;
- int ret = laser_driver->ProcessMessage(BaseClient, hdr, data, resp_data,
resp_len);
- hdr->subtype = PLAYER_FIDUCIAL_REQ_GET_GEOM;
- hdr->device_index = device_id.index;
-
- assert(*resp_len == sizeof(player_laser_geom_t));
- player_laser_geom_t lgeom = * reinterpret_cast<player_laser_geom_t * >
(resp_data);
- player_fiducial_geom_t * fgeom =
reinterpret_cast<player_fiducial_geom_t * > (resp_data);
-
- fgeom->pose[0] = lgeom.pose[0];
- fgeom->pose[1] = lgeom.pose[1];
- fgeom->pose[2] = lgeom.pose[2];
- fgeom->size[0] = lgeom.size[0];
- fgeom->size[1] = lgeom.size[1];
- fgeom->fiducial_size[0] = ntohs((int) (0.05 * 1000));
- fgeom->fiducial_size[1] = ntohs((int) (this->bit_count * this->bit_width
* 1000));
-
- *resp_len=sizeof(player_fiducial_geom_t);
-
- return ret;
- }*/
return -1;
}
////////////////////////////////////////////////////////////////////////////////
- // Get the laser data
- /*int LaserBarcode::ReadLaser()
- {
- size_t size;
-
- // Get the laser data.
- size = this->laser_driver->GetData(this->laser_id,
- (uint8_t*)&this->laser_data,
- sizeof(this->laser_data),
- &this->laser_timestamp);
- assert(size <= sizeof(this->laser_data));
-
- // Do some byte swapping
- this->laser_data.resolution = ntohs(this->laser_data.resolution);
- this->laser_data.range_res = ntohs(this->laser_data.range_res);
- this->laser_data.min_angle = ntohs(this->laser_data.min_angle);
- this->laser_data.max_angle = ntohs(this->laser_data.max_angle);
- this->laser_data.range_count = ntohs(this->laser_data.range_count);
- for (int i = 0; i < this->laser_data.range_count; i++)
- this->laser_data.ranges[i] = ntohs(this->laser_data.ranges[i]);
-
- return 0;
- }*/
-
-
-
////////////////////////////////////////////////////////////////////////////////
// Analyze the laser data and return beacon data
void LaserBarcode::FindBeacons(const player_laser_data_t *laser_data,
--- 295,302 ----
***************
*** 413,420 ****
if (id < 0)
continue;
!
! // Check for array overflow.
! if (data->fiducials_count >= ARRAYSIZE(data->fiducials))
! continue;
double ox = (bx + ax) / 2;
--- 369,378 ----
if (id < 0)
continue;
!
! if (this->data.fiducials_count+1 > this->fdata_allocated)
! {
! this->fdata_allocated = this->data.fiducials_count+1;
! this->data.fiducials =
(player_fiducial_item_t*)realloc(this->data.fiducials,
sizeof(this->data.fiducials[0])*this->fdata_allocated);
! }
double ox = (bx + ax) / 2;
***************
*** 546,621 ****
{
// Write the data with the laser timestamp
! this->Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN,
&this->data, sizeof(this->data));
return;
}
-
-
////////////////////////////////////////////////////////////////////////////////
- // Process configuration requests
- /*int LaserBarcode::HandleConfig()
- {
- int subtype;
- size_t len;
- void *client;
- char req[PLAYER_MAX_REQREP_SIZE];
-
- while ((len = this->GetConfig(&client, req, sizeof(req),NULL)) > 0)
- {
- subtype = ((uint8_t*) req)[0];
-
- switch (subtype)
- {
- case PLAYER_FIDUCIAL_REQ_GET_GEOM:
- {
- HandleGetGeom(client, req, len);
- break;
- }
- default:
- {
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- break;
- }
- }
- }
-
- return(0);
- }
-
-
-
////////////////////////////////////////////////////////////////////////////////
- // Handle geometry requests.
- void LaserBarcode::HandleGetGeom(void *client, void *request, size_t len)
- {
- unsigned short reptype;
- struct timeval ts;
- int replen;
- player_laser_geom_t lgeom;
- player_fiducial_geom_t fgeom;
-
- // Get the geometry from the laser
- replen = this->laser_driver->Request(this->laser_id, this,
- request, len, NULL,
- &reptype, &lgeom, sizeof(lgeom), &ts);
- if (replen <= 0 || replen != sizeof(lgeom))
- {
- PLAYER_ERROR("unable to get geometry from laser device");
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- }
-
- fgeom.pose[0] = lgeom.pose[0];
- fgeom.pose[1] = lgeom.pose[1];
- fgeom.pose[2] = lgeom.pose[2];
- fgeom.size[0] = lgeom.size[0];
- fgeom.size[1] = lgeom.size[1];
- fgeom.fiducial_size[0] = ntohs((int) (0.05 * 1000));
- fgeom.fiducial_size[1] = ntohs((int) (this->bit_count * this->bit_width *
1000));
-
- if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, &fgeom, sizeof(fgeom), &ts)
!= 0)
- PLAYER_ERROR("PutReply() failed");
-
- return;
- }
- */
--- 504,510 ----
{
// Write the data with the laser timestamp
! this->Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN,
&this->data);
return;
}
Index: laserbar.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/fiducial/laserbar.cc,v
retrieving revision 1.21
retrieving revision 1.22
diff -C2 -d -r1.21 -r1.22
*** laserbar.cc 24 Oct 2007 22:32:04 -0000 1.21
--- laserbar.cc 1 Nov 2007 22:16:18 -0000 1.22
***************
*** 176,179 ****
--- 176,180 ----
private: struct timeval ftimestamp;
private: player_fiducial_data_t fdata;
+ unsigned int fdata_allocated;
};
***************
*** 217,221 ****
int LaserBar::Setup()
{
!
this->laser_device = deviceTable->GetDevice(this->laser_addr);
--- 218,223 ----
int LaserBar::Setup()
{
! fdata_allocated = 0;
! fdata.fiducials = NULL;
this->laser_device = deviceTable->GetDevice(this->laser_addr);
***************
*** 250,253 ****
--- 252,256 ----
this->laser_device = NULL;
+ free(fdata.fiducials);
return 0;
}
***************
*** 309,322 ****
this->Unlock();
- uint32_t size = sizeof(this->fdata) - sizeof(this->fdata.fiducials) +
- this->fdata.fiducials_count * sizeof(this->fdata.fiducials[0]);
-
printf("Count[%d]\n",this->fdata.fiducials_count);
this->Publish(this->device_addr,
PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN,
! reinterpret_cast<void*>(&this->fdata),
! size, &hdr->timestamp);
!
return 0;
--- 312,320 ----
this->Unlock();
printf("Count[%d]\n",this->fdata.fiducials_count);
this->Publish(this->device_addr,
PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN,
! reinterpret_cast<void*>(&this->fdata),0, &hdr->timestamp);
return 0;
***************
*** 350,354 ****
this->Publish(this->device_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_FIDUCIAL_REQ_GET_GEOM,
! (void*)&fgeom, sizeof(fgeom), NULL);
return 0;
--- 348,352 ----
this->Publish(this->device_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_FIDUCIAL_REQ_GET_GEOM,
! (void*)&fgeom);
return 0;
***************
*** 359,492 ****
-
-
////////////////////////////////////////////////////////////////////////////////
- // Get data from buffer (called by server thread).
- /*size_t
- LaserBar::GetData(player_device_id_t id,
- void* dest, size_t len,
- struct timeval* timestamp)
- {
- int i;
- size_t laser_size;
- struct timeval laser_timestamp;
-
- // Get the laser data.
- laser_size = this->laser_driver->GetData(this->laser_id,
- (void*) &this->ldata,
- sizeof(this->ldata),
- &laser_timestamp);
- assert(laser_size <= sizeof(this->ldata));
-
- // If the laser doesnt have new data, just return a copy of our old
- // data.
- if ((laser_timestamp.tv_sec != this->ftimestamp.tv_sec) ||
- (laser_timestamp.tv_usec != this->ftimestamp.tv_usec))
- {
- // Do some byte swapping
- this->ldata.resolution = ntohs(this->ldata.resolution);
- this->ldata.range_res = ntohs(this->ldata.range_res);
- this->ldata.min_angle = ntohs(this->ldata.min_angle);
- this->ldata.max_angle = ntohs(this->ldata.max_angle);
- this->ldata.range_count = ntohs(this->ldata.range_count);
- for (i = 0; i < this->ldata.range_count; i++)
- this->ldata.ranges[i] = ntohs(this->ldata.ranges[i]);
-
- // Analyse the laser data
- this->Find();
-
- // Do some byte-swapping on the fiducial data.
- for (i = 0; i < this->fdata.count; i++)
- {
- this->fdata.fiducials[i].pos[0] =
htonl(this->fdata.fiducials[i].pos[0]);
- this->fdata.fiducials[i].pos[1] =
htonl(this->fdata.fiducials[i].pos[1]);
- this->fdata.fiducials[i].rot[2] =
htonl(this->fdata.fiducials[i].rot[2]);
- }
- this->fdata.count = htons(this->fdata.count);
- }
-
- // Copy results
- assert(len >= sizeof(this->fdata));
- memcpy(dest, &this->fdata, sizeof(this->fdata));
-
- // Copy the laser timestamp
- *timestamp = laser_timestamp;
-
- this->ftimestamp = laser_timestamp;
-
- return (sizeof(this->fdata));
- }
- */
-
-
////////////////////////////////////////////////////////////////////////////////
- // Put configuration in buffer (called by server thread)
- /*int
- LaserBar::PutConfig(player_device_id_t id, void *client,
- void *src, size_t len,
- struct timeval* timestamp)
- {
- int subtype;
-
- if (len < 1)
- {
- PLAYER_ERROR("empty requestion; ignoring");
- return 0;
- }
-
- subtype = ((uint8_t*) src)[0];
- switch (subtype)
- {
- case PLAYER_FIDUCIAL_REQ_GET_GEOM:
- {
- HandleGetGeom(client, src, len);
- break;
- }
- default:
- {
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- break;
- }
- }
-
- return (0);
- }*/
-
-
-
////////////////////////////////////////////////////////////////////////////////
- // Handle geometry requests.
- /*void LaserBar::HandleGetGeom(void *client, void *request, int len)
- {
- unsigned short reptype;
- struct timeval ts;
- int replen;
- player_laser_geom_t lgeom;
- player_fiducial_geom_t fgeom;
-
- // Get the geometry from the laser
- replen = this->laser_driver->Request(this->laser_id, this,
- request, len, NULL,
- &reptype, &lgeom, sizeof(lgeom), &ts);
- if (replen <= 0 || replen != sizeof(lgeom))
- {
- PLAYER_ERROR("unable to get geometry from laser device");
- if (PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL) != 0)
- PLAYER_ERROR("PutReply() failed");
- }
-
- fgeom.pose[0] = lgeom.pose[0];
- fgeom.pose[1] = lgeom.pose[1];
- fgeom.pose[2] = lgeom.pose[2];
- fgeom.size[0] = lgeom.size[0];
- fgeom.size[1] = lgeom.size[1];
- fgeom.fiducial_size[0] = ntohs((int) (this->reflector_width * 1000));
- fgeom.fiducial_size[1] = ntohs((int) (this->reflector_width * 1000));
-
- if (PutReply(client, PLAYER_MSGTYPE_RESP_ACK, &fgeom, sizeof(fgeom),&ts) !=
0)
- PLAYER_ERROR("PutReply() failed");
-
- return;
- }
- */
-
////////////////////////////////////////////////////////////////////////////////
// Analyze the laser data to find reflectors.
--- 357,360 ----
***************
*** 630,639 ****
{
player_fiducial_item_t *fiducial;
!
! //printf("adding %f %f %f\n", pr, pb, po);
!
! //assert(this->fdata.count < ARRAYSIZE(this->fdata.fiducials));
!
! fiducial = this->fdata.fiducials + this->fdata.fiducials_count++;
fiducial->id = (int16_t) -1;
--- 498,508 ----
{
player_fiducial_item_t *fiducial;
! this->fdata.fiducials_count++;
! if (this->fdata.fiducials_count > this->fdata_allocated)
! {
! this->fdata_allocated = this->fdata.fiducials_count;
! this->fdata.fiducials =
(player_fiducial_item_t*)realloc(this->fdata.fiducials,
sizeof(this->fdata.fiducials[0])*this->fdata_allocated);
! }
! fiducial = &this->fdata.fiducials[fdata.fiducials_count-1];
fiducial->id = (int16_t) -1;
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
