Revision: 8299
http://playerstage.svn.sourceforge.net/playerstage/?rev=8299&view=rev
Author: thjc
Date: 2009-10-18 13:50:43 +0000 (Sun, 18 Oct 2009)
Log Message:
-----------
Applid patch 2858751: writelog, readlog and dummy
Modified Paths:
--------------
code/player/trunk/server/drivers/shell/dummy.cc
code/player/trunk/server/drivers/shell/readlog.cc
code/player/trunk/server/drivers/shell/writelog.cc
Modified: code/player/trunk/server/drivers/shell/dummy.cc
===================================================================
--- code/player/trunk/server/drivers/shell/dummy.cc 2009-10-18 13:46:14 UTC
(rev 8298)
+++ code/player/trunk/server/drivers/shell/dummy.cc 2009-10-18 13:50:43 UTC
(rev 8299)
@@ -39,8 +39,26 @@
@par Provides
-- This driver can support any interface (currently supported are: laser,
camera,
- position2d, ptz, and wsn).
+This driver can theoretically support any interface. Currently supported
interfaces:
+- @ref interface_camera
+- @ref interface_laser
+- @ref interface_ranger
+- @ref interface_position2d
+- @ref interface_ptz
+- @ref interface_wsn
+- @ref interface_gps
+- @ref interface_position3d
+- @ref interface_blobfinder
+- @ref interface_joystick
+- @ref interface_power
+- @ref interface_pointcloud3d
+- @ref interface_imu
+- @ref interface_fiducial
+- @ref interface_wifi
+- @ref interface_actarray
+- @ref interface_opaque
+- @ref interface_dio
+- @ref interface_aio
@par Requires
@@ -67,7 +85,7 @@
)
@endverbatim
-...@author Andrew Howard, Radu Bogdan Rusu
+...@author Andrew Howard, Radu Bogdan Rusu, Rich Mattes
*/
/** @} */
@@ -89,6 +107,11 @@
// Destructor
~Dummy ();
+
+ // Process Messages (so we can NACK to all requests that come in)
+ virtual int ProcessMessage(QueuePointer & resp_queue,
+ player_msghdr * hdr,
+ void * data);
private:
@@ -160,6 +183,7 @@
{
unsigned int i = 0;
struct timespec req;
+ int loopcount = 0;
// void *client;
req.tv_sec = (time_t) (1.0 / this->rate);
@@ -171,7 +195,7 @@
if (nanosleep(&req, NULL) == -1)
continue;
-// ProcessMessages();
+ ProcessMessages();
// Process pending configuration requests
/* while (this->GetConfig(this->local_id, &client, this->req_buffer,
PLAYER_MAX_REQREP_SIZE, NULL))
@@ -195,22 +219,31 @@
data.format = PLAYER_CAMERA_FORMAT_RGB888;
data.compression = PLAYER_CAMERA_COMPRESS_RAW;
data.image_count = w * h * 3;
-
+
+ data.image = new uint8_t[data.image_count];
+
+
+
for (int j = 0; j < h; j++)
{
for (int i = 0; i < w; i++)
{
- data.image[(i + j * w) * 3 + 0] = ((i + j) % 2) * 255;
- data.image[(i + j * w) * 3 + 1] = ((i + j) % 2) * 255;
- data.image[(i + j * w) * 3 + 2] = ((i + j) % 2) * 255;
+ data.image[(i + j * w) * 3 + 0] = loopcount;//((i + j)
% 2) * 255;
+ data.image[(i + j * w) * 3 + 1] = loopcount;//((i + j)
% 2) * 255;
+ data.image[(i + j * w) * 3 + 2] = loopcount;//((i + j)
% 2) * 255;
}
}
- int data_len = sizeof(data) - sizeof(data.image) + w * h * 3;
+ int data_len = sizeof(data);// - sizeof(data.image) + w * h *
3;
Publish (device_addr, PLAYER_MSGTYPE_DATA,
PLAYER_CAMERA_DATA_STATE, (void*)&data, data_len,
NULL);
+ delete[] data.image;
+ loopcount++;
+ if (loopcount > 255)
+ loopcount = 0;
+
break;
}
case PLAYER_LASER_CODE:
@@ -242,6 +275,25 @@
free(data.intensity);
break;
}
+ case PLAYER_RANGER_CODE:
+ {
+ // Printing simple ranger data
+ player_ranger_data_range data;
+ data.ranges_count = 361;
+ data.ranges = (double *)
+ malloc( data.ranges_count * sizeof(double) );
+
+ for (i = 0; i < data.ranges_count; i++)
+ {
+ data.ranges[i] = 8;
+ }
+
+ Publish (device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_RANGER_DATA_RANGE, (void*)&data, sizeof(data),
+ NULL);
+ free(data.ranges);
+ break;
+ }
case PLAYER_POSITION2D_CODE:
{
player_position2d_data_t data;
@@ -293,7 +345,282 @@
sizeof (data), NULL);
break;
}
+ case PLAYER_GPS_CODE:
+ {
+ player_gps_data_t data;
+ data.time_sec = 1234567890;
+ data.time_usec = 1;
+ data.latitude = 1e7;
+ data.longitude = 1e7;
+ data.altitude = 1e3;
+ data.utm_e = 10.0;
+ data.utm_n = 10.0;
+ data.quality = 2;
+ data.num_sats = 7;
+ data.hdop = 10;
+ data.vdop = 10;
+ data.err_horz = 1.0;
+ data.err_vert = 1.0;
+ Publish (device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_GPS_DATA_STATE, (void*)&data,
+ sizeof (data), NULL);
+ break;
+ }
+ case PLAYER_POSITION3D_CODE:
+ {
+ player_position3d_data_t data;
+ data.pos.px = 1.0;
+ data.pos.py = 1.0;
+ data.pos.pz = 1.0;
+ data.pos.proll = 1.0;
+ data.pos.ppitch = 1.0;
+ data.pos.pyaw = 1.0;
+ data.vel.px = 1.0;
+ data.vel.py = 1.0;
+ data.vel.pz = 1.0;
+ data.vel.proll = 0;
+ data.vel.ppitch = 0;
+ data.vel.pyaw = 0;
+ data.stall = 0;
+ Publish (device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_POSITION3D_DATA_STATE, (void*)&data,
+ sizeof (data), NULL);
+ break;
+ }
+ case PLAYER_JOYSTICK_CODE:
+ {
+ player_joystick_data_t data;
+ data.pos[0] = 1.0;
+ data.pos[1] = 1.0;
+ data.pos[2] = 1.0;
+ data.scale[0] = 1.0;
+ data.scale[1] = 1.0;
+ data.scale[2] = 1.0;
+ data.buttons = 7;
+ Publish (device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_JOYSTICK_DATA_STATE, (void*)&data,
+ sizeof (data), NULL);
+ break;
+ }
+ case PLAYER_BLOBFINDER_CODE:
+ {
+ player_blobfinder_data_t data;
+ data.width = 320;
+ data.height = 240;
+ data.blobs_count = 2;
+ data.blobs = new player_blobfinder_blob_t[data.blobs_count];
+ for(int b=0; b < (int)data.blobs_count; b++)
+ {
+ data.blobs[b].id = b;
+ data.blobs[b].color = b;
+ data.blobs[b].area = b;
+ data.blobs[b].x = b;
+ data.blobs[b].y = b;
+ data.blobs[b].left = b;
+ data.blobs[b].right = b;
+ data.blobs[b].top = b;
+ data.blobs[b].bottom = b;
+ data.blobs[b].range = b;
+ }
+
+ Publish (device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_BLOBFINDER_DATA_BLOBS, (void*)&data,
+ sizeof(data), NULL);
+
+ delete[] data.blobs;
+
+ break;
+ }
+ case PLAYER_POWER_CODE:
+ {
+ player_power_data_t data;
+ data.valid = 0xFF;
+ data.volts = 1.0;
+ data.percent = 1.0;
+ data.joules = 1.0;
+ data.watts = 1.0;
+ data.charging = 1;
+ Publish (device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_POWER_DATA_STATE, (void*)&data,
+ sizeof (data), NULL);
+ break;
+ }
+ case PLAYER_POINTCLOUD3D_CODE:
+ {
+ player_pointcloud3d_data_t data;
+ data.points_count = 10;
+ data.points = new
player_pointcloud3d_element_t[data.points_count];
+ for (int i=0; i<(int)data.points_count;i++)
+ {
+ data.points[i].point.px = i;
+ data.points[i].point.py = i;
+ data.points[i].point.pz = i;
+
+ data.points[i].color.alpha = 0;
+ data.points[i].color.red = 128;
+ data.points[i].color.green = 128;
+ data.points[i].color.blue = 128;
+ }
+
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_POINTCLOUD3D_DATA_STATE, (void*) &data,
+ sizeof(data) , NULL);
+ delete[] data.points;
+
+ break;
+ }
+ case PLAYER_IMU_CODE:
+ {
+ player_imu_data_state_t data;
+
+ data.pose.px = 1;
+ data.pose.py = 1;
+ data.pose.pz = 1;
+ data.pose.proll = 1;
+ data.pose.ppitch = 1;
+ data.pose.pyaw = 1;
+
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_IMU_DATA_STATE, (void*) &data,
+ sizeof(data), NULL);
+ break;
+ }
+ case PLAYER_FIDUCIAL_CODE:
+ {
+ player_fiducial_data_t data;
+ data.fiducials_count = 5;
+
+ data.fiducials = new
player_fiducial_item_t[data.fiducials_count];
+ for (int i=0;i<(int)data.fiducials_count;i++)
+ {
+ data.fiducials[i].id = i;
+ data.fiducials[i].pose.px = i;
+ data.fiducials[i].pose.py = i;
+ data.fiducials[i].pose.pz = i;
+ data.fiducials[i].pose.proll = i;
+ data.fiducials[i].pose.ppitch = i;
+ data.fiducials[i].pose.pyaw = i;
+ data.fiducials[i].upose.px = i;
+ data.fiducials[i].upose.py = i;
+ data.fiducials[i].upose.pz = i;
+ data.fiducials[i].upose.proll = i;
+ data.fiducials[i].upose.ppitch = i;
+ data.fiducials[i].upose.pyaw = i;
+ }
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_FIDUCIAL_DATA_SCAN, (void*)&data,
+ sizeof(data), NULL);
+ delete [] data.fiducials;
+ break;
+ }
+ case PLAYER_WIFI_CODE:
+ {
+ player_wifi_data_t data;
+ data.links_count = 2;
+ data.throughput = 1;
+ data.bitrate = 54;
+ data.mode = 1;
+ data.qual_type = 1;
+ data.maxqual = 1;
+ data.maxlevel = 1;
+ data.maxnoise = 10;
+ strcpy(data.ap, "AccessPoint");
+
+ data.links = new player_wifi_link_t[data.links_count];
+ for (int i=0;i<(int)data.links_count;i++)
+ {
+ data.links[i].mac_count = 18;
+ memcpy(data.links[i].mac, "00:11:22:33:44:55\0",
sizeof(char)*18);
+ data.links[i].ip_count = 10;
+ memcpy(data.links[i].ip,"127.0.0.1\0",sizeof(char)*10);
+ data.links[i].essid_count = 6;
+ memcpy(data.links[i].essid, "ESSID\0", sizeof(char)*6);
+ data.links[i].mode = 1;
+ data.links[i].freq = 1;
+ data.links[i].encrypt = 1;
+ data.links[i].qual = 1;
+ data.links[i].level = 1;
+ data.links[i].noise = 1;
+ }
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_WIFI_DATA_STATE, (void*)&data,
+ sizeof(data), NULL);
+ delete [] data.links;
+ break;
+
+ }
+ case PLAYER_ACTARRAY_CODE:
+ {
+ player_actarray_data_t data;
+ data.actuators_count = 2;
+ data.motor_state = 1;
+ data.actuators = new
player_actarray_actuator_t[data.actuators_count];
+ for (int i=0;i<(int)data.actuators_count;i++)
+ {
+ data.actuators[i].position = 1;
+ data.actuators[i].speed = 1;
+ data.actuators[i].acceleration = 1;
+ data.actuators[i].current = 1;
+ data.actuators[i].state = 1;
+ }
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_ACTARRAY_DATA_STATE, (void*)&data,
+ sizeof(data), NULL);
+ delete[] data.actuators;
+ break;
+ }
+ case PLAYER_OPAQUE_CODE:
+ {
+ player_opaque_data_t data;
+ data.data_count = 8;
+ data.data = new uint8_t[data.data_count];
+ for(int i=0;i<(int)data.data_count;i++)
+ {
+ data.data[i] = i;
+ }
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_OPAQUE_DATA_STATE, (void*)&data, sizeof(data),
NULL);
+ delete [] data.data;
+ break;
+ }
+ case PLAYER_DIO_CODE:
+ {
+ player_dio_data_t data;
+ data.count = 8;
+ data.bits = 0xAA;
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_DIO_DATA_VALUES, (void*)&data, sizeof(data),
NULL);
+ break;
+ }
+ case PLAYER_AIO_CODE:
+ {
+ player_aio_data_t data;
+ data.voltages_count = 5;
+ data.voltages = new float[data.voltages_count];
+ for (int i=0; i<(int)data.voltages_count;i++)
+ {
+ data.voltages[i] = (float)i;
+ }
+ Publish(device_addr, PLAYER_MSGTYPE_DATA,
+ PLAYER_AIO_DATA_STATE, (void*)&data, sizeof(data),
NULL);
+ delete[] data.voltages;
+ break;
+ }
}
}
return;
}
+////////////////////////////////////////////////////////////////////////////////
+// Process Message function to deny config requests.
+int Dummy::ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr,void
* data)
+{
+ switch (hdr->type){
+ case PLAYER_MSGTYPE_REQ:
+ hdr->type = PLAYER_MSGTYPE_RESP_NACK;
+ this->Publish(resp_queue, hdr, data);
+ return (0);
+ default:
+ return (-1);
+ }
+ return (-1);
+}
Modified: code/player/trunk/server/drivers/shell/readlog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/readlog.cc 2009-10-18 13:46:14 UTC
(rev 8298)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2009-10-18 13:50:43 UTC
(rev 8299)
@@ -68,15 +68,14 @@
- @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_gps
- @ref interface_joystick
- @ref interface_position3d
+- @ref interface_power
+- @ref interface_dio
+- @ref interface_aio
The driver also provides an interface for controlling the playback:
@@ -124,7 +123,7 @@
)
@endverbatim
-...@author Andrew Howard, Radu Bogdan Rusu
+...@author Andrew Howard, Radu Bogdan Rusu, Rich Mattes
*/
/** @} */
@@ -197,6 +196,11 @@
private: int ProcessPositionConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
void * data);
+
+ // Process position3d interface configuration requests
+ private: int ProcessPosition3dConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data);
// Process fiducial interface configuration requests
private: int ProcessFiducialConfig(QueuePointer & resp_queue,
@@ -244,31 +248,27 @@
int linenum, int token_count, char **tokens,
double time);
-#if 0
// Parse blobfinder data
private: int ParseBlobfinder(player_devaddr_t id,
unsigned short type, unsigned short subtype,
int linenum,
int token_count, char **tokens, double time);
-
// Parse camera data
private: int ParseCamera(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,
int linenum,
int token_count, char **tokens, double time);
-
+
// Parse joystick data
private: int ParseJoystick(player_devaddr_t id,
unsigned short type, unsigned short subtype,
int linenum,
int token_count, char **tokens, double time);
-#endif
// Parse laser data
private: int ParseLaser(player_devaddr_t id,
@@ -357,16 +357,18 @@
unsigned short type, unsigned short subtype,
int linenum, int token_count, char **tokens,
double time);
-#if 0
// Parse position3d data
private: int ParsePosition3d(player_devaddr_t id,
unsigned short type, unsigned short subtype,
int linenum,
int token_count, char **tokens, double time);
+ // Parse power data
+ private: int ParsePower(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time);
-#endif
-
// List of provided devices
private: int provide_count;
private: player_devaddr_t provide_ids[1024];
@@ -963,6 +965,40 @@
}
int
+ReadLog::ProcessPosition3dConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data)
+{
+ switch(hdr->subtype)
+ {
+ case PLAYER_POSITION3D_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)
+ return(-1);
+
+ if(!this->provide_metadata[j])
+ return(-1);
+
+ this->Publish(this->provide_ids[j], resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK, hdr->subtype,
+ this->provide_metadata[j],
+ sizeof(player_position3d_geom_t),
+ NULL);
+ return(0);
+ }
+ default:
+ return(-1);
+ }
+}
+
+int
ReadLog::ProcessFiducialConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
void * data)
@@ -1260,6 +1296,11 @@
{
return(this->ProcessPositionConfig(resp_queue, hdr, data));
}
+ else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
+ (hdr->addr.interf == PLAYER_POSITION3D_CODE))
+ {
+ return(this->ProcessPosition3dConfig(resp_queue, hdr, data));
+ }
else if(particles_set &&
Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_LOCALIZE_REQ_GET_PARTICLES,
@@ -1335,10 +1376,9 @@
int linenum, int token_count, char **tokens,
double time)
{
-#if 0
if (id.interf == PLAYER_BLOBFINDER_CODE)
return this->ParseBlobfinder(id, type, subtype, linenum,
- token_count, tokens, time);
+ token_count, tokens, time);
else if (id.interf == PLAYER_CAMERA_CODE)
return this->ParseCamera(id, type, subtype, linenum,
token_count, tokens, time);
@@ -1348,11 +1388,10 @@
else if (id.interf == PLAYER_JOYSTICK_CODE)
return this->ParseJoystick(id, type, subtype, linenum,
token_count, tokens, time);
-#endif
- if (id.interf == PLAYER_LASER_CODE)
+ else if (id.interf == PLAYER_LASER_CODE)
return this->ParseLaser(id, type, subtype, linenum,
token_count, tokens, time);
- if (id.interf == PLAYER_RANGER_CODE)
+ else if (id.interf == PLAYER_RANGER_CODE)
return this->ParseRanger(id, type, subtype, linenum,
token_count, tokens, time);
else if (id.interf == PLAYER_FIDUCIAL_CODE)
@@ -1398,108 +1437,108 @@
else if (id.interf == PLAYER_RFID_CODE)
return this->ParseRFID(id, type, subtype, linenum, token_count, tokens,
time);
-#if 0
else if (id.interf == PLAYER_POSITION3D_CODE)
return this->ParsePosition3d(id, type, subtype, linenum,
token_count, tokens, time);
-#endif
+ else if (id.interf == PLAYER_POWER_CODE)
+ return this->ParsePower(id, type, subtype, linenum,
+ token_count, tokens, time);
+
PLAYER_WARN1("unknown interface code [%s]",
::lookup_interface_name(0, id.interf));
return -1;
}
-#if 0
////////////////////////////////////////////////////////////////////////////
// Parse blobfinder data
-int ReadLog::ParseBlobfinder(player_devaddr_t id, int linenum,
- int token_count, char **tokens, struct timeval
time)
+int ReadLog::ParseBlobfinder(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum, int token_count, char **tokens,
+ double time)
{
player_blobfinder_data_t data;
- player_blobfinder_blob_t *blob;
size_t size;
int i, blob_count;
- if (token_count < 9)
+ if (token_count < 10)
{
PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
return -1;
}
- data.width = NUINT16(atoi(tokens[6]));
- data.height = NUINT16(atoi(tokens[7]));
- blob_count = atoi(tokens[8]);
- data.blob_count = NUINT16(blob_count);
+ data.width = atoi(tokens[7]);
+ data.height = atoi(tokens[8]);
+ blob_count = atoi(tokens[9]);
+ data.blobs_count = blob_count;
- if (token_count < 9 + blob_count * 10)
+ if (token_count < 10 + blob_count * 10)
{
PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
return -1;
}
+ player_blobfinder_blob_t blob[blob_count];
for (i = 0; i < blob_count; i++)
{
- blob = data.blobs + i;
- blob->id = NINT16(atoi(tokens[9 + i]));
- blob->color = NUINT32(atoi(tokens[10 + i]));
- blob->area = NUINT32(atoi(tokens[11 + i]));
- blob->x = NUINT16(atoi(tokens[12 + i]));
- blob->y = NUINT16(atoi(tokens[13 + i]));
- blob->left = NUINT16(atoi(tokens[14 + i]));
- blob->right = NUINT16(atoi(tokens[15 + i]));
- blob->top = NUINT16(atoi(tokens[16 + i]));
- blob->bottom = NUINT16(atoi(tokens[17 + i]));
- blob->range = NUINT16(M_MM(atof(tokens[18 + i])));
+ blob[i].id = atoi(tokens[10 + i*10]);
+ blob[i].color = atoi(tokens[11 + i*10]);
+ blob[i].area = atoi(tokens[12 + i*10]);
+ blob[i].x = atoi(tokens[13 + i*10]);
+ blob[i].y = atoi(tokens[14 + i*10]);
+ blob[i].left = atoi(tokens[15 + i*10]);
+ blob[i].right = atoi(tokens[16 + i*10]);
+ blob[i].top = atoi(tokens[17 + i*10]);
+ blob[i].bottom = atoi(tokens[18 + i*10]);
+ blob[i].range = atof(tokens[19 + i*10]);
}
+ data.blobs = blob;
size = sizeof(data) - sizeof(data.blobs) + blob_count *
sizeof(data.blobs[0]);
- this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, size, &time);
+ this->Publish(id,type,subtype, (void*) &data, size, &time);
return 0;
}
-
-
////////////////////////////////////////////////////////////////////////////
// Parse camera data
-int ReadLog::ParseCamera(player_devaddr_t id, int linenum,
- int token_count, char **tokens, struct timeval
time)
+int ReadLog::ParseCamera(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum, int token_count, char **tokens,
+ double time)
{
- player_camera_data_t *data;
+ player_camera_data_t data;
size_t src_size, dst_size;
- if (token_count < 13)
+ if (token_count < 14)
{
PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
return -1;
}
- data = (player_camera_data_t*) malloc(sizeof(player_camera_data_t));
- assert(data);
+ data.width = atoi(tokens[7]);
+ data.height = atoi(tokens[8]);
+ data.bpp = atoi(tokens[9]);
+ data.format = atoi(tokens[10]);
+ data.compression = atoi(tokens[11]);
+ data.image_count = atoi(tokens[12]);
- data->width = NUINT16(atoi(tokens[6]));
- data->height = NUINT16(atoi(tokens[7]));
- data->bpp = atoi(tokens[8]);
- data->format = atoi(tokens[9]);
- data->compression = atoi(tokens[10]);
- data->image_size = NUINT32(atoi(tokens[11]));
-
// Check sizes
- src_size = strlen(tokens[12]);
+ src_size = strlen(tokens[13]);
dst_size = ::DecodeHexSize(src_size);
- assert(dst_size = NUINT32(data->image_size));
- assert(dst_size < sizeof(data->image));
+ assert(dst_size == data.image_count);
+ data.image = new uint8_t[dst_size];
+ // TODO: This assertion doesn't seem to work
+ //assert(dst_size < sizeof(data.image));
// Decode string
- ::DecodeHex(data->image, dst_size, tokens[12], src_size);
+ ::DecodeHex(data.image, dst_size, tokens[13], src_size);
- this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, data, sizeof(*data) -
sizeof(data->image) + dst_size, &time);
-
- free(data);
-
+ this->Publish(id,type,subtype,(void*) &data, sizeof(data) -
sizeof(data.image) + dst_size, &time);
+
+ delete [] data.image;
+
return 0;
}
-#endif
-
////////////////////////////////////////////////////////////////////////////
// Parse fiducial data
int ReadLog::ParseFiducial(player_devaddr_t id,
@@ -1605,39 +1644,41 @@
return 0;
}
-#if 0
+
////////////////////////////////////////////////////////////////////////////
// Parse GPS data
-int ReadLog::ParseGps(player_devaddr_t id, int linenum,
- int token_count, char **tokens, struct timeval time)
+int ReadLog::ParseGps(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum, int token_count, char **tokens,
+ double time)
{
player_gps_data_t data;
- if (token_count < 17)
+ if (token_count < 19)
{
PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
return -1;
}
- data.time_sec = NUINT32((int) atof(tokens[6]));
- data.time_usec = NUINT32((int) fmod(atof(tokens[6]), 1.0));
+ data.time_sec = (int) atof(tokens[7]);
+ data.time_usec = (int) (atof(tokens[7]) - (float)(data.time_sec) * 1e6);
- data.latitude = NINT32((int) (60 * 60 * 60 * atof(tokens[7])));
- data.longitude = NINT32((int) (60 * 60 * 60 * atof(tokens[8])));
- data.altitude = NINT32(M_MM(atof(tokens[9])));
+ data.latitude = (int) (atof(tokens[8]) * 1e7);
+ data.longitude = (int) (atof(tokens[9]) * 1e7);
+ data.altitude = (int) (atof(tokens[10]) * 1e3);
- data.utm_e = NINT32(CM_MM(atof(tokens[10])));
- data.utm_n = NINT32(CM_MM(atof(tokens[11])));
+ data.utm_e = atof(tokens[11]);
+ data.utm_n = atof(tokens[12]);
- data.hdop = NINT16((int) (10 * atof(tokens[12])));
- data.hdop = NINT16((int) (10 * atof(tokens[13])));
- data.err_horz = NUINT32(M_MM(atof(tokens[14])));
- data.err_vert = NUINT32(M_MM(atof(tokens[15])));
+ data.hdop = (int) (10 * atof(tokens[13]));
+ data.hdop = (int) (10 * atof(tokens[14]));
+ data.err_horz = atof(tokens[15]);
+ data.err_vert = atof(tokens[16]);
- data.quality = atoi(tokens[16]);
- data.num_sats = atoi(tokens[17]);
+ data.quality = atoi(tokens[17]);
+ data.num_sats = atoi(tokens[18]);
- this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0,&data, sizeof(data), &time);
+ this->Publish(id,type,subtype, (void*) &data, sizeof(data), &time);
return 0;
}
@@ -1645,29 +1686,33 @@
////////////////////////////////////////////////////////////////////////////
// Parse joystick data
-int ReadLog::ParseJoystick(player_devaddr_t id, int linenum,
- int token_count, char **tokens, struct timeval time)
+int ReadLog::ParseJoystick(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum, int token_count, char **tokens,
+ double time)
{
player_joystick_data_t data;
- if (token_count < 11)
+ if (token_count < 14)
{
PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
return -1;
}
- data.xpos = NINT16((short) atoi(tokens[6]));
- data.ypos = NINT16((short) atoi(tokens[7]));
- data.xscale = NINT16((short) atoi(tokens[8]));
- data.yscale = NINT16((short) atoi(tokens[9]));
- data.buttons = NUINT16((unsigned short) (unsigned int) atoi(tokens[10]));
+ data.pos[0] = atoi(tokens[7]);
+ data.pos[1] = atoi(tokens[8]);
+ data.pos[2] = atoi(tokens[9]);
+ data.scale[0] = atoi(tokens[10]);
+ data.scale[1] = atoi(tokens[11]);
+ data.scale[2] = atoi(tokens[12]);
+ data.buttons = (unsigned int)atoi(tokens[13]);
- this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, sizeof(data), &time);
+ this->Publish(id,type,subtype,(void*) &data, sizeof(data), &time);
return 0;
}
-#endif
+
////////////////////////////////////////////////////////////////////////////
// Parse laser data
int ReadLog::ParseLaser(player_devaddr_t id,
@@ -3504,42 +3549,123 @@
}
}
-#if 0
////////////////////////////////////////////////////////////////////////////
// Parse position3d data
-int ReadLog::ParsePosition3d(player_devaddr_t id, int linenum,
- int token_count, char **tokens, struct timeval
time)
+int ReadLog::ParsePosition3d(player_devaddr_t id, unsigned short type,
+ unsigned short subtype, int linenum, int token_count,
+ char **tokens, double time)
{
- player_position3d_data_t data;
+ switch (type){
+ case PLAYER_MSGTYPE_DATA:
+
+ switch (subtype) {
+ case PLAYER_POSITION3D_DATA_STATE:
+
+ player_position3d_data_t data;
+
+ if (token_count < 20)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
+ return -1;
+ }
- if (token_count < 19)
- {
- PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
- return -1;
- }
+ data.pos.px = atof(tokens[7]);
+ data.pos.py = atof(tokens[8]);
+ data.pos.pz = atof(tokens[9]);
- data.xpos = NINT32(M_MM(atof(tokens[6])));
- data.ypos = NINT32(M_MM(atof(tokens[7])));
- data.zpos = NINT32(M_MM(atof(tokens[8])));
+ data.pos.proll = atof(tokens[10]);
+ data.pos.ppitch = atof(tokens[11]);
+ data.pos.pyaw = atof(tokens[12]);
- data.roll = NINT32(1000 * atof(tokens[9]));
- data.pitch = NINT32(1000 * atof(tokens[10]));
- data.yaw = NINT32(1000 * atof(tokens[11]));
+ data.vel.px = atof(tokens[13]);
+ data.vel.py = atof(tokens[14]);
+ data.vel.pz = atof(tokens[15]);
- data.xspeed = NINT32(M_MM(atof(tokens[12])));
- data.yspeed = NINT32(M_MM(atof(tokens[13])));
- data.zspeed = NINT32(M_MM(atof(tokens[14])));
+ data.vel.proll = atof(tokens[16]);
+ data.vel.ppitch = atof(tokens[17]);
+ data.vel.pyaw = atof(tokens[18]);
- data.rollspeed = NINT32(1000 * atof(tokens[15]));
- data.pitchspeed = NINT32(1000 * atof(tokens[16]));
- data.yawspeed = NINT32(1000 * atof(tokens[17]));
+ data.stall = atoi(tokens[19]);
- data.stall = atoi(tokens[18]);
+ this->Publish(id,type,subtype,(void*) &data, sizeof(data), &time);
- this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, sizeof(data), &time);
-
- return 0;
+ return 0;
+
+ case PLAYER_POSITION3D_DATA_GEOMETRY:
+ player_position3d_geom_t geom;
+ if (token_count < 16)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
+ return -1;
+ }
+ 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.sw = atof(tokens[13]);
+ geom.size.sl = atof(tokens[14]);
+ geom.size.sh = atof(tokens[15]);
+
+ this->Publish(id,type,subtype,(void*) &geom, sizeof(geom), &time);
+
+ return 0;
+ default:
+ return (-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ player_position3d_geom_t geom;
+ if (token_count < 16)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
+ return -1;
+ }
+ 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.sw = atof(tokens[13]);
+ geom.size.sl = atof(tokens[14]);
+ geom.size.sh = atof(tokens[15]);
+
+ this->Publish(id,type,subtype,(void*) &geom, sizeof(geom), &time);
+ return 0;
+ default:
+ return (-1);
+ }
}
-
-#endif
-
+////////////////////////////////////////////////////////////////////////////
+// Parse power data
+int ReadLog::ParsePower(player_devaddr_t id, unsigned short type,
+ unsigned short subtype, int linenum, int token_count,
+ char **tokens, double time)
+{
+ switch (type){
+ case PLAYER_MSGTYPE_DATA:
+
+ player_power_data_t data;
+ if (token_count < 13)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum);
+ return -1;
+ }
+ data.volts = atof(tokens[7]);
+ data.percent = atof(tokens[8]);
+ data.joules = atof(tokens[9]);
+ data.watts = atof(tokens[10]);
+ data.charging = atoi(tokens[11]);
+ data.valid = atoi(tokens[12]);
+
+ this->Publish(id,type,subtype,(void*) &data, sizeof(data), &time);
+ return 0;
+ default:
+ return (-1);
+ }
+}
Modified: code/player/trunk/server/drivers/shell/writelog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/writelog.cc 2009-10-18 13:46:14 UTC
(rev 8298)
+++ code/player/trunk/server/drivers/shell/writelog.cc 2009-10-18 13:50:43 UTC
(rev 8299)
@@ -82,15 +82,13 @@
- @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_gps
- @ref interface_joystick
- @ref interface_position3d
- @ref interface_power
+- @ref interface_dio
+- @ref interface_aio
@par Configuration requests
@@ -118,6 +116,10 @@
- extension (string)
- Default: ".log"
- File extension for the log file.
+- filename (string)
+ - Default: basename+timestamp+extension
+ - Use this option to override the default basename/timestamp/extension
+ combination and set your own arbitrary filename for the log.
- autorecord (integer)
- Default: 0
- Default log state; set to 1 for continous logging.
@@ -146,7 +148,7 @@
)
@endverbatim
-...@author Andrew Howard, Radu Bogdan Rusu
+...@author Andrew Howard, Radu Bogdan Rusu, Rich Mattes
*/
/** @} */
@@ -281,23 +283,22 @@
// 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);
+ private: int WriteBlobfinder(player_msghdr_t* hdr, void *data);
// Write GPS data to file
- private: void WriteGps(player_gps_data_t *data);
-
+ private: int WriteGps(player_msghdr_t* hdr, void *data);
+
// Write joystick data to file
- private: void WriteJoystick(player_joystick_data_t *data);
+ private: int WriteJoystick(player_msghdr_t* hdr, void *data);
// Write position3d data to file
- private: void WritePosition3d(player_position3d_data_t *data);
+ private: int WritePosition3d(player_msghdr_t* hdr, void *data);
// Write power data to file
- private: void WritePower(player_power_data_t *data);
+ private: int WritePower(player_msghdr_t* hdr, void *data);
-#endif
// Where to save files
private: char log_directory[1024];
@@ -634,6 +635,7 @@
// oh well.
PLAYER_WARN("unable to get position geometry");
}
+
else
{
// log it
@@ -641,6 +643,26 @@
delete msg;
}
}
+ else if (device->addr.interf == PLAYER_POSITION3D_CODE)
+ {
+ // Get the position geometry
+ Message* msg;
+ if(!(msg = device->device->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+ PLAYER_POSITION3D_REQ_GET_GEOM,
+ NULL, 0, NULL, true)))
+ {
+ // oh well.
+ PLAYER_WARN("unable to get position3d geometry");
+ }
+
+ else
+ {
+ // log it
+ this->Write(device, msg->GetHeader(), msg->GetPayload());
+ delete msg;
+ }
+ }
/* HHAA 15-02-2007 */
else if (device->addr.interf == PLAYER_BUMPER_CODE)
{
@@ -917,24 +939,24 @@
retval = this->WriteCamera(device, hdr, data);
break;
case PLAYER_FIDUCIAL_CODE:
- this->WriteFiducial(hdr, data);
+ retval = this->WriteFiducial(hdr, data);
break;
-#if 0
+ case PLAYER_GPS_CODE:
+ retval = this->WriteGps(hdr, data);
+ break;
case PLAYER_BLOBFINDER_CODE:
- this->WriteBlobfinder((player_blobfinder_data_t*) data);
+ retval = this->WriteBlobfinder(hdr, data);
break;
- case PLAYER_GPS_CODE:
- this->WriteGps((player_gps_data_t*) data);
- break;
case PLAYER_JOYSTICK_CODE:
- this->WriteJoystick((player_joystick_data_t*) data);
+ retval = this->WriteJoystick(hdr, data);
break;
case PLAYER_POSITION3D_CODE:
- this->WritePosition3d((player_position3d_data_t*) data);
+ retval = this->WritePosition3d(hdr, data);
break;
case PLAYER_POWER_CODE:
- this->WritePower((player_power_data_t*) data);
+ retval = this->WritePower(hdr, data);
break;
+#if 0
case PLAYER_PLAYER_CODE:
break;
#endif
@@ -1209,7 +1231,14 @@
- sh (float): height of the device, in meters
- have_config(uint8): If non-zero, the config data has been filled
- config of device:
-
+ - min_angle (float): start angle of scans, in radians
+ - max_angle (float): end angle of scans, in radians
+ - angular_res (float): scan resolution, in radians
+ - min_range (float): minimum range, in meters
+ - max_range (float): maximum range, in meters
+ - range_res (float): range resolution, in meters
+ - frequency (float): scanning frequency, in Hz
+
- 4:1 (PLAYER_RANGER_REQ_GET_GEOM) - Ranger pose information. The format is:
- pose of device:
- px (float): X coordinate of the pose, in meters
@@ -1556,7 +1585,7 @@
- va (float): yaw coordinate of the device's velocity, in radians/sec
- stall (int): Motor stall flag
-- 4:0 (PLAYER_POSITION2D_REQ_GET_GEOM) Geometry info. The format is:
+- 4:1 (PLAYER_POSITION2D_REQ_GET_GEOM) Geometry info. The format is:
- px (float): X coordinate of the offset of the device's center of rotation,
in meters
- py (float): Y coordinate of the offset of the device's center of rotation,
in meters
- pa (float): yaw coordinate of the offset of the device's center of
rotation, in radians
@@ -2485,7 +2514,7 @@
}
-#if 0
+
/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_blobfinder Blobfinder format
@@ -2504,33 +2533,40 @@
- range (int): in mm, of range to blob (if supported)
*/
-void WriteLog::WriteBlobfinder(player_blobfinder_data_t *data)
+int WriteLog::WriteBlobfinder(player_msghdr_t* hdr, void *data)
{
+ player_blobfinder_data_t* bdata;
+ bdata = (player_blobfinder_data_t*) data;
+
+ switch(hdr->type){
+ case PLAYER_MSGTYPE_DATA:
- fprintf(this->file, " %d %d %d",
- HUINT16(data->width),
- HUINT16(data->height),
- HUINT16(data->blob_count));
+ fprintf(this->file, "%d %d %d",
+ bdata->width,
+ bdata->height,
+ bdata->blobs_count);
- for(int i=0; i < HUINT16(data->blob_count); i++)
- {
- fprintf(this->file, " %d %d %d %d %d %d %d %d %d %f",
- HINT16(data->blobs[i].id),
- HUINT32(data->blobs[i].color),
- HUINT32(data->blobs[i].area),
- HUINT16(data->blobs[i].x),
- HUINT16(data->blobs[i].y),
- HUINT16(data->blobs[i].left),
- HUINT16(data->blobs[i].right),
- HUINT16(data->blobs[i].top),
- HUINT16(data->blobs[i].bottom),
- MM_M(HUINT16(data->blobs[i].range)));
- }
+ for(int i=0; i < (int)bdata->blobs_count; i++)
+ {
+ fprintf(this->file, " %d %d %d %d %d %d %d %d %d %f",
+ bdata->blobs[i].id,
+ bdata->blobs[i].color,
+ bdata->blobs[i].area,
+ bdata->blobs[i].x,
+ bdata->blobs[i].y,
+ bdata->blobs[i].left,
+ bdata->blobs[i].right,
+ bdata->blobs[i].top,
+ bdata->blobs[i].bottom,
+ bdata->blobs[i].range);
+ }
- return;
+ return (0);
+ default:
+ return (-1);
+ }
}
-#endif
/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_camera Camera format
@@ -2727,8 +2763,8 @@
}
}
-#if 0
+
/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_gps GPS format
@@ -2748,32 +2784,37 @@
- quality (int): quality of fix (0 = invalid, 1 = GPS fix, 2 = DGPS fix)
- num_sats (int): number of satellites in view
*/
-void WriteLog::WriteGps(player_gps_data_t *data)
+int WriteLog::WriteGps(player_msghdr_t* hdr, void *data)
{
- fprintf(this->file,
- "%.3f "
- "%.6f %.6f %.6f "
- "%.3f %.3f "
- "%.3f %.3f %.3f %.3f "
- "%d %d",
- (double) (uint32_t) HINT32(data->time_sec) +
- (double) (uint32_t) HINT32(data->time_sec) * 1e-6,
- (double) HINT32(data->latitude) / (1e7),
- (double) HINT32(data->longitude) / (1e7),
- MM_M(HINT32(data->altitude)),
- CM_M(HINT32(data->utm_e)),
- CM_M(HINT32(data->utm_n)),
- (double) HINT16(data->hdop) / 10,
- (double) HINT16(data->vdop) / 10,
- MM_M(HINT32(data->err_horz)),
- MM_M(HINT32(data->err_vert)),
- (int) data->quality,
- (int) data->num_sats);
-
- return;
+ player_gps_data_t *gdata;
+ gdata = (player_gps_data_t*) data;
+ switch(hdr->type){
+ case PLAYER_MSGTYPE_DATA:
+ fprintf(this->file,
+ "%.3f "
+ "%.7f %.7f %.7f "
+ "%.3f %.3f "
+ "%.3f %.3f %.3f %.3f "
+ "%d %d",
+ (double)gdata->time_sec +
+ (double)gdata->time_sec * 1e-6,
+ (double)gdata->latitude / (1e7),
+ (double)gdata->longitude / (1e7),
+ (double)gdata->altitude / (1e3),
+ gdata->utm_e,
+ gdata->utm_n,
+ (double)gdata->hdop / 10.0,
+ (double)gdata->vdop / 10.0,
+ gdata->err_horz,
+ gdata->err_vert,
+ gdata->quality,
+ gdata->num_sats);
+ return (0);
+ default:
+ return (-1);
+ }
}
-
/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_joystick Joystick format
@@ -2782,20 +2823,30 @@
The format for each @ref interface_joystick message is:
- xpos (int): unscaled X position of joystick
- ypos (int): unscaled Y position of joystick
+ - yawpos (int): unscaled Yaw position of the joystick
- xscale (int): maximum X position
- yscale (int): maximum Y position
+ - yawscale (int): maximum Yaw position
- buttons (hex string): bitmask of button states
*/
-void WriteLog::WriteJoystick(player_joystick_data_t *data)
+int WriteLog::WriteJoystick(player_msghdr_t* hdr, void *data)
{
- fprintf(this->file, "%+d %+d %d %d %X",
- HINT16(data->xpos),
- HINT16(data->ypos),
- HINT16(data->xscale),
- HINT16(data->yscale),
- HUINT16(data->buttons));
-
- return;
+ player_joystick_data_t *jdata;
+ jdata = (player_joystick_data_t*) data;
+ switch (hdr->type){
+ case PLAYER_MSGTYPE_DATA:
+ fprintf(this->file, "%+d %+d %+d %d %d %d %X",
+ jdata->pos[0],
+ jdata->pos[1],
+ jdata->pos[2],
+ jdata->scale[0],
+ jdata->scale[1],
+ jdata->scale[2],
+ jdata->buttons);
+ return (0);
+ default:
+ return (-1);
+ }
}
@@ -2806,6 +2857,7 @@
@brief position3d format
The format for each @ref interface_position3d message is:
+- 1:1 (PLAYER_POSITION3D_DATA_STATE) Odometry information. The format is:
- xpos (float): in meters
- ypos (float): in meters
- zpos (float): in meters
@@ -2819,30 +2871,104 @@
- pitchspeed(float): in radians / second
- yawspeed(float): in radians / second
- stall (int): motor stall sensor
+
+- 1:2 (PLAYER_POSITION3D_DATA_GEOM) Geometry information. The format is:
+ - xpos (float): in meters
+ - ypos (float): in meters
+ - zpos (float): in meters
+ - roll (float): in radians
+ - pitch (float): in radians
+ - yaw (float): in radians
+ - width (float): in meters
+ - length (float): in meters
+ - height (float): in meters
+
+- 4:1 (PLAYER_POSITION3D_REQ_GET_GEOM) Geometry information. The format is:
+ - xpos (float): in meters
+ - ypos (float): in meters
+ - zpos (float): in meters
+ - roll (float): in radians
+ - pitch (float): in radians
+ - yaw (float): in radians
+ - width (float): in meters
+ - length (float): in meters
+ - height (float): in meters
*/
-void WriteLog::WritePosition3d(player_position3d_data_t *data)
+int WriteLog::WritePosition3d(player_msghdr_t* hdr, void *data)
{
- fprintf(this->file,
- "%+.4f %+.4f %+.4f "
- "%+.4f %+.4f %+.4f "
- "%+.4f %+.4f %+.4f "
- "%+.4f %+.4f %+.4f "
- "%d",
- MM_M(HINT32(data->xpos)),
- MM_M(HINT32(data->ypos)),
- MM_M(HINT32(data->zpos)),
- HINT32(data->roll) / 1000.0,
- HINT32(data->pitch) / 1000.0,
- HINT32(data->yaw) / 1000.0,
- MM_M(HINT32(data->xspeed)),
- MM_M(HINT32(data->yspeed)),
- MM_M(HINT32(data->zspeed)),
- HINT32(data->rollspeed) / 1000.0,
- HINT32(data->pitchspeed) / 1000.0,
- HINT32(data->yawspeed) / 1000.0,
- data->stall);
+ switch (hdr->type){
+ case PLAYER_MSGTYPE_DATA:
+ switch(hdr->subtype){
+ case PLAYER_POSITION3D_DATA_STATE:
+ player_position3d_data_t *pdata;
+ pdata = (player_position3d_data_t*)
data;
+ fprintf(this->file,
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f "
+ "%d",
+ pdata->pos.px,
+ pdata->pos.py,
+ pdata->pos.pz,
+ pdata->pos.proll,
+ pdata->pos.ppitch,
+ pdata->pos.pyaw,
+ pdata->vel.px,
+ pdata->vel.py,
+ pdata->vel.pz,
+ pdata->vel.proll,
+ pdata->vel.ppitch,
+ pdata->vel.pyaw,
+ pdata->stall);
- return;
+ return(0);
+ case PLAYER_POSITION3D_DATA_GEOMETRY:
+ player_position3d_geom_t *gdata;
+ gdata = (player_position3d_geom_t*)
data;
+ fprintf(this->file,
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f ",
+ gdata->pose.px,
+ gdata->pose.py,
+ gdata->pose.pz,
+ gdata->pose.proll,
+ gdata->pose.ppitch,
+ gdata->pose.pyaw,
+ gdata->size.sw,
+ gdata->size.sl,
+ gdata->size.sh);
+ return (0);
+ default:
+ return (-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch (hdr->subtype){
+ case PLAYER_POSITION3D_REQ_GET_GEOM:
+ printf("w00t\n");
+ player_position3d_geom_t *gdata;
+ gdata = (player_position3d_geom_t*)
data;
+ fprintf(this->file,
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f "
+ "%+.4f %+.4f %+.4f ",
+ gdata->pose.px,
+ gdata->pose.py,
+ gdata->pose.pz,
+ gdata->pose.proll,
+ gdata->pose.ppitch,
+ gdata->pose.pyaw,
+ gdata->size.sw,
+ gdata->size.sl,
+ gdata->size.sh);
+ return (0);
+ default:
+ return (-1);
+ }
+ default:
+ return (-1);
+ }
}
@@ -2852,15 +2978,43 @@
@brief power log format
The format for each @ref interface_power message is:
- - charge (float): in volts
+ - battery voltage (float): in volts
+ - charge percentage (float): in percent
+ - energy stored (float): in joules
+ - estimated energy consumption (float): in watts
+ - charging status (int): 1 for charging
+ - valid (int): bitfield for valid fields
+
+This driver wil attempt to print all of the data fields,though in most cases
the fields are not all used.
+If the field is invalid (unused), a 0 will be printed. The "valid" bitfield
should be used to verify which fields are active.
*/
-void WriteLog::WritePower(player_power_data_t *data)
+int WriteLog::WritePower(player_msghdr_t* hdr, void *data)
{
- fprintf(this->file, "%.1f ", HUINT16(data->charge) / 10.0);
- return;
+ player_power_data_t *pdata;
+ pdata = (player_power_data_t*) data;
+ switch (hdr->type){
+ case(PLAYER_MSGTYPE_DATA):
+ if (!(pdata->valid & PLAYER_POWER_MASK_VOLTS))
+ pdata->volts = 0;
+ if (!(pdata->valid & PLAYER_POWER_MASK_WATTS))
+ pdata->watts = 0;
+ if (!(pdata->valid & PLAYER_POWER_MASK_JOULES))
+ pdata->joules = 0;
+ if (!(pdata->valid & PLAYER_POWER_MASK_PERCENT))
+ pdata->percent = 0;
+ if (!(pdata->valid & PLAYER_POWER_MASK_CHARGING))
+ pdata->charging = 0;
+
+ fprintf(this->file, "%.3f %.3f %.3f %.3f %d %d",
+ pdata->volts,
+ pdata->percent,
+ pdata->joules,
+ pdata->watts,
+ pdata->charging,
+ pdata->valid);
+ return (0);
+ default:
+ return (-1);
+ }
}
-
-#endif
-
-
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
