From: <gb...@us...> - 2009-08-26 08:24:05
|
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. |