Revision: 7802
http://playerstage.svn.sourceforge.net/playerstage/?rev=7802&view=rev
Author: thjc
Date: 2009-06-06 09:51:04 +0000 (Sat, 06 Jun 2009)
Log Message:
-----------
applied patch 2609220: Re enable fiducial interface in writelog and readlog
drivers
Modified Paths:
--------------
code/player/trunk/server/drivers/shell/readlog.cc
code/player/trunk/server/drivers/shell/writelog.cc
Modified: code/player/trunk/server/drivers/shell/readlog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/readlog.cc 2009-06-06 09:39:40 UTC
(rev 7801)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2009-06-06 09:51:04 UTC
(rev 7802)
@@ -66,13 +66,13 @@
- @ref interface_opaque
- @ref interface_ptz
- @ref interface_actarray
+- @ref interface_fiducial
The following interfaces are supported in principle but are currently
disabled because they need to be updated:
- @ref interface_blobfinder
- @ref interface_camera
-- @ref interface_fiducial
- @ref interface_gps
- @ref interface_joystick
- @ref interface_position3d
@@ -195,6 +195,11 @@
player_msghdr_t * hdr,
void * data);
+ // Process fiducial interface configuration requests
+ private: int ProcessFiducialConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data);
+
// Process laser interface configuration requests
private: int ProcessLaserConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
@@ -225,6 +230,12 @@
int linenum, int token_count, char **tokens,
double time);
+ // Parse fiducial data
+ private: int ParseFiducial(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum, int token_count, char **tokens,
+ double time);
+
#if 0
// Parse blobfinder data
private: int ParseBlobfinder(player_devaddr_t id,
@@ -238,12 +249,6 @@
int linenum,
int token_count, char **tokens, double time);
- // Parse fiducial data
- private: int ParseFiducial(player_devaddr_t id,
- unsigned short type, unsigned short subtype,
- int linenum,
- int token_count, char **tokens, double time);
-
// Parse gps data
private: int ParseGps(player_devaddr_t id,
unsigned short type, unsigned short subtype,
@@ -938,6 +943,46 @@
}
int
+ReadLog::ProcessFiducialConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data)
+{
+ switch(hdr->subtype)
+ {
+ case PLAYER_FIDUCIAL_REQ_GET_GEOM:
+ {
+ // Find the right place from which to retrieve it
+ int j;
+ for(j=0;j<this->provide_count;j++)
+ {
+ if(Device::MatchDeviceAddress(this->provide_ids[j], hdr->addr))
+ break;
+ }
+ if(j>=this->provide_count)
+ {
+ puts("no matching device");
+ return(-1);
+ }
+
+ if(!this->provide_metadata[j])
+ {
+ puts("no metadata");
+ return(-1);
+ }
+
+ this->Publish(this->provide_ids[j], resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
+ this->provide_metadata[j],
+ sizeof(player_fiducial_geom_t),
+ NULL);
+ return(0);
+ }
+ default:
+ return(-1);
+ }
+}
+
+int
ReadLog::ProcessLaserConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
void * data)
@@ -1091,6 +1136,11 @@
return(this->ProcessLogConfig(resp_queue, hdr, data));
}
else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
+ (hdr->addr.interf == PLAYER_FIDUCIAL_CODE))
+ {
+ return(this->ProcessFiducialConfig(resp_queue, hdr, data));
+ }
+ else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
(hdr->addr.interf == PLAYER_LASER_CODE))
{
return(this->ProcessLaserConfig(resp_queue, hdr, data));
@@ -1197,9 +1247,6 @@
else if (id.interf == PLAYER_CAMERA_CODE)
return this->ParseCamera(id, type, subtype, linenum,
token_count, tokens, time);
- else if (id.interf == PLAYER_FIDUCIAL_CODE)
- return this->ParseFiducial(id, type, subtype, linenum,
- token_count, tokens, time);
else if (id.interf == PLAYER_GPS_CODE)
return this->ParseGps(id, type, subtype, linenum,
token_count, tokens, time);
@@ -1210,6 +1257,9 @@
if (id.interf == PLAYER_LASER_CODE)
return this->ParseLaser(id, type, subtype, linenum,
token_count, tokens, time);
+ else if (id.interf == PLAYER_FIDUCIAL_CODE)
+ return this->ParseFiducial(id, type, subtype, linenum,
+ token_count, tokens, time);
else if (id.interf == PLAYER_LOCALIZE_CODE)
return this->ParseLocalize(id, type, subtype, linenum,
token_count, tokens, time);
@@ -1350,48 +1400,114 @@
return 0;
}
+#endif
-
////////////////////////////////////////////////////////////////////////////
// Parse fiducial data
-int ReadLog::ParseFiducial(player_devaddr_t id, int linenum,
- int token_count, char **tokens, struct timeval
time)
+int ReadLog::ParseFiducial(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum, int token_count, char **tokens,
+ double time)
{
- player_fiducial_data_t data;
- int fiducial_count;
-
if (token_count < 7)
{
PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
return -1;
}
- fiducial_count = atoi( tokens[6] );
- data.count = NUINT16( fiducial_count );
+ switch(type) {
+ case PLAYER_MSGTYPE_DATA:
+ switch (subtype) {
+ case PLAYER_FIDUCIAL_DATA_SCAN:
+ player_fiducial_data_t data;
+ int fiducial_count;
- for( int i = 0; i < fiducial_count; i++ )
- {
- data.fiducials[i].id = NINT16( atof(tokens[13*i + 7]) );
- data.fiducials[i].pos[0] = NINT32(M_MM(atof(tokens[13*i+ 8])));
- data.fiducials[i].pos[1] = NINT32(M_MM(atof(tokens[13*i+ 9])));
- data.fiducials[i].pos[2] = NINT32(M_MM(atof(tokens[13*i+10])));
- data.fiducials[i].rot[0] = NINT32(M_MM(atof(tokens[13*i+11])));
- data.fiducials[i].rot[1] = NINT32(M_MM(atof(tokens[13*i+12])));
- data.fiducials[i].rot[2] = NINT32(M_MM(atof(tokens[13*i+13])));
- data.fiducials[i].upos[0] = NINT32(M_MM(atof(tokens[13*i+14])));
- data.fiducials[i].upos[1] = NINT32(M_MM(atof(tokens[13*i+15])));
- data.fiducials[i].upos[2] = NINT32(M_MM(atof(tokens[13*i+16])));
- data.fiducials[i].urot[0] = NINT32(M_MM(atof(tokens[13*i+17])));
- data.fiducials[i].urot[1] = NINT32(M_MM(atof(tokens[13*i+18])));
- data.fiducials[i].urot[2] = NINT32(M_MM(atof(tokens[13*i+19])));
+ fiducial_count = atoi(tokens[7]);
+
+ data.fiducials_count = fiducial_count;
+ //data.fiducials = (player_fiducial_item_t *)
malloc(data.fiducials_count * sizeof(data.fiducials[0]));
+ data.fiducials = new
player_fiducial_item_t[data.fiducials_count];
+
+ for (int i = 0; i < fiducial_count; i++) {
+ data.fiducials[i].id = atof(tokens[13 * i + 8]);
+ data.fiducials[i].pose.px = atof(tokens[13 * i + 9]);
+ data.fiducials[i].pose.py = atof(tokens[13 * i + 10]);
+ data.fiducials[i].pose.pz = atof(tokens[13 * i + 11]);
+ data.fiducials[i].pose.proll = atof(tokens[13 * i +
12]);
+ data.fiducials[i].pose.ppitch = atof(tokens[13 * i +
13]);
+ data.fiducials[i].pose.pyaw = atof(tokens[13 * i +
14]);
+ data.fiducials[i].upose.px = atof(tokens[13 * i + 15]);
+ data.fiducials[i].upose.py = atof(tokens[13 * i + 16]);
+ data.fiducials[i].upose.pz = atof(tokens[13 * i + 17]);
+ data.fiducials[i].upose.proll = atof(tokens[13 * i +
18]);
+ data.fiducials[i].upose.ppitch = atof(tokens[13 * i +
19]);
+ data.fiducials[i].upose.pyaw = atof(tokens[13 * i +
20]);
+ }
+ this->Publish(id, type, subtype,
+ (void*) & data, sizeof (data), &time);
+
+ delete [] data.fiducials;
+ return (0);
+ default:
+ PLAYER_ERROR1("unimplemented fiducial data subtype %d\n",
subtype);
+ return(-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch (subtype) {
+ case PLAYER_FIDUCIAL_REQ_GET_GEOM:
+
+ if (token_count < 17) {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+ player_fiducial_geom_t* geom;
+
+ geom = (player_fiducial_geom_t*) calloc(1, sizeof
(player_fiducial_geom_t));
+ assert(geom);
+
+ geom->pose.px = atof(tokens[7]);
+ geom->pose.py = atof(tokens[8]);
+ geom->pose.pz = atof(tokens[9]);
+ geom->pose.proll = atof(tokens[10]);
+ geom->pose.ppitch = atof(tokens[11]);
+ geom->pose.pyaw = atof(tokens[12]);
+ geom->size.sl = atof(tokens[13]);
+ geom->size.sw = atof(tokens[14]);
+ geom->size.sh = atof(tokens[15]);
+ geom->fiducial_size.sl = atof(tokens[16]);
+ geom->fiducial_size.sw = atof(tokens[17]);
+
+ // Find the right place to put it
+ int j;
+ for (j = 0; j<this->provide_count; j++) {
+ if (Device::MatchDeviceAddress(this->provide_ids[j],
id))
+ break;
+ }
+ assert(j<this->provide_count);
+
+ if (this->provide_metadata[j])
+ free(this->provide_metadata[j]);
+
+ this->provide_metadata[j] = (void*) geom;
+
+ // nothing to publish
+ return (0);
+
+ default:
+ PLAYER_ERROR1("unimplemented fiducial data subtype %d\n",
subtype);
+ return(-1);
+ }
+ default:
+ PLAYER_ERROR1("unimplemented fiducial data type %d\n", type);
+ return (-1);
+
}
- this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, sizeof(data), &time);
-
return 0;
}
-
+#if 0
////////////////////////////////////////////////////////////////////////////
// Parse GPS data
int ReadLog::ParseGps(player_devaddr_t id, int linenum,
Modified: code/player/trunk/server/drivers/shell/writelog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/writelog.cc 2009-06-06 09:39:40 UTC
(rev 7801)
+++ code/player/trunk/server/drivers/shell/writelog.cc 2009-06-06 09:51:04 UTC
(rev 7802)
@@ -80,12 +80,12 @@
- @ref interface_pointcloud3d
- @ref interface_actarray
- @ref interface_camera
+- @ref interface_fiducial
The following interfaces are supported in principle but are currently
disabled because they need to be updated:
- @ref interface_blobfinder
-- @ref interface_fiducial
- @ref interface_gps
- @ref interface_joystick
- @ref interface_position3d
@@ -274,13 +274,13 @@
// Write camera data to file
private: int WriteCamera(WriteLogDevice *device, player_msghdr_t* hdr, void
*data);
+
+ // Write fiducial data to file
+ private: int WriteFiducial(player_msghdr_t* hdr, void *data);
#if 0
// Write blobfinder data to file
private: void WriteBlobfinder(player_blobfinder_data_t *data);
- // Write fiducial data to file
- private: void WriteFiducial(player_fiducial_data_t *data);
-
// Write GPS data to file
private: void WriteGps(player_gps_data_t *data);
@@ -876,13 +876,13 @@
case PLAYER_CAMERA_CODE:
retval = this->WriteCamera(device, hdr, data);
break;
+ case PLAYER_FIDUCIAL_CODE:
+ this->WriteFiducial(hdr, data);
+ break;
#if 0
case PLAYER_BLOBFINDER_CODE:
this->WriteBlobfinder((player_blobfinder_data_t*) data);
break;
- case PLAYER_FIDUCIAL_CODE:
- this->WriteFiducial((player_fiducial_data_t*) data);
- break;
case PLAYER_GPS_CODE:
this->WriteGps((player_gps_data_t*) data);
break;
@@ -2226,14 +2226,14 @@
return -1;
}
-#if 0
-
/** @ingroup tutorial_datalog
- * @defgroup player_driver_writelog_fiducial Fiducial format
+ * @defgroup player_driver_writelog_fiducial fiducial format
@brief fiducial log format
-The format for each @ref interface_fiducial message is:
+The following messages from @ref interface_fiducial interface are logged:
+
+PLAYER_MSGTYPE_DATA:PLAYER_FIDUCIAL_DATA_SCAN (1:1) has the folowing format:
- count (int): number of fiducials to follow
- list of fiducials; for each fiducial:
- id (int): fiducial ID
@@ -2250,34 +2250,83 @@
- upitch (float): uncertainty in relative pitch orientation, in radians
- uyaw (float): uncertainty in relative yaw orientation, in radians
+PLAYER_MSGTYPE_RESP_ACK:PLAYER_FIDUCIAL_REQ_GET_GEOM (4:1) has the following
format:
+ - x (float): relative X position, in meters
+ - y (float): relative Y position, in meters
+ - z (float): relative Z position, in meters
+ - roll (float): relative roll orientation, in radians
+ - pitch (float): relative pitch orientation, in radians
+ - yaw (float): relative yaw orientation, in radians
+ - length (float): fiducial finder length, in meters
+ - width (float): fiducial finder width, in meters
+ - height (float): fiducial finder height, in meters
+ - fiducial_length (float): fiducial marker length, in meters
+ - fiducial_width (float): fiducial marker width, in meters
*/
-void WriteLog::WriteFiducial(player_fiducial_data_t *data)
+int WriteLog::WriteFiducial(player_msghdr_t* hdr, void *data)
{
- // format: <count> [<id> <x> <y> <z> <roll> <pitch> <yaw> <ux> <uy> <uz>
etc] ...
- fprintf(this->file, "%d", HUINT16(data->count));
- for(int i=0;i<HUINT16(data->count);i++)
- {
- fprintf(this->file, " %d"
- " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f"
- " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f",
- HINT16(data->fiducials[i].id),
- MM_M(HINT32(data->fiducials[i].pos[0])),
- MM_M(HINT32(data->fiducials[i].pos[1])),
- MM_M(HINT32(data->fiducials[i].pos[2])),
- MM_M(HINT32(data->fiducials[i].rot[0])),
- MM_M(HINT32(data->fiducials[i].rot[1])),
- MM_M(HINT32(data->fiducials[i].rot[2])),
- MM_M(HINT32(data->fiducials[i].upos[0])),
- MM_M(HINT32(data->fiducials[i].upos[1])),
- MM_M(HINT32(data->fiducials[i].upos[2])),
- MM_M(HINT32(data->fiducials[i].urot[0])),
- MM_M(HINT32(data->fiducials[i].urot[1])),
- MM_M(HINT32(data->fiducials[i].urot[2])));
- }
+ player_fiducial_data_t *fiducial_data;
+ player_fiducial_geom_t *fiducial_geom;
- return;
+ switch (hdr->type) {
+ case PLAYER_MSGTYPE_DATA:
+ switch (hdr->subtype) {
+ case PLAYER_FIDUCIAL_DATA_SCAN:
+ fiducial_data = (player_fiducial_data_t*) data;
+ // format: <count> [<id> <x> <y> <z> <roll> <pitch> <yaw>
<ux> <uy> <uz> <uroll> <upitch> <uyaw>] ...
+ fprintf(this->file, "%d", fiducial_data->fiducials_count);
+ for (int i = 0; i < fiducial_data->fiducials_count; i++) {
+ fprintf(this->file, " %d"
+ " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f"
+ " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f",
+ fiducial_data->fiducials[i].id,
+ fiducial_data->fiducials[i].pose.px,
+ fiducial_data->fiducials[i].pose.py,
+ fiducial_data->fiducials[i].pose.pz,
+ fiducial_data->fiducials[i].pose.proll,
+ fiducial_data->fiducials[i].pose.ppitch,
+ fiducial_data->fiducials[i].pose.pyaw,
+ fiducial_data->fiducials[i].upose.px,
+ fiducial_data->fiducials[i].upose.py,
+ fiducial_data->fiducials[i].upose.pz,
+ fiducial_data->fiducials[i].upose.proll,
+ fiducial_data->fiducials[i].upose.ppitch,
+ fiducial_data->fiducials[i].upose.pyaw);
+ }
+ return(0);
+ default:
+ return(-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(hdr->subtype) {
+ case PLAYER_FIDUCIAL_REQ_GET_GEOM:
+ fiducial_geom = (player_fiducial_geom_t*) data;
+ //format: <x> <y> <z> <roll> <pitch> <yaw> <length> ...
+ // <width> <height> <fiducial_length> <fiducial_width>
+ fprintf(this->file, "%+7.3f %+7.3f %+7.3f %+7.3f %+7.3f"
+ "%+7.3f %+7.3f %+7.3f %+7.3f %+7.3f %+7.3f",
+ fiducial_geom->pose.px,
+ fiducial_geom->pose.py,
+ fiducial_geom->pose.pz,
+ fiducial_geom->pose.proll,
+ fiducial_geom->pose.ppitch,
+ fiducial_geom->pose.pyaw,
+ fiducial_geom->size.sl,
+ fiducial_geom->size.sw,
+ fiducial_geom->size.sh,
+ fiducial_geom->fiducial_size.sl,
+ fiducial_geom->fiducial_size.sw);
+
+ return(0);
+ default:
+ return(-1);
+ }
+ default:
+ return(-1);
+ }
}
+#if 0
/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_gps GPS format
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
OpenSolaris 2009.06 is a cutting edge operating system for enterprises
looking to deploy the next generation of Solaris that includes the latest
innovations from Sun and the OpenSource community. Download a copy and
enjoy capabilities such as Networking, Storage and Virtualization.
Go to: http://p.sf.net/sfu/opensolaris-get
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
