Revision: 8228
http://playerstage.svn.sourceforge.net/playerstage/?rev=8228&view=rev
Author: gbiggs
Date: 2009-08-26 08:23:52 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Applied patch #2843865: added ranger to writelog/readlog
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-08-26 08:19:56 UTC
(rev 8227)
+++ code/player/trunk/server/drivers/shell/readlog.cc 2009-08-26 08:23:52 UTC
(rev 8228)
@@ -57,6 +57,7 @@
The readlog driver can provide the following device interfaces.
- @ref interface_laser
+- @ref interface_ranger
- @ref interface_position2d
- @ref interface_sonar
- @ref interface_wifi
@@ -158,6 +159,7 @@
#define strdup _strdup
#endif
+
#if 0
// we use this pointer to reset timestamps in the client objects when the
// log gets rewound
@@ -206,6 +208,11 @@
player_msghdr_t * hdr,
void * data);
+ // Process ranger interface configuration requests
+ private: int ProcessRangerConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data);
+
// Process sonar interface configuration requests
private: int ProcessSonarConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
@@ -269,6 +276,12 @@
int linenum,
int token_count, char **tokens, double time);
+ // Parse ranger data
+ private: int ParseRanger(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time);
+
// Parse localize data
private: int ParseLocalize(player_devaddr_t id, unsigned short type,
unsigned short subtype,
@@ -395,6 +408,12 @@
// Should we auto-rewind? This is set in the log devie in the .cfg
// file, and defaults to false
public: bool autorewind;
+
+ private: typedef struct {
+ player_ranger_geom_t* geom;
+ player_ranger_config_t* config;
+ } ranger_meta_t;
+
};
@@ -1024,6 +1043,76 @@
}
int
+ReadLog::ProcessRangerConfig(QueuePointer & resp_queue,
+ player_msghdr_t * hdr,
+ void * data)
+{
+ switch(hdr->subtype)
+ {
+ case PLAYER_RANGER_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,
+ ((ranger_meta_t*)this->provide_metadata[j])->geom,
+ sizeof(player_ranger_geom_t),
+ NULL);
+ return(0);
+ }
+
+ case PLAYER_RANGER_REQ_GET_CONFIG:
+ {
+ // 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,
+ ((ranger_meta_t*)this->provide_metadata[j])->config,
+ sizeof(player_ranger_config_t),
+ NULL);
+ return(0);
+ }
+
+ default:
+ return(-1);
+ }
+}
+
+int
ReadLog::ProcessSonarConfig(QueuePointer & resp_queue,
player_msghdr_t * hdr,
void * data)
@@ -1147,6 +1236,11 @@
return(this->ProcessLaserConfig(resp_queue, hdr, data));
}
else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
+ (hdr->addr.interf == PLAYER_RANGER_CODE))
+ {
+ return(this->ProcessRangerConfig(resp_queue, hdr, data));
+ }
+ else if((hdr->type == PLAYER_MSGTYPE_REQ) &&
(hdr->addr.interf == PLAYER_SONAR_CODE))
{
return(this->ProcessSonarConfig(resp_queue, hdr, data));
@@ -1258,6 +1352,9 @@
if (id.interf == PLAYER_LASER_CODE)
return this->ParseLaser(id, type, subtype, linenum,
token_count, tokens, time);
+ if (id.interf == PLAYER_RANGER_CODE)
+ return this->ParseRanger(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);
@@ -1681,55 +1778,7 @@
return ret;
}
- case PLAYER_LASER_DATA_SCANANGLE:
- {
- player_laser_data_scanangle_t data;
- if (token_count < 13)
- {
- PLAYER_ERROR2("incomplete line at %s:%d",
- this->filename,
linenum);
- return -1;
- }
-
- data.id = atoi(tokens[7]);
- data.max_range = static_cast<float> (atof(tokens[8]));
- data.ranges_count = atoi(tokens[9]);
- data.intensity_count = data.ranges_count;
- data.angles_count = data.ranges_count;
-
- data.ranges = new float[ data.ranges_count ];
- data.intensity = new uint8_t[ data.ranges_count ];
- data.angles = new float[ data.ranges_count ];
-
- count = 0;
- for (i = 10; i < token_count; i += 3)
- {
- data.ranges[count] = static_cast<float>
(atof(tokens[i + 0]));
- data.angles[count] = static_cast<float>
(atof(tokens[i + 1]));
- data.intensity[count] = atoi(tokens[i + 2]);
- count += 1;
- }
-
- if (count != (int)data.ranges_count)
- {
- PLAYER_ERROR2("range count mismatch at %s:%d",
- this->filename,
linenum);
- ret = -1;
- }
- else
- {
- this->Publish(id, static_cast<uint8_t>
(type), static_cast<uint8_t> (subtype),
- (void*)&data,
sizeof(data), &time);
- }
- delete [] data.ranges;
- delete [] data.intensity;
- delete [] data.angles;
-
- return ret;
- }
-
-
default:
PLAYER_ERROR1("unknown laser data subtype %d\n", subtype);
return(-1);
@@ -1790,7 +1839,631 @@
}
+////////////////////////////////////////////////////////////////////////////
+// Parse ranger data
+int ReadLog::ParseRanger(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time)
+{
+ int i, count, ret;
+ ret = 0;
+ switch(type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ switch(subtype)
+ {
+ case PLAYER_RANGER_DATA_RANGE:
+ {
+ player_ranger_data_range_t data;
+ if (token_count < 8)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ data.ranges_count = atoi(tokens[7]);
+
+ data.ranges = new double[ data.ranges_count ];
+
+ count = 0;
+ for (i = 8; i < token_count; i++)
+ {
+ data.ranges[count] = static_cast<double> (atof(tokens[i + 0]));
+ count++;
+ }
+
+ if (count != (int)data.ranges_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ ret = -1;
+ }
+ else
+ {
+ this->Publish(id, static_cast<uint8_t> (type),
static_cast<uint8_t> (subtype),
+ (void*)&data, sizeof(data), &time);
+ }
+ delete [] data.ranges;
+
+ return ret;
+ }
+
+ case PLAYER_RANGER_DATA_RANGESTAMPED:
+ {
+ player_ranger_data_rangestamped_t data;
+
+ if (token_count < 10)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int total_count=7;
+ data.data.ranges_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.data.ranges = new double[ data.data.ranges_count ];
+
+ count = 0;
+ int loop_size=fmin(token_count, total_count+data.data.ranges_count);
+ for (i = total_count; i < loop_size; i += 2)
+ {
+ data.data.ranges[count] = static_cast<double> (atof(tokens[i]));
+ count++;
+ total_count++;
+ }
+
+ if (count != (int)data.data.ranges_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ return -1;
+ }
+
+ data.have_geom = atoi(tokens[total_count]);
+ total_count++;
+
+ if (data.have_geom)
+ {
+ if (token_count < total_count+11)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ return -1;
+ }
+
+ data.geom.pose.px = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.py = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pz = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.proll = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.ppitch = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pyaw = static_cast<double>
(atof(tokens[total_count]));
+
+ total_count++;
+ data.geom.size.sw = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sl = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sh = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+
+ data.geom.element_poses_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_poses = new player_pose3d_t [
data.geom.element_poses_count ];
+
+ count = 0;
+ loop_size=fmin(token_count,
total_count+data.geom.element_poses_count*6);
+ for (i = total_count; i < loop_size; i += 6)
+ {
+ data.geom.element_poses[count].px = static_cast<double>
(atof(tokens[i]));
+ total_count++;
+ data.geom.element_poses[count].py = static_cast<double>
(atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_poses[count].pz = static_cast<double>
(atof(tokens[i+2]));
+ total_count++;
+ data.geom.element_poses[count].proll = static_cast<double>
(atof(tokens[i+3]));
+ total_count++;
+ data.geom.element_poses[count].ppitch = static_cast<double>
(atof(tokens[i+4]));
+ total_count++;
+ data.geom.element_poses[count].pyaw = static_cast<double>
(atof(tokens[i+5]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_poses_count || total_count
> token_count)
+ {
+ PLAYER_ERROR2("poses count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ delete [] data.geom.element_poses;
+ return -1;
+ }
+
+
+ data.geom.element_sizes_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_sizes = new player_bbox3d_t [
data.geom.element_sizes_count ];
+
+ count = 0;
+ loop_size=fmin(token_count,
total_count+data.geom.element_sizes_count*3);
+ for (i = total_count; i < loop_size; i += 3)
+ {
+ data.geom.element_sizes[count].sw = static_cast<double>
(atof(tokens[i]));
+ total_count++;
+ data.geom.element_sizes[count].sl = static_cast<double>
(atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_sizes[count].sh = static_cast<double>
(atof(tokens[i+2]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_sizes_count || total_count
> token_count)
+ {
+ PLAYER_ERROR2("sizes count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ return -1;
+ }
+ }
+
+ data.have_config = atoi(tokens[total_count]);
+
+ if (data.have_config)
+ {
+
+ if (token_count < total_count+7)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.ranges;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ data.config.min_angle = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.max_angle = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.angular_res = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.min_range = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.max_range = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.range_res = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.frequency = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ }
+
+ if (total_count != token_count)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d: number of tokens does not
"
+ "match count", filename, linenum);
+ delete [] data.data.ranges;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t>
(subtype),
+ (void*)&data, sizeof(data), &time);
+ delete [] data.data.ranges;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+
+ return ret;
+ }
+
+ case PLAYER_RANGER_DATA_INTNS:
+ {
+ player_ranger_data_intns_t data;
+
+ if (token_count < 8)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ data.intensities_count = atoi(tokens[7]);
+ data.intensities = new double[ data.intensities_count ];
+
+ count = 0;
+ for (i = 8; i < token_count; i++)
+ {
+ data.intensities[count] = static_cast<double> (atof(tokens[i +
0]));
+ count++;
+ }
+
+ if (count != (int)data.intensities_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ ret = -1;
+ }
+ else
+ {
+ this->Publish(id, static_cast<uint8_t> (type),
static_cast<uint8_t> (subtype),
+ (void*)&data, sizeof(data), &time);
+ }
+ delete [] data.intensities;
+
+ return ret;
+ }
+
+ case PLAYER_RANGER_DATA_INTNSSTAMPED:
+ {
+ player_ranger_data_intnsstamped_t data;
+
+ if (token_count < 10)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int total_count=7;
+ data.data.intensities_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.data.intensities = new double[ data.data.intensities_count ];
+
+ count = 0;
+ int loop_size=fmin(token_count,
total_count+data.data.intensities_count);
+ for (i = total_count; i < loop_size; i += 2)
+ {
+ data.data.intensities[count] = static_cast<double>
(atof(tokens[i]));
+ count++;
+ total_count++;
+ }
+
+ if (count != (int)data.data.intensities_count)
+ {
+ PLAYER_ERROR2("range count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ return -1;
+ }
+
+ data.have_geom = atoi(tokens[total_count]);
+ total_count++;
+
+ if (data.have_geom)
+ {
+ if (token_count < total_count+11)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ return -1;
+ }
+
+ data.geom.pose.px = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.py = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pz = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.proll = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.ppitch = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.pose.pyaw = static_cast<double>
(atof(tokens[total_count]));
+
+ total_count++;
+ data.geom.size.sw = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sl = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.geom.size.sh = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+
+ data.geom.element_poses_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_poses = new player_pose3d_t [
data.geom.element_poses_count ];
+
+ count = 0;
+ loop_size=fmin(token_count,
total_count+data.geom.element_poses_count*6);
+ for (i = total_count; i < loop_size; i += 6)
+ {
+ data.geom.element_poses[count].px = static_cast<double>
(atof(tokens[i]));
+ total_count++;
+ data.geom.element_poses[count].py = static_cast<double>
(atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_poses[count].pz = static_cast<double>
(atof(tokens[i+2]));
+ total_count++;
+ data.geom.element_poses[count].proll = static_cast<double>
(atof(tokens[i+3]));
+ total_count++;
+ data.geom.element_poses[count].ppitch = static_cast<double>
(atof(tokens[i+4]));
+ total_count++;
+ data.geom.element_poses[count].pyaw = static_cast<double>
(atof(tokens[i+5]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_poses_count || total_count
> token_count)
+ {
+ PLAYER_ERROR2("poses count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ delete [] data.geom.element_poses;
+ return -1;
+ }
+
+
+ data.geom.element_sizes_count = atoi(tokens[total_count]);
+ total_count++;
+
+ data.geom.element_sizes = new player_bbox3d_t [
data.geom.element_sizes_count ];
+
+ count = 0;
+ loop_size=fmin(token_count,
total_count+data.geom.element_sizes_count*3);
+ for (i = total_count; i < loop_size; i += 3)
+ {
+ data.geom.element_sizes[count].sw = static_cast<double>
(atof(tokens[i]));
+ total_count++;
+ data.geom.element_sizes[count].sl = static_cast<double>
(atof(tokens[i+1]));
+ total_count++;
+ data.geom.element_sizes[count].sh = static_cast<double>
(atof(tokens[i+2]));
+ total_count++;
+ count++;
+ }
+
+ if (count != (int)data.geom.element_sizes_count || total_count
> token_count)
+ {
+ PLAYER_ERROR2("sizes count mismatch at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ return -1;
+ }
+ }
+
+ data.have_config = atoi(tokens[total_count]);
+
+ if (data.have_config)
+ {
+
+ if (token_count < total_count+7)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ delete [] data.data.intensities;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ data.config.min_angle = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.max_angle = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.angular_res = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.min_range = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.max_range = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.range_res = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ data.config.frequency = static_cast<double>
(atof(tokens[total_count]));
+ total_count++;
+ }
+
+ if (total_count != token_count)
+ {
+ PLAYER_ERROR2("invalid line at %s:%d: number of tokens does not
"
+ "match count", filename, linenum);
+ delete [] data.data.intensities;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return -1;
+ }
+
+ this->Publish(id, static_cast<uint8_t> (type), static_cast<uint8_t>
(subtype),
+ (void*)&data, sizeof(data), &time);
+ delete [] data.data.intensities;
+ if (data.have_geom)
+ {
+ delete [] data.geom.element_poses;
+ delete [] data.geom.element_sizes;
+ }
+ return ret;
+ }
+
+ default:
+ PLAYER_ERROR1("unknown ranger data subtype %d\n", subtype);
+ return(-1);
+ }
+ break;
+
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(subtype)
+ {
+ case PLAYER_RANGER_REQ_GET_GEOM:
+ {
+ if(token_count < 18)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int num_poses=atoi(tokens[16]);
+
+ if(token_count < 18+num_poses*6)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ int num_sizes=atoi(tokens[17+num_poses*6]);
+
+ if(token_count < 18+num_poses*6+num_sizes*3)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+
+ // cache it
+ player_ranger_geom_t* geom =
+
(player_ranger_geom_t*)calloc(1,sizeof(player_ranger_geom_t)+sizeof(player_pose3d_t)*num_poses+sizeof(player_bbox3d_t)*num_sizes);
+ 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.sw = atof(tokens[13]);
+ geom->size.sl = atof(tokens[14]);
+ geom->size.sh = atof(tokens[15]);
+ geom->element_poses_count = num_poses;
+ geom->element_poses = new player_pose3d_t [ num_poses ];
+ geom->element_sizes_count = num_sizes;
+ geom->element_sizes = new player_bbox3d_t [ num_sizes ];
+
+ for (int i=0; i < num_poses; i++)
+ {
+ geom->element_poses[i].px=atof(tokens[17+i*6]);
+ geom->element_poses[i].py=atof(tokens[17+i*6+1]);
+ geom->element_poses[i].pz=atof(tokens[17+i*6+2]);
+ geom->element_poses[i].proll=atof(tokens[17+i*6+3]);
+ geom->element_poses[i].ppitch=atof(tokens[17+i*6+4]);
+ geom->element_poses[i].pyaw=atof(tokens[17+i*6+5]);
+ }
+
+ for (int i=0; i < num_sizes; i++)
+ {
+ geom->element_sizes[i].sw=atof(tokens[17+num_poses*6+1+i*3]);
+ geom->element_sizes[i].sl=atof(tokens[17+num_poses*6+1+i*3+1]);
+ geom->element_sizes[i].sh=atof(tokens[17+num_poses*6+1+i*3+2]);
+ }
+
+
+ // 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 something is already here, use it
+ if(this->provide_metadata[j])
+ ((ranger_meta_t*)this->provide_metadata[j])->geom = geom;
+ else
+ {
+ ranger_meta_t* meta =
+ (ranger_meta_t*)calloc(1,sizeof(ranger_meta_t));
+ meta->geom=geom;
+ this->provide_metadata[j] = (void*)meta;
+ }
+
+ // nothing to publish
+ return(0);
+ }
+
+ case PLAYER_RANGER_REQ_GET_CONFIG:
+ {
+ if(token_count < 14)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+ // cache it
+ player_ranger_config_t* config =
+ (player_ranger_config_t*)calloc(1,sizeof(player_ranger_config_t));
+ assert(config);
+
+ config->min_angle = atof(tokens[7]);
+ config->max_angle = atof(tokens[8]);
+ config->angular_res = atof(tokens[9]);
+ config->min_range = atof(tokens[10]);
+ config->max_range = atof(tokens[11]);
+ config->range_res = atof(tokens[12]);
+ config->frequency = atof(tokens[13]);
+
+ // 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 something is already here, use it
+ if(this->provide_metadata[j]) {
+ ((ranger_meta_t*)this->provide_metadata[j])->config = config;
+ }
+ else
+ {
+ ranger_meta_t* meta =
+ (ranger_meta_t*)calloc(1,sizeof(ranger_meta_t));
+ meta->config=config;
+ this->provide_metadata[j] = (void*)meta;
+ }
+
+ // nothing to publish
+ return(0);
+ }
+
+ default:
+ PLAYER_ERROR1("unknown ranger reply subtype %d\n", subtype);
+ return(-1);
+ }
+ break;
+
+ default:
+ PLAYER_ERROR1("unknown ranger msg type %d\n", type);
+ return(-1);
+ }
+}
+
+
+
////////////////////////////////////////////////////////////////////////////
// Parse localize data
int ReadLog::ParseLocalize(player_devaddr_t id,
Modified: code/player/trunk/server/drivers/shell/writelog.cc
===================================================================
--- code/player/trunk/server/drivers/shell/writelog.cc 2009-08-26 08:19:56 UTC
(rev 8227)
+++ code/player/trunk/server/drivers/shell/writelog.cc 2009-08-26 08:23:52 UTC
(rev 8228)
@@ -70,6 +70,7 @@
The writelog driver can will log data from the following interfaces:
- @ref interface_laser
+- @ref interface_ranger
- @ref interface_sonar
- @ref interface_position2d
- @ref interface_ptz
@@ -225,6 +226,9 @@
// Write laser data to file
private: int WriteLaser(player_msghdr_t* hdr, void *data);
+ // Write ranger data to file
+ private: int WriteRanger(player_msghdr_t* hdr, void *data);
+
// Write localize data to file
private: int WriteLocalize(player_msghdr_t* hdr, void *data);
@@ -585,9 +589,42 @@
delete msg;
}
}
+ else if (device->addr.interf == PLAYER_RANGER_CODE)
+ {
+ // Get the ranger geometry
+ Message* msg;
+ if(!(msg = device->device->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_GET_GEOM,
+ NULL, 0, NULL, true)))
+ {
+ // oh well.
+ PLAYER_WARN("unable to get ranger geometry");
+ }
+ else
+ {
+ // log it
+ this->Write(device, msg->GetHeader(), msg->GetPayload());
+ delete msg;
+ }
+ if(!(msg = device->device->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+ PLAYER_RANGER_REQ_GET_CONFIG,
+ NULL, 0, NULL, true)))
+ {
+ // oh well.
+ PLAYER_WARN("unable to get ranger config");
+ }
+ else
+ {
+ // log it
+ this->Write(device, msg->GetHeader(), msg->GetPayload());
+ delete msg;
+ }
+ }
else if (device->addr.interf == PLAYER_POSITION2D_CODE)
{
- // Get the laser geometry
+ // Get the position geometry
Message* msg;
if(!(msg = device->device->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
@@ -607,7 +644,7 @@
/* HHAA 15-02-2007 */
else if (device->addr.interf == PLAYER_BUMPER_CODE)
{
- // Get the laser geometry
+ // Get the bumper geometry
Message* msg;
if(!(msg = device->device->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
@@ -627,7 +664,7 @@
/* HHAA 15-02-2007 */
else if (device->addr.interf == PLAYER_IR_CODE)
{
- // Get the laser geometry
+ // Get the IR geometry
Message* msg;
if(!(msg = device->device->Request(this->InQueue,
PLAYER_MSGTYPE_REQ,
@@ -827,6 +864,9 @@
case PLAYER_LASER_CODE:
retval = this->WriteLaser(hdr, data);
break;
+ case PLAYER_RANGER_CODE:
+ retval = this->WriteRanger(hdr, data);
+ break;
case PLAYER_LOCALIZE_CODE:
retval = this->WriteLocalize(hdr, data);
break;
@@ -1039,22 +1079,22 @@
}
return(0);
- case PLAYER_LASER_DATA_SCANANGLE:
- scanangle = (player_laser_data_scanangle_t*)data;
- fprintf(this->file, "%04d %+07.4f %04d ",
- scanangle->id, scanangle->max_range,
scanangle->ranges_count);
-
- for (i = 0; i < scanangle->ranges_count; i++)
- {
- fprintf(this->file, "%.3f ",
scanangle->ranges[i]);
- fprintf(this->file, "%.3f ",
scanangle->angles[i]);
- if(i < scanangle->intensity_count)
- fprintf(this->file, "%2d ",
scanangle->intensity[i]);
- else
- fprintf(this->file, "%2d ", 0);
- }
- return(0);
-
+ case PLAYER_LASER_DATA_SCANANGLE:
+ scanangle = (player_laser_data_scanangle_t*)data;
+ fprintf(this->file, "%04d %+07.4f %04d ",
+ scanangle->id, scanangle->max_range, scanangle->ranges_count);
+
+ for (i = 0; i < scanangle->ranges_count; i++)
+ {
+ fprintf(this->file, "%.3f ", scanangle->ranges[i]);
+ fprintf(this->file, "%.3f ", scanangle->angles[i]);
+ if(i < scanangle->intensity_count)
+ fprintf(this->file, "%2d ", scanangle->intensity[i]);
+ else
+ fprintf(this->file, "%2d ", 0);
+ }
+ return(0);
+
default:
return(-1);
}
@@ -1078,12 +1118,334 @@
}
}
+
/** @ingroup tutorial_datalog
+ * @defgroup player_driver_writelog_ranger ranger format
+
+...@brief ranger log format
+
+The following type:subtype ranger messages can be logged:
+- 1:1 (PLAYER_RANGER_DATA_RANGE) - A range scan. The format is:
+ - ranges_count (uint): number of ranges
+ - list of ranges_count ranges:
+ - range (double): distance
+
+- 1:2 (PLAYER_RANGER_DATA_RANGEPOSE) - A range scan optionally with
+the (possibly estimated) geometry of the device when the scan was
+acquired and optional sensor configuration. The format is:
+ - ranges_count (uint): number of ranges
+ - list of ranges_count ranges:
+ - range (double): distance
+ - have_geom (uint8): If non-zero, the geometry data has been filled
+ - geometry of device at the time of range data:
+ - pose of device:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - size of device:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - element_poses_count (uint): pose of each individual range sensor that
makes up the device
+ - list of element_poses_count poses:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - element_sizes_count (uint): size of each individual range sensor that
makes up the device
+ - list of element_sizes_count sizes:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - 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
+
+- 1:3 (PLAYER_RANGER_DATA_INTNS) - An intensity scan. The format is:
+ - intensities_count (uint): number of intensities
+ - list of intensities_count intensities:
+ - intensity (double)
+
+- 1:4 (PLAYER_RANGER_DATA_ITNSPOSE) - An intensity scan with an attached pose
(estimated from the time of the scan). The format is:
+ - intensities_count (uint): number of intensities
+ - list of intensities_count intensities:
+ - intensity (double)
+ - have_geom (uint8): If non-zero, the geometry data has been filled
+ - geometry of device at the time of intensity data:
+ - pose of device:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - size of device:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - element_poses_count (uint): pose of each individual range sensor that
makes up the device
+ - list of element_poses_count poses:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - element_sizes_count (uint): size of each individual range sensor that
makes up the device
+ - list of element_sizes_count sizes:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - have_config(uint8): If non-zero, the config data has been filled
+ - config of device:
+
+- 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
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - size of device:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+ - element_poses_count (uint): pose of each individual range sensor that
makes up the device
+ - list of element_poses_count poses:
+ - px (float): X coordinate of the pose, in meters
+ - py (float): Y coordinate of the pose, in meters
+ - pz (float): Z coordinate of the pose, in meters
+ - proll (float): roll coordinate of the pose, in radians
+ - ppitch (float): pitch coordinate of the pose, in radians
+ - pyaw (float): yaw coordinate of the pose, in radians
+ - element_sizes_count (uint): size of each individual range sensor that
makes up the device
+ - list of element_sizes_count sizes:
+ - sw (float): width of the device, in meters
+ - sl (float): length of the device, in meters
+ - sh (float): height of the device, in meters
+*/
+int
+WriteLog::WriteRanger(player_msghdr_t* hdr, void *data)
+{
+ size_t i;
+ player_ranger_data_range_t* rscan;
+ player_ranger_data_rangestamped_t* rscanpose;
+ player_ranger_data_intns_t* iscan;
+ player_ranger_data_intnsstamped_t* iscanpose;
+ player_ranger_geom_t* geom;
+ player_ranger_config_t* config;
+
+ // Check the type
+ switch(hdr->type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ // Check the subtype
+ switch(hdr->subtype)
+ {
+ case PLAYER_RANGER_DATA_RANGE:
+ rscan = (player_ranger_data_range_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", rscan->ranges_count);
+
+ for (i = 0; i < rscan->ranges_count; i++)
+ {
+ fprintf(this->file, "%.3f ", rscan->ranges[i]);
+ }
+ return(0);
+
+ case PLAYER_RANGER_DATA_RANGESTAMPED:
+ rscanpose = (player_ranger_data_rangestamped_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", rscanpose->data.ranges_count);
+
+ for (i = 0; i < rscanpose->data.ranges_count; i++)
+ {
+ fprintf(this->file, "%.3f ", rscanpose->data.ranges[i]);
+ }
+
+ fprintf(this->file, "%d ", rscanpose->have_geom);
+
+ if (rscanpose->have_geom)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f %+07.3f %+07.3f %+07.3f ",
+ rscanpose->geom.pose.px, rscanpose->geom.pose.py,
rscanpose->geom.pose.pz,
+ rscanpose->geom.pose.proll, rscanpose->geom.pose.ppitch,
rscanpose->geom.pose.pyaw,
+ rscanpose->geom.size.sw, rscanpose->geom.size.sl,
rscanpose->geom.size.sh);
+
+ fprintf(this->file, "%04d ", rscanpose->geom.element_poses_count);
+
+ for (i = 0; i < rscanpose->geom.element_poses_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f ",
+ rscanpose->geom.element_poses[i].px,
rscanpose->geom.element_poses[i].py, rscanpose->geom.element_poses[i].pz,
+ rscanpose->geom.element_poses[i].proll,
rscanpose->geom.element_poses[i].ppitch, rscanpose->geom.element_poses[i].pyaw);
+ }
+
+ fprintf(this->file, "%04d ", rscanpose->geom.element_sizes_count);
+
+ for (i = 0; i < rscanpose->geom.element_sizes_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f ",
+ rscanpose->geom.element_sizes[i].sw,
rscanpose->geom.element_sizes[i].sl, rscanpose->geom.element_sizes[i].sh);
+ }
+ }
+
+ if (rscanpose->have_config)
+ {
+ fprintf(this->file, "%.4f %.4f %.4f %.4f %.4f %.4f %.4f ",
+ rscanpose->config.min_angle, rscanpose->config.max_angle,
+ rscanpose->config.angular_res,
rscanpose->config.min_range,
+ rscanpose->config.max_range, rscanpose->config.range_res,
+ rscanpose->config.frequency);
+ }
+
+ return(0);
+
+
+ case PLAYER_RANGER_DATA_INTNS:
+ iscan = (player_ranger_data_intns_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", iscan->intensities_count);
+
+ for (i = 0; i < iscan->intensities_count; i++)
+ {
+ fprintf(this->file, "%.3f ", iscan->intensities[i]);
+ }
+ return(0);
+
+
+ case PLAYER_RANGER_DATA_INTNSSTAMPED:
+ iscanpose = (player_ranger_data_intnsstamped_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+ fprintf(this->file, "%04d ", iscanpose->data.intensities_count);
+
+ for (i = 0; i < iscanpose->data.intensities_count; i++)
+ {
+ fprintf(this->file, "%.3f ", iscanpose->data.intensities[i]);
+ }
+
+ fprintf(this->file, "%d ", iscanpose->have_geom);
+
+ if (iscanpose->have_geom)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f %+07.3f %+07.3f %+07.3f ",
+ iscanpose->geom.pose.px, iscanpose->geom.pose.py,
iscanpose->geom.pose.pz,
+ iscanpose->geom.pose.proll, iscanpose->geom.pose.ppitch,
iscanpose->geom.pose.pyaw,
+ iscanpose->geom.size.sw, iscanpose->geom.size.sl,
iscanpose->geom.size.sh);
+
+ fprintf(this->file, "%04d ", iscanpose->geom.element_poses_count);
+
+ for (i = 0; i < iscanpose->geom.element_poses_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f ",
+ iscanpose->geom.element_poses[i].px,
iscanpose->geom.element_poses[i].py, iscanpose->geom.element_poses[i].pz,
+ iscanpose->geom.element_poses[i].proll,
iscanpose->geom.element_poses[i].ppitch, iscanpose->geom.element_poses[i].pyaw);
+ }
+
+ fprintf(this->file, "%04d ", iscanpose->geom.element_sizes_count);
+
+ for (i = 0; i < iscanpose->geom.element_sizes_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f ",
+ iscanpose->geom.element_sizes[i].sw,
iscanpose->geom.element_sizes[i].sl, iscanpose->geom.element_sizes[i].sh);
+ }
+ }
+
+ if (iscanpose->have_config)
+ {
+ fprintf(this->file, "%.4f %.4f %.4f %.4f %.4f %.4f %.4f ",
+ iscanpose->config.min_angle, iscanpose->config.max_angle,
+ iscanpose->config.angular_res,
iscanpose->config.min_range,
+ iscanpose->config.max_range, iscanpose->config.range_res,
+ iscanpose->config.frequency);
+ }
+
+ return(0);
+
+ default:
+ return(-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(hdr->subtype)
+ {
+ case PLAYER_RANGER_REQ_GET_GEOM:
+ geom = (player_ranger_geom_t*)data;
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f %+07.3f %+07.3f ",
+ geom->pose.px,
+ geom->pose.py,
+ geom->pose.pz,
+ geom->pose.proll,
+ geom->pose.ppitch,
+ geom->pose.pyaw,
+ geom->size.sw,
+ geom->size.sl,
+ geom->size.sh);
+
+ fprintf(this->file, "%04d ", geom->element_poses_count);
+
+ for (i = 0; i < geom->element_poses_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f %+07.3f %+07.3f
%+07.3f ",
+ geom->element_poses[i].px, geom->element_poses[i].py,
geom->element_poses[i].pz,
+ geom->element_poses[i].proll,
geom->element_poses[i].ppitch, geom->element_poses[i].pyaw);
+ }
+
+ fprintf(this->file, "%04d ", geom->element_sizes_count);
+
+ for (i = 0; i < geom->element_sizes_count; i++)
+ {
+ fprintf(this->file, "%+07.3f %+07.3f %+07.3f ",
+ geom->element_sizes[i].sw, geom->element_sizes[i].sl,
geom->element_sizes[i].sh);
+ }
+
+ return(0);
+
+ case PLAYER_RANGER_REQ_GET_CONFIG:
+ config = (player_ranger_config_t*)data;
+
+ fprintf(this->file, "%lf %lf %lf %lf %lf %lf %lf ",
+ config->min_angle, config->max_angle,
+ config->angular_res, config->min_range,
+ config->max_range, config->range_res,
+ config->frequency);
+
+ return(0);
+
+ default:
+ return(-1);
+ }
+ default:
+ return(-1);
+ }
+}
+
+
+/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_localize localize format
-...@brief laser log format
+...@brief localize log format
-The following type:subtype laser messages can be logged:
+The following type:subtype localize messages can be logged:
- 1:1 (PLAYER_LOCALIZE_DATA_HYPOTHS) - A set of pose hypotheses. The format
is:
- pending_count (int): number of pending (unprocessed observations)
- pending time (float): time stamp of the last observation processed
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Let Crystal Reports handle the reporting - Free Crystal Reports 2008 30-Day
trial. Simplify your report design, integration and deployment - and focus on
what you do best, core application coding. Discover what's new with
Crystal Reports now. http://p.sf.net/sfu/bobj-july
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
