Revision: 6358
http://playerstage.svn.sourceforge.net/playerstage/?rev=6358&view=rev
Author: gbiggs
Date: 2008-04-18 00:51:55 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 4458:4467 from trunk
Modified Paths:
--------------
code/player/branches/cmake/client_libs/libplayerc/bindings/python/playerc.i
code/player/branches/cmake/server/drivers/camera/uvc/cameraUVC.cc
code/player/branches/cmake/server/drivers/mixed/erratic/erratic.cc
code/player/branches/cmake/server/drivers/shell/kartowriter.cc
code/player/branches/cmake/server/drivers/shell/writelog.cc
Modified:
code/player/branches/cmake/client_libs/libplayerc/bindings/python/playerc.i
===================================================================
--- code/player/branches/cmake/client_libs/libplayerc/bindings/python/playerc.i
2008-04-18 07:36:45 UTC (rev 6357)
+++ code/player/branches/cmake/client_libs/libplayerc/bindings/python/playerc.i
2008-04-18 07:51:55 UTC (rev 6358)
@@ -560,6 +560,11 @@
return dict;
}
+ void set_entry_raw(player_blackboard_entry_t *entry)
+ {
+ playerc_blackboard_set_entry(self,entry);
+ }
+
PyObject *set_entry(PyObject *dict)
{
PyObject *key;
Modified: code/player/branches/cmake/server/drivers/camera/uvc/cameraUVC.cc
===================================================================
--- code/player/branches/cmake/server/drivers/camera/uvc/cameraUVC.cc
2008-04-18 07:36:45 UTC (rev 6357)
+++ code/player/branches/cmake/server/drivers/camera/uvc/cameraUVC.cc
2008-04-18 07:51:55 UTC (rev 6358)
@@ -98,6 +98,7 @@
table->AddDriver("camerauvc", CameraUvc_Init);
}
+/*
extern "C"
{
int player_driver_init(DriverTable* table)
@@ -106,6 +107,7 @@
return(0);
}
}
+*/
CameraUvc::CameraUvc(ConfigFile* cf, int section)
: Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_CAMERA_CODE)
Modified: code/player/branches/cmake/server/drivers/mixed/erratic/erratic.cc
===================================================================
--- code/player/branches/cmake/server/drivers/mixed/erratic/erratic.cc
2008-04-18 07:36:45 UTC (rev 6357)
+++ code/player/branches/cmake/server/drivers/mixed/erratic/erratic.cc
2008-04-18 07:51:55 UTC (rev 6358)
@@ -176,6 +176,7 @@
#include <unistd.h>
#include <math.h>
#include <termios.h>
+#include <stdlib.h> // for abs()
#include "erratic.h"
Modified: code/player/branches/cmake/server/drivers/shell/kartowriter.cc
===================================================================
--- code/player/branches/cmake/server/drivers/shell/kartowriter.cc
2008-04-18 07:36:45 UTC (rev 6357)
+++ code/player/branches/cmake/server/drivers/shell/kartowriter.cc
2008-04-18 07:51:55 UTC (rev 6358)
@@ -22,8 +22,8 @@
**/
/** @ingroup drivers */
/** @{ */
-/** @defgroup driver_kartowriter kartowriter
- * @brief Karto logger
+/** @defgroup driver_logger
+ * @brief Karto logger plugin
@par Compile-time dependencies
@@ -54,9 +54,9 @@
@par Configuration requests
- - PLAYER_LOG_REQ_SET_WRITE_STATE
- - PLAYER_LOG_REQ_GET_STATE
- - PLAYER_LOG_REQ_SET_FILENAME (not yet implemented)
+ - PLAYER_LOG_SET_WRITE_STATE
+ - PLAYER_LOG_GET_STATE
+ - PLAYER_LOG_SET_FILENAME (not yet implemented)
@par Example
@@ -95,202 +95,266 @@
#include <libplayercore/playercore.h>
#include <unistd.h>
#include <math.h>
+#include <assert.h>
#include <libplayercore/addr_util.h>
#include "sys/types.h" /* for time_t */
#include "time.h" /* for struct tm */
#include <list>
#define ID_MAX_SIZE 1024
+#define DRIVER_NAME "kartowriter"
+#define DEFAULT_UID "http://unknown";
+#define MAX_HOSTNAME 256
- using namespace std;
+using namespace std;
-class LogDevice {
+class LogDevice
+ {
private:
public:
+ Device* pDevice;
+ char* pUid;
player_devaddr_t addr;
- Device* device;
- char *uid;
// constructor
- LogDevice(player_devaddr_t* addr);
-
+ LogDevice(const player_devaddr_t& playerAddr);
//destructor
- ~LogDevice();
-
+ virtual ~LogDevice();
//prints the Device unique ID
char* getUID();
-
};
-LogDevice::LogDevice(player_devaddr_t* addr) {
- this->addr = *addr;
- uid = NULL;
- uid = getUID();
-}
+LogDevice::LogDevice(const player_devaddr_t& playerAddr) : pDevice(NULL),
pUid(NULL), addr(playerAddr)
+ {
+ pUid = getUID();
+ }
LogDevice::~LogDevice() {
- free(uid);
+ free(pUid);
}
+//
+// Main Class
+//
-
-class KartoLogger:public Driver
+class KartoLogger : public Driver
{
private:
list<LogDevice*> devices;
- char karto_filename[MAX_FILENAME_SIZE];
- FILE *karto_file;
+ char kartoFilename[MAX_FILENAME_SIZE];
+ FILE* kartoFile;
bool debug; // Debug flag to print more message
- bool enable; // Enabling flag
+ bool enableLogger; // Enabling flag
bool compress; // Compress flag (use gzip at the end or not)
+ player_pose2d_t currentPose;
- // The current robot pose
- player_pose2d_t position_pose;
-
-
// When we start logging
- double start_time;
+ double startTime;
- // Functions to open and close the log file
+ // Side effect set and unset the file descriptor kartoFile.
int OpenLog();
int CloseLog();
// Specific functions to write the correct XML format depending
// of the data
- void WritePosition(player_devaddr_t dev,player_msghdr_t* hdr,
player_position2d_data_t* data);
- void WriteLaserScan(player_devaddr_t dev,player_msghdr_t* hdr,
player_laser_data_t* scan);
- void WriteSonarScan(player_devaddr_t dev,player_msghdr_t* hdr,
player_sonar_data_t* scan);
+ void WritePosition(player_msghdr_t* hdr, player_position2d_data_t*
data);
+ void WriteLaserScan(player_msghdr_t* hdr, player_laser_data_t* scan);
+ void WriteLaserScanPose(player_msghdr_t* pHdr, player_laser_data_scanpose_t*
pScan);
+ void WriteSonarScan(player_msghdr_t* hdr, player_sonar_data_t* scan);
// Print the geometry (and all specific info) of any LogDevices
- int logGeometry(LogDevice *d);
+ int WriteGeometry(LogDevice* pDev);
+ void ComputeQuaternion(double heading, double attitude, double bank
,double* x,double* y, double* z, double* w);
public:
- int KartoLoggerDestroy();
-
// constructor
- KartoLogger( ConfigFile* cf, int section);
+ KartoLogger( ConfigFile* pCf, int section);
- ~KartoLogger();
+ virtual ~KartoLogger();
virtual void Main();
int Setup();
int Shutdown();
char* getUID(player_devaddr_t dev);
- virtual int Unsubscribe(Device* id);
+ virtual int Unsubscribe(Device* pId);
// MessageHandler
- int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void
* data);
+ int ProcessMessage(QueuePointer &pQueue, player_msghdr* pHdr, void*
pData);
};
-void computeQuaternion(double heading, double attitude, double bank ,double*
x,double* y, double* z, double* w);
-
-void
- KartoLogger::Main()
+void KartoLogger::Main()
{
- for(;;)
+ while(true)
{
usleep(20000);
pthread_testcancel();
this->ProcessMessages();
- // Verifie qu'on doit toujours marcher
- if(!this->enable)
- continue;
+ if(!this->enableLogger)
+ {
+ break;
+ }
}
printf("exiting...\n");
}
-int
- KartoLogger::Unsubscribe(Device* device)
+//
+// Unsubscribe the device to Player
+//
+int KartoLogger::Unsubscribe(Device* pDevice)
{
- return(device->Unsubscribe(this->InQueue));
+ if (pDevice != NULL)
+ {
+ return pDevice->Unsubscribe(this->InQueue);
+ }
+ else
+ {
+ return -1;
+ }
}
-int KartoLogger::ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr,
void * data)
+//
+// Main function KartoLogger::ProcessMessage
+// We expect control message for the PLAYER_LOG_CODE interface:
+// /** Types of log device: read */
+// #define PLAYER_LOG_TYPE_READ 1
+// /** Types of log device: write */
+// #define PLAYER_LOG_TYPE_WRITE 2
+//
+// /** Request/reply subtype: set write state */
+// #define PLAYER_LOG_REQ_SET_WRITE_STATE 1
+// /** Request/reply subtype: set read state */
+// #define PLAYER_LOG_REQ_SET_READ_STATE 2
+// /** Request/reply subtype: get state */
+// #define PLAYER_LOG_REQ_GET_STATE 3
+// /** Request/reply subtype: rewind */
+// #define PLAYER_LOG_REQ_SET_READ_REWIND 4
+// #define PLAYER_LOG_REQ_SET_READ_REWIND 4
+// /** Request/reply subtype: set filename to write */
+// #define PLAYER_LOG_REQ_SET_FILENAME 5
+// see libplayercore/player.h
+// Warning !!!
+// If player code changes this code need to be
+// updated.
+// Since the logger is registred to many devices
+// we currently only suppport laser, sonar and position2d interfaces.
+// For each msg of type PLAYER_MSGTYPE_DATA (which are the message
+// containing the data from the subscribed devices) we are differentiating
+// on the interface type and then calling the right write functions:
+// WritePosition, WriteLaserScan, WriteSonarScan
+//
+// Logic:
+// First we check for the type of message
+// If there a REQuest type, we check for the correct size.
+// Then we change the value (enablingLogger) and send ACK
+// If it's a data message (LAYER_MSGTYPE_DATA),
+// Call the right Write function depending of the type.
+//
+// Return values:
+// 0 everything is fine
+// -1 message was not the right format or length and the message is discarded
+//
+int KartoLogger::ProcessMessage(QueuePointer &pQueue, player_msghdr* pHdr,
void* pData)
{
//writelog control
- if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ if(Message::MatchMessage(pHdr, PLAYER_MSGTYPE_REQ,
PLAYER_LOG_REQ_SET_WRITE_STATE,
this->device_addr))
{
- player_log_set_write_state_t* sreq =
(player_log_set_write_state_t*)data;
+ if(pHdr->size != sizeof(player_log_set_write_state_t))
+ {
+ PLAYER_ERROR2("request is wrong length (%d != %d);
ignoring",
+
pHdr->size, sizeof(player_log_set_write_state_t));
+ return -1;
+ }
+ player_log_set_write_state_t* pWriteState =
(player_log_set_write_state_t*)pData;
- if(sreq->state)
+ if(pWriteState->state)
{
puts("KartoLogger is now logging...");
- this->enable = true;
+ this->enableLogger = true;
}
else
{
puts("KartoLogger has stopped logging...");
- this->enable = false;
+ this->enableLogger = false;
}
// send an empty ACK
- this->Publish(this->device_addr, resp_queue,
+ this->Publish(this->device_addr, pQueue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LOG_REQ_SET_WRITE_STATE);
- return(0);
+ return 0;
}
- else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ else if(Message::MatchMessage(pHdr, PLAYER_MSGTYPE_REQ,
PLAYER_LOG_REQ_GET_STATE, this->device_addr))
{
- player_log_get_state_t greq;
+ if(pHdr->size != 0)
+ {
+ PLAYER_ERROR2("request is wrong length (%d != %d);
ignoring",
+ pHdr->size, 0);
+ return -1;
+ }
- greq.type = PLAYER_LOG_TYPE_WRITE;
- if(this->enable)
- greq.state = 1;
+ player_log_get_state_t playerState;
+
+ playerState.type = PLAYER_LOG_TYPE_WRITE;
+ if(this->enableLogger)
+ {
+ playerState.state = 1;
+ }
else
- greq.state = 0;
+ {
+ playerState.state = 0;
+ }
this->Publish(this->device_addr,
- resp_queue,
+ pQueue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_LOG_REQ_GET_STATE,
- (void*)&greq, sizeof(greq), NULL);
- return(0);
+ (void*)&playerState, sizeof(playerState), NULL);
+ return 0;
}
- else if(hdr->type == PLAYER_MSGTYPE_DATA)
+ else if(pHdr->type == PLAYER_MSGTYPE_DATA)
{
// If logging is stopped, then don't log
- if(!this->enable)
- return(0);
-
-
- list<LogDevice*>::iterator iter;
- for (iter=devices.begin(); iter != devices.end(); iter++)
- {
- LogDevice* device = (*iter);
- if((device->addr.host == hdr->addr.host) &&
- (device->addr.robot == hdr->addr.robot) &&
- (device->addr.interf == hdr->addr.interf) &&
- (device->addr.index == hdr->addr.index)) {
- player_interface_t iface;
- ::lookup_interface_code(device->addr.interf,
&iface);
- switch (iface.interf)
- {
- case PLAYER_LASER_CODE:
- this->WriteLaserScan(device->addr, hdr,
(player_laser_data_t*)data);
- break;
- case PLAYER_POSITION2D_CODE:
- this->WritePosition( device->addr, hdr,
(player_position2d_data_t*)data);
- break;
- case PLAYER_SONAR_CODE:
- this->WriteSonarScan(device->addr, hdr,
(player_sonar_data_t*)data);
- break;
+ if(!this->enableLogger)
+ {
+ return 0;
+ }
+ player_interface_t iface;
+ ::lookup_interface_code(pHdr->addr.interf, &iface);
+ switch (iface.interf)
+ {
+ case PLAYER_LASER_CODE:
+ if (pHdr->subtype ==
PLAYER_LASER_DATA_SCANPOSE) {
+ this->WriteLaserScanPose(pHdr,
(player_laser_data_scanpose_t*)pData);
}
+ else {
+ this->WriteLaserScan(pHdr,
(player_laser_data_t*)pData);
+ }
+ break;
+ case PLAYER_POSITION2D_CODE:
+ this->WritePosition(pHdr,
(player_position2d_data_t*)pData);
+ break;
+ case PLAYER_SONAR_CODE:
+ this->WriteSonarScan(pHdr,
(player_sonar_data_t*)pData);
+ break;
+ default:
+ // do nothing ignore the message
+ PLAYER_ERROR1("Don't know this interface:
%d\n",iface.interf);
+ break;
}
- }
}
- return(0);
+ return 0;
}
@@ -302,109 +366,95 @@
int KartoLogger::Shutdown()
{
// Stop the driver thread.
- this->enable=false;
-
- KartoLoggerDestroy();
+ this->enableLogger=false;
CloseLog();
- printf("KartoLogger has been shutdown\n");
- return(0);
+ printf("KartoWriter has been shutdown\n");
+ return 0;
}
//
-// KartoLogger::KartoLoggerDestroy : Delete instances of LogDevices
+// KartoLogger::OpenLog()
//
-int KartoLogger::KartoLoggerDestroy()
-{
- list<LogDevice*>::iterator iter;
- for (iter=devices.begin(); iter != devices.end(); iter++){
- Unsubscribe((*iter)->device);
- delete (*iter);
- }
- return(0);
-}
-
-//
-// KartoLogger:
-//
int KartoLogger::OpenLog() {
- this->karto_file = fopen(this->karto_filename, "w+");
- if(this->karto_file == NULL)
+ this->kartoFile = fopen(this->kartoFilename, "w+");
+ if(this->kartoFile == NULL)
{
- PLAYER_ERROR2("unable to open [%s]: %s\n",
this->karto_filename, strerror(errno));
- return(-1);
+ PLAYER_ERROR2("unable to open [%s]: %s\n", this->kartoFilename,
strerror(errno));
+ return -1;
}
time_t t;
time(&t);
- GlobalTime->GetTimeDouble(&(this->start_time));
- fprintf(this->karto_file,"<?xml version='1.0'
encoding='utf-8'?>\n<!DOCTYPE KartoLogger SYSTEM
\"http://karto.ai.sri.com/dtd/KartoLogger.dtd\"; >\n<KartoLogger
version=\"1.0\">\n<UTCTime>\n\t%s</UTCTime>\n",ctime(&t));
- return(0);
+ GlobalTime->GetTimeDouble(&(this->startTime));
+ fprintf(this->kartoFile,"<?xml version='1.0'
encoding='utf-8'?>\n<!DOCTYPE KartoLogger SYSTEM
\"http://karto.ai.sri.com/dtd/KartoLogger.dtd\"; >\n<KartoLogger
version=\"1.0\">\n<UTCTime>\n\t%s</UTCTime>\n",ctime(&t));
+ return 0;
}
//
-// KartoLogger:
+// KartoLogger::CloseLog()
//
int KartoLogger::CloseLog() {
- if(this->karto_file)
+ if(this->kartoFile)
{
- fprintf(this->karto_file,"</DeviceStates>\n</KartoLogger>\n");
- fflush(this->karto_file);
- fclose(this->karto_file);
+ fprintf(this->kartoFile,"</DeviceStates>\n</KartoLogger>\n");
+ fflush(this->kartoFile);
+ fclose(this->kartoFile);
if (this->compress)
printf("Sorry gzip compression is not yet
implemented\n");
- this->karto_file = NULL;
+ this->kartoFile = NULL;
}
- return(0);
+ return 0;
}
//
//KartoLogger::Setup Initialize KartoLogger using the KartoLoggerInit function
+// return code is 0 everything is fine, -1 error
//
-int
- KartoLogger::Setup()
+int KartoLogger::Setup()
{
- //int ret;
- // Open Log
- OpenLog();
-
- fprintf(this->karto_file,"<DeviceList>\n");
- list<LogDevice*>::iterator iter;
- for (iter=devices.begin(); iter != devices.end(); iter++)
+ int ret;
+ ret = OpenLog();
+ if(ret != 0)
+ {
+ return ret;
+ }
+ fprintf(this->kartoFile,"<DeviceList>\n");
+ for (list<LogDevice*>::iterator iter = devices.begin(); iter !=
devices.end(); iter++)
{
- (*iter)->device = deviceTable->GetDevice((*iter)->addr);
- printf(" Setup for %s\n",(*iter)->uid);
- if (!(*iter)->device) {
- PLAYER_ERROR("unable to locate suitable position
device");
- return(-1);
+ (*iter)->pDevice = deviceTable->GetDevice((*iter)->addr);
+ printf(" Setup for %s\n",(*iter)->pUid);
+ if (!(*iter)->pDevice) {
+ PLAYER_ERROR("unable to locate suitable device");
+ return -1;
}
- if((*iter)->device->Subscribe(this->InQueue) != 0)
+ if((*iter)->pDevice->Subscribe(this->InQueue) != 0)
{
- PLAYER_ERROR("unable to subscribe to position device");
- return(-1);
+ PLAYER_ERROR("unable to subscribe to device");
+ return -1;
}
- this->logGeometry((*iter));
+ this->WriteGeometry((*iter));
}
- fprintf(this->karto_file,"</DeviceList>\n<DeviceStates>\n");
+ fprintf(this->kartoFile,"</DeviceList>\n<DeviceStates>\n");
// Start device thread
this->StartThread();
- return(0);
+ return ret;
}
//
-// KartoLogger:
+// KartoLogger::Destructor
//
KartoLogger::~KartoLogger()
{
- //Shutdown();
+ Shutdown();
}
//
// KartoLogger:KartoLogger constructor
-//
-KartoLogger::KartoLogger( ConfigFile* cf, int section) :
-Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,PLAYER_LOG_CODE )
+// TODO:initialize all variables
+KartoLogger::KartoLogger( ConfigFile* cf, int section) : Driver(cf, section,
false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,PLAYER_LOG_CODE ),
+ kartoFile(NULL),
debug(false),enableLogger(false),
+
compress(false), startTime(0.0)
{
-
player_devaddr_t addr;
for (int i = 0; i < cf->GetTupleCount(section, "requires"); i++)
@@ -414,21 +464,28 @@
this->SetError(-1);
return;
}
- devices.push_front(new LogDevice(&addr));
+ LogDevice* pDev = new LogDevice(addr);
+ if (pDev != NULL)
+ {
+ devices.push_front(pDev);
+ }
+ else
+ {
+ PLAYER_ERROR("Can't create LogDevice (out of memory)");
+ }
}
-
- strncpy(karto_filename,
- cf->ReadString(section, "file","output.xml"),
- sizeof(karto_filename));
- if (cf->ReadInt(section, "debug",0) == 1)
+ strncpy(kartoFilename,
+ cf->ReadString(section, "file", "output.xml"),
+ sizeof(kartoFilename));
+ if (cf->ReadInt(section, "debug", 0) == 1)
debug = true;
- if(cf->ReadInt(section, "autorecord", 1) > 0)
- this->enable = true;
+ if (cf->ReadInt(section, "autorecord", 1) > 0)
+ this->enableLogger = true;
else
- this->enable = false;
+ this->enableLogger = false;
- if(cf->ReadInt(section, "gzip", 0) > 0)
+ if (cf->ReadInt(section, "gzip", 0) > 0)
this->compress = true;
else
this->compress = false;
@@ -436,108 +493,118 @@
}
//
-// KartoLogger:
+// KartoLogger_Init
//
// a factory creation function
Driver* KartoLogger_Init( ConfigFile* cf, int section)
{
- return((Driver*)(new KartoLogger( cf, section)));
+ Driver* pDriver = (Driver*)(new KartoLogger(cf, section));
+ assert(pDriver != NULL);
+ return pDriver;
+
}
//
-// KartoLogger:
+// KartoLogger_Register
//
// a driver registration function
-void
- kartowriter_Register(DriverTable* table)
+void kartowriter_Register(DriverTable* table)
{
- table->AddDriver("kartowriter", KartoLogger_Init);
+ table->AddDriver(DRIVER_NAME, KartoLogger_Init);
}
//
// KartoLogger:
//
-int KartoLogger::logGeometry(LogDevice* dev) {
- Message* msg;
- switch(dev->addr.interf) {
- case PLAYER_LASER_CODE:
- {
- if(!(msg = dev->device->Request(this->InQueue,
+int KartoLogger::WriteGeometry(LogDevice* pDev) {
+ Message* pMsg = NULL;
+ switch(pDev->addr.interf) {
+ case PLAYER_LASER_CODE: {
+ if(!(pMsg = pDev->pDevice->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
PLAYER_LASER_REQ_GET_GEOM,
- NULL, 0, NULL,false)))
+ NULL, 0, NULL, false)))
{
PLAYER_ERROR("failed to get laser geometry");
return -1;
}
// Store the pose
- player_laser_geom_t laser_geom =
*((player_laser_geom_t*)msg->GetPayload());
+ player_laser_geom_t laser_geom =
*((player_laser_geom_t*)pMsg->GetPayload());
char hostname[256];
- packedaddr_to_dottedip(hostname,256,dev->addr.host);
+ packedaddr_to_dottedip(hostname,256,pDev->addr.host);
double x,y,z,w;
- computeQuaternion(laser_geom.pose.pyaw,0,0,&x,&y,&z,&w);
-
fprintf(this->karto_file,"<LaserRangeFinder>\n\t<ID>%s</ID>\n\t<Pose>\n\t\t<Position>\n\t\t\t<X>%.3f</X>\n\t\t\t<Y>0.0</Y>\n\t\t\t<Z>%.3f</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t\t<X>%f</X>\n\t\t\t<Y>%f</Y>\n\t\t\t<Z>%f</Z>\n\t\t\t<W>%f</W>\n\t\t</Orientation>\n\t</Pose>\n</LaserRangeFinder>\n",dev->getUID(),laser_geom.pose.py,laser_geom.pose.px,x,y,z,w);
+ ComputeQuaternion(laser_geom.pose.pyaw,0,0,&x,&y,&z,&w);
+
fprintf(this->kartoFile,"<LaserRangeFinder>\n\t<ID>%s</ID>\n\t<Pose>\n\t\t<Position>\n\t\t\t<X>%.3f</X>\n\t\t\t<Y>0.0</Y>\n\t\t\t<Z>%.3f</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t\t<X>%f</X>\n\t\t\t<Y>%f</Y>\n\t\t\t<Z>%f</Z>\n\t\t\t<W>%f</W>\n\t\t</Orientation>\n\t</Pose>\n</LaserRangeFinder>\n",pDev->getUID(),laser_geom.pose.py,laser_geom.pose.px,x,y,z,w);
+ }
break;
- }
case PLAYER_SONAR_CODE:
- {
// Get the sonar poses
- if(!(msg = dev->device->Request(this->InQueue,
+ if(!(pMsg = pDev->pDevice->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
PLAYER_SONAR_REQ_GET_GEOM,
NULL, 0, NULL,false)))
{
PLAYER_ERROR("failed to get sonar geometry");
- return(-1);
+ return -1;
}
// Store the sonar poses
- // player_sonar_geom_t* sonar_geom =
*((player_sonar_geom_t*)msg->GetPayload());
+ // player_sonar_geom_t* sonar_geom =
*((player_sonar_geom_t*)pMsg->GetPayload());
break;
- }
case PLAYER_POSITION2D_CODE:
- {
-
fprintf(this->karto_file,"<Drive>\n\t<ID>%s</ID>\n\t<Pose>\n\t\t<Position>\n\t\t\t<X>0</X>\n\t\t\t<Y>0</Y>\n\t\t\t<Z>0.0</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t\t<X>0</X>\n\t\t\t<Y>0</Y>\n\t\t\t<Z>0</Z>\n\t\t\t<W>1</W>\n\t\t</Orientation>\n\t</Pose>\n</Drive>\n",dev->getUID());
+
fprintf(this->kartoFile,"<Drive>\n\t<ID>%s</ID>\n\t<Pose>\n\t\t<Position>\n\t\t\t<X>0</X>\n\t\t\t<Y>0</Y>\n\t\t\t<Z>0.0</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t\t<X>0</X>\n\t\t\t<Y>0</Y>\n\t\t\t<Z>0</Z>\n\t\t\t<W>1</W>\n\t\t</Orientation>\n\t</Pose>\n</Drive>\n",pDev->getUID());
break;
- }
}
-
- //delete msg;
- return(0);
+ if (pMsg != NULL)
+ {
+ delete pMsg;
+ }
+ return 0;
}
//
-// KartoLogger:
-//
+// KartoLogger:getUID
+// No need to check for buffer overflow on the sprintf since hostname is max
256, dev.robot is uint32, dev.index is uint16
char* KartoLogger::getUID(player_devaddr_t dev) {
char* id = (char*)malloc(ID_MAX_SIZE);
- char hostname[256];
- packedaddr_to_dottedip(hostname,256,dev.host);
+ if (id == NULL)
+ {
+ return DEFAULT_UID;
+ }
+ char hostname[MAX_HOSTNAME];
+ packedaddr_to_dottedip(hostname,MAX_HOSTNAME,dev.host);
switch (dev.interf) {
case PLAYER_LASER_CODE:
sprintf(id,"%s:%d:laser:%d",hostname,dev.robot,dev.index);
- break;
+ break;
case PLAYER_SONAR_CODE:
- sprintf(id,"%s:%d:sonar:%d",hostname,dev.robot,dev.index);
- break;
+
sprintf(id,"%s:%d:sonar:%d",hostname,dev.robot,dev.index);
+ break;
case PLAYER_POSITION2D_CODE:
- sprintf(id,"%s:%d:position2d:%d",hostname,dev.robot,dev.index);
- break;
+
sprintf(id,"%s:%d:position2d:%d",hostname,dev.robot,dev.index);
+ break;
default:
- sprintf(id,"%s:%d:unknown:%d",hostname,dev.robot,dev.index);
+
sprintf(id,"%s:%d:unknown:%d",hostname,dev.robot,dev.index);
}
- return (id);
+ return id;
}
//
-// KartoLogger:
-//
+// KartoLogger:getUID()
+// No need to check for buffer overflow on the sprintf since hostname is max
256, addr->robot is uint32, addr->index is uint16
char* LogDevice::getUID() {
- char* id = (char*)malloc(ID_MAX_SIZE);
- if (this->uid == NULL) {
- char hostname[256];
- packedaddr_to_dottedip(hostname,256,this->addr.host);
+ if (this->pUid != NULL)
+ {
+ return (this->pUid);
+ }
+ char* id = (char*)malloc(ID_MAX_SIZE);
+ if (id == NULL)
+ {
+ return DEFAULT_UID;
+ }
+ char hostname[MAX_HOSTNAME];
+ packedaddr_to_dottedip(hostname,MAX_HOSTNAME,this->addr.host);
switch (this->addr.interf) {
case PLAYER_LASER_CODE:
sprintf(id,"%s:%d:laser:%d",hostname,this->addr.robot,this->addr.index);
@@ -552,92 +619,122 @@
sprintf(id,"%s:%d:unknown:%d",hostname,this->addr.robot,this->addr.index);
}
return (id);
- }
- else
- return (this->uid);
}
//
// KartoLogger:
//
void
- KartoLogger::WriteLaserScan(player_devaddr_t dev, player_msghdr_t* hdr,
- player_laser_data_t* scan)
+ KartoLogger::WriteLaserScan(player_msghdr_t* pHdr, player_laser_data_t*
pScan)
{
double t;
+ double x,y,z,w;
+ ComputeQuaternion(this->currentPose.pa,0,0,&x,&y,&z,&w);
GlobalTime->GetTimeDouble(&t);
- t = t - this->start_time;
-
fprintf(this->karto_file,"<RangeScan>\n\t<DeviceID>%s</DeviceID>\n\t<Time>%.3lf</Time>\n\t\n\t<MinAngle>%.4f</MinAngle>\n\t<MaxAngle>%.4f</MaxAngle>\n\t<Resolution>%f</Resolution>\n\t\t<DistanceMeasurements>\n",getUID(dev),t,scan->min_angle,
scan->max_angle,scan->resolution);
- for(unsigned int i=0;i<scan->ranges_count;i++)
+ t = t - this->startTime;
+ if (debug) {
+
fprintf(this->kartoFile,"<LocalizedRangeScan>\n\t<DeviceID>%s</DeviceID>\n\t<Time>%.3lf</Time>\n\t\n\t<MinAngle>%.4f</MinAngle>\n\t<MaxAngle>%.4f</MaxAngle>\n\t<Resolution>%f</Resolution>\n\t\t<DistanceMeasurements>\n",getUID(pHdr->addr),t,pScan->min_angle,
pScan->max_angle,pScan->resolution);
+ }
+ else {
+
fprintf(this->kartoFile,"<RangeScan>\n\t<DeviceID>%s</DeviceID>\n\t<Time>%.3lf</Time>\n\t\n\t<MinAngle>%.4f</MinAngle>\n\t<MaxAngle>%.4f</MaxAngle>\n\t<Resolution>%f</Resolution>\n\t\t<DistanceMeasurements>\n",getUID(pHdr->addr),t,pScan->min_angle,
pScan->max_angle,pScan->resolution);
+ }
+ for (unsigned int i = 0; i < pScan->ranges_count; i++)
{
- //
fprintf(this->karto_file,"\t\t<float>%.3f</float>\n\t\t<Intensity>%d</Intensity>\n",scan->ranges[i],scan->intensity[i]);
-
fprintf(this->karto_file,"\t\t<float>%.3f</float>\n",scan->ranges[i]);
+ //
fprintf(this->kartoFile,"\t\t<float>%.3f</float>\n\t\t<Intensity>%d</Intensity>\n",scan->ranges[i],scan->intensity[i]);
+
fprintf(this->kartoFile,"\t\t<float>%.3f</float>\n",pScan->ranges[i]);
}
- fprintf(this->karto_file,"\t\t</DistanceMeasurements>\n</RangeScan>\n");
+ fprintf(this->kartoFile,"\t\t</DistanceMeasurements>\n");
+ if (debug) {
+
fprintf(this->kartoFile,"\t<Pose>\n\t\t<Position>\n\t\t<X>%.3f</X>\n\t\t<Y>0.0</Y>\n\t\t<Z>%.3f</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t<X>%f</X>\n\t\t<Y>%f</Y>\n\t\t<Z>%f</Z>\n\t\t<W>%f</W>\n\t\t</Orientation>\n\t</Pose>\n",this->currentPose.py,this->currentPose.px,x,y,z,w);
+ fprintf(this->kartoFile,"</LocalizedRangeScan>\n");
+ }
+ else {
+ fprintf(this->kartoFile,"</RangeScan>\n");
+ }
+
}
//
-// KartoLogger:
+// KartoLogger::WriteLaserScanPose
//
void
- KartoLogger::WritePosition(player_devaddr_t dev, player_msghdr_t* hdr,
player_position2d_data_t* data)
+KartoLogger::WriteLaserScanPose(player_msghdr_t* pHdr,
player_laser_data_scanpose_t* pScan)
{
double t;
GlobalTime->GetTimeDouble(&t);
- t = t - this->start_time;
- this->position_pose = data->pos;
double x,y,z,w;
- computeQuaternion(this->position_pose.pa,0,0,&x,&y,&z,&w);
-
fprintf(this->karto_file,"<DrivePose>\n\t<DeviceID>%s</DeviceID>\n\t<Time>%.3lf</Time>\n\t<Pose>\n\t\t<Position>\n\t\t<X>%.3f</X>\n\t\t<Y>0.0</Y>\n\t\t<Z>%.3f</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t<X>%f</X>\n\t\t<Y>%f</Y>\n\t\t<Z>%f</Z>\n\t\t<W>%f</W>\n\t\t</Orientation>\n\t</Pose>\n</DrivePose>\n",getUID(dev),t,this->position_pose.py,this->position_pose.px,x,y,z,w);
+ ComputeQuaternion(pScan->pose.pa,0,0,&x,&y,&z,&w);
+ t = t - this->startTime;
+
fprintf(this->kartoFile,"<LocalizedRangeScan>\n\t<DeviceID>%s</DeviceID>\n\t<Time>%.3lf</Time>\n\t\n\t<MinAngle>%.4f</MinAngle>\n\t<MaxAngle>%.4f</MaxAngle>\n\t<Resolution>%f</Resolution>\n\t\t<DistanceMeasurements>\n",getUID(pHdr->addr),t,pScan->scan.min_angle,
pScan->scan.max_angle,pScan->scan.resolution);
+ for (unsigned int i = 0; i < pScan->scan.ranges_count; i++)
+ {
+ //
fprintf(this->kartoFile,"\t\t<float>%.3f</float>\n\t\t<Intensity>%d</Intensity>\n",scan->ranges[i],scan->intensity[i]);
+
fprintf(this->kartoFile,"\t\t<float>%.3f</float>\n",pScan->scan.ranges[i]);
+ }
+ fprintf(this->kartoFile,"\t\t</DistanceMeasurements>\n");
+
fprintf(this->kartoFile,"<Pose>\n\t\t<Position>\n\t\t<X>%.3f</X>\n\t\t<Y>0.0</Y>\n\t\t<Z>%.3f</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t<X>%f</X>\n\t\t<Y>%f</Y>\n\t\t<Z>%f</Z>\n\t\t<W>%f</W>\n\t\t</Orientation>\n\t</Pose>\n",pScan->pose.py,pScan->pose.px,x,y,z,w);
+ fprintf(this->kartoFile,"</LocalizedRangeScan>\n");
}
+//
+// KartoLogger::WritePosition
+//
+void KartoLogger::WritePosition(player_msghdr_t* pHdr,
player_position2d_data_t* pData)
+{
+ double t;
+ GlobalTime->GetTimeDouble(&t);
+ t = t - this->startTime;
+ this->currentPose = pData->pos;
+ double x,y,z,w;
+ ComputeQuaternion(this->currentPose.pa,0,0,&x,&y,&z,&w);
+
fprintf(this->kartoFile,"<DrivePose>\n\t<DeviceID>%s</DeviceID>\n\t<Time>%.3lf</Time>\n\t<Pose>\n\t\t<Position>\n\t\t<X>%.3f</X>\n\t\t<Y>0.0</Y>\n\t\t<Z>%.3f</Z>\n\t\t</Position>\n\t\t<Orientation>\n\t\t<X>%f</X>\n\t\t<Y>%f</Y>\n\t\t<Z>%f</Z>\n\t\t<W>%f</W>\n\t\t</Orientation>\n\t</Pose>\n</DrivePose>\n",getUID(pHdr->addr),t,this->currentPose.py,this->currentPose.px,x,y,z,w);
+}
+
//
-// KartoLogger:
+// KartoLogger::WriteSonarScan
//
-void
- KartoLogger::WriteSonarScan(player_devaddr_t dev, player_msghdr_t* hdr,
- player_sonar_data_t* scan)
+void KartoLogger::WriteSonarScan(player_msghdr_t* pHdr, player_sonar_data_t*
pScan)
{
- fprintf(this->karto_file,"<RangeScan
timestamp=\"%.3lf\">\n\t<SensorID>%s</SensorID>\n\t<RangeCount>%d</RangeCount>\n",hdr->timestamp,getUID(dev),scan->ranges_count);
- for(unsigned int i=0;i<scan->ranges_count;i++)
+ fprintf(this->kartoFile,"<RangeScan
timestamp=\"%.3lf\">\n\t<SensorID>%s</SensorID>\n\t<RangeCount>%d</RangeCount>\n",pHdr->timestamp,getUID(pHdr->addr),pScan->ranges_count);
+ for(unsigned int i = 0; i < pScan->ranges_count; i++)
{
- fprintf(this->karto_file,"\t\t<Range
timestamp=\"%.3lf\">%.3f</Range>\n",hdr->timestamp,scan->ranges[i]);
+ fprintf(this->kartoFile,"\t\t<Range
timestamp=\"%.3lf\">%.3f</Range>\n",pHdr->timestamp,pScan->ranges[i]);
}
- fprintf(this->karto_file,"</RangeScan>\n");
+ fprintf(this->kartoFile,"</RangeScan>\n");
}
//
-// Compute the quaternion value for a give heading
+// KartoLogger::ComputeQuaternion - Compute the quaternion value for a give
heading
//
-void computeQuaternion(double heading, double attitude, double bank , double*
x, double* y, double* z, double* w) {
- double c1,c2,c3,s1,s2,s3;
- c1 = cos(heading/2);
- c2 = cos(attitude/2);
- c3 = cos(bank/2);
- s1 = sin(heading / 2);
- s2 = sin(attitude / 2);
- s3 = sin(bank / 2);
- *w = c1*c2*c3 - s1*s2*s3 ;
- *x = s1* s2* c3 +c1* c2* s3 ;
- *y = s1* c2* c3 + c1* s2* s3;
- *z = c1* s2* c3 - s1* c2 *s3;
+void KartoLogger::ComputeQuaternion(double heading, double attitude, double
bank , double* x, double* y, double* z, double* w) {
+ double c1 = cos(heading / 2.0);
+ double c2 = cos(attitude / 2.0);
+ double c3 = cos(bank / 2.0);
+ double s1 = sin(heading / 2.0);
+ double s2 = sin(attitude / 2.0);
+ double s3 = sin(bank / 2.0);
+ *w = (c1 * c2 * c3) - (s1 * s2 * s3);
+ *x = (s1 * s2 * c3) + (c1 * c2 * s3);
+ *y = (s1 * c2 * c3) + (c1 * s2 * s3);
+ *z = (c1 * s2 * c3) - (s1 * c2 * s3);
}
-#if 0
+
////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.
/* need the extern to avoid C++ name-mangling */
+/*
extern "C" {
int player_driver_init(DriverTable* table)
{
puts("KartoLogger driver initializing");
KartoLogger_Register(table);
puts("KartoLogger initialization done");
- return(0);
+ return 0;
}
}
-#endif
+*/
Modified: code/player/branches/cmake/server/drivers/shell/writelog.cc
===================================================================
--- code/player/branches/cmake/server/drivers/shell/writelog.cc 2008-04-18
07:36:45 UTC (rev 6357)
+++ code/player/branches/cmake/server/drivers/shell/writelog.cc 2008-04-18
07:51:55 UTC (rev 6358)
@@ -1028,8 +1028,13 @@
scanpose->scan.ranges_count);
for (i = 0; i < scanpose->scan.ranges_count; i++)
- fprintf(this->file, "%.3f %2d ",
- scanpose->scan.ranges[i], scanpose->scan.intensity[i]);
+ {
+ fprintf(this->file, "%.3f ", scanpose->scan.ranges[i]);
+ if(i < scanpose->scan.intensity_count)
+ fprintf(this->file, "%2d ", scanpose->scan.intensity[i]);
+ else
+ fprintf(this->file, "%2d ", 0);
+ }
return(0);
default:
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 2008 JavaOne(SM) Conference
Don't miss this year's exciting event. There's still time to save $100.
Use priority code J8TL2D2.
http://ad.doubleclick.net/clk;198757673;13503038;p?http://java.sun.com/javaone
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
