From: <th...@us...> - 2009-06-06 09:51:10
|
Revision: 7802 http://playerstage.svn.sourceforge.net/playerstage/?rev=7802&view=rev Author: thjc Date: 2009-06-06 09:51:04 +0000 (Sat, 06 Jun 2009) Log Message: ----------- applied patch 2609220: Re enable fiducial interface in writelog and readlog drivers Modified Paths: -------------- code/player/trunk/server/drivers/shell/readlog.cc code/player/trunk/server/drivers/shell/writelog.cc Modified: code/player/trunk/server/drivers/shell/readlog.cc =================================================================== --- code/player/trunk/server/drivers/shell/readlog.cc 2009-06-06 09:39:40 UTC (rev 7801) +++ code/player/trunk/server/drivers/shell/readlog.cc 2009-06-06 09:51:04 UTC (rev 7802) @@ -66,13 +66,13 @@ - @ref interface_opaque - @ref interface_ptz - @ref interface_actarray +- @ref interface_fiducial The following interfaces are supported in principle but are currently disabled because they need to be updated: - @ref interface_blobfinder - @ref interface_camera -- @ref interface_fiducial - @ref interface_gps - @ref interface_joystick - @ref interface_position3d @@ -195,6 +195,11 @@ player_msghdr_t * hdr, void * data); + // Process fiducial interface configuration requests + private: int ProcessFiducialConfig(QueuePointer & resp_queue, + player_msghdr_t * hdr, + void * data); + // Process laser interface configuration requests private: int ProcessLaserConfig(QueuePointer & resp_queue, player_msghdr_t * hdr, @@ -225,6 +230,12 @@ int linenum, int token_count, char **tokens, double time); + // Parse fiducial data + private: int ParseFiducial(player_devaddr_t id, + unsigned short type, unsigned short subtype, + int linenum, int token_count, char **tokens, + double time); + #if 0 // Parse blobfinder data private: int ParseBlobfinder(player_devaddr_t id, @@ -238,12 +249,6 @@ int linenum, int token_count, char **tokens, double time); - // Parse fiducial data - private: int ParseFiducial(player_devaddr_t id, - unsigned short type, unsigned short subtype, - int linenum, - int token_count, char **tokens, double time); - // Parse gps data private: int ParseGps(player_devaddr_t id, unsigned short type, unsigned short subtype, @@ -938,6 +943,46 @@ } int +ReadLog::ProcessFiducialConfig(QueuePointer & resp_queue, + player_msghdr_t * hdr, + void * data) +{ + switch(hdr->subtype) + { + case PLAYER_FIDUCIAL_REQ_GET_GEOM: + { + // Find the right place from which to retrieve it + int j; + for(j=0;j<this->provide_count;j++) + { + if(Device::MatchDeviceAddress(this->provide_ids[j], hdr->addr)) + break; + } + if(j>=this->provide_count) + { + puts("no matching device"); + return(-1); + } + + if(!this->provide_metadata[j]) + { + puts("no metadata"); + return(-1); + } + + this->Publish(this->provide_ids[j], resp_queue, + PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, + this->provide_metadata[j], + sizeof(player_fiducial_geom_t), + NULL); + return(0); + } + default: + return(-1); + } +} + +int ReadLog::ProcessLaserConfig(QueuePointer & resp_queue, player_msghdr_t * hdr, void * data) @@ -1091,6 +1136,11 @@ return(this->ProcessLogConfig(resp_queue, hdr, data)); } else if((hdr->type == PLAYER_MSGTYPE_REQ) && + (hdr->addr.interf == PLAYER_FIDUCIAL_CODE)) + { + return(this->ProcessFiducialConfig(resp_queue, hdr, data)); + } + else if((hdr->type == PLAYER_MSGTYPE_REQ) && (hdr->addr.interf == PLAYER_LASER_CODE)) { return(this->ProcessLaserConfig(resp_queue, hdr, data)); @@ -1197,9 +1247,6 @@ else if (id.interf == PLAYER_CAMERA_CODE) return this->ParseCamera(id, type, subtype, linenum, token_count, tokens, time); - else if (id.interf == PLAYER_FIDUCIAL_CODE) - return this->ParseFiducial(id, type, subtype, linenum, - token_count, tokens, time); else if (id.interf == PLAYER_GPS_CODE) return this->ParseGps(id, type, subtype, linenum, token_count, tokens, time); @@ -1210,6 +1257,9 @@ if (id.interf == PLAYER_LASER_CODE) return this->ParseLaser(id, type, subtype, linenum, token_count, tokens, time); + else if (id.interf == PLAYER_FIDUCIAL_CODE) + return this->ParseFiducial(id, type, subtype, linenum, + token_count, tokens, time); else if (id.interf == PLAYER_LOCALIZE_CODE) return this->ParseLocalize(id, type, subtype, linenum, token_count, tokens, time); @@ -1350,48 +1400,114 @@ return 0; } +#endif - //////////////////////////////////////////////////////////////////////////// // Parse fiducial data -int ReadLog::ParseFiducial(player_devaddr_t id, int linenum, - int token_count, char **tokens, struct timeval time) +int ReadLog::ParseFiducial(player_devaddr_t id, + unsigned short type, unsigned short subtype, + int linenum, int token_count, char **tokens, + double time) { - player_fiducial_data_t data; - int fiducial_count; - if (token_count < 7) { PLAYER_ERROR2("incomplete line at %s:%d", this->filename, linenum); return -1; } - fiducial_count = atoi( tokens[6] ); - data.count = NUINT16( fiducial_count ); + switch(type) { + case PLAYER_MSGTYPE_DATA: + switch (subtype) { + case PLAYER_FIDUCIAL_DATA_SCAN: + player_fiducial_data_t data; + int fiducial_count; - for( int i = 0; i < fiducial_count; i++ ) - { - data.fiducials[i].id = NINT16( atof(tokens[13*i + 7]) ); - data.fiducials[i].pos[0] = NINT32(M_MM(atof(tokens[13*i+ 8]))); - data.fiducials[i].pos[1] = NINT32(M_MM(atof(tokens[13*i+ 9]))); - data.fiducials[i].pos[2] = NINT32(M_MM(atof(tokens[13*i+10]))); - data.fiducials[i].rot[0] = NINT32(M_MM(atof(tokens[13*i+11]))); - data.fiducials[i].rot[1] = NINT32(M_MM(atof(tokens[13*i+12]))); - data.fiducials[i].rot[2] = NINT32(M_MM(atof(tokens[13*i+13]))); - data.fiducials[i].upos[0] = NINT32(M_MM(atof(tokens[13*i+14]))); - data.fiducials[i].upos[1] = NINT32(M_MM(atof(tokens[13*i+15]))); - data.fiducials[i].upos[2] = NINT32(M_MM(atof(tokens[13*i+16]))); - data.fiducials[i].urot[0] = NINT32(M_MM(atof(tokens[13*i+17]))); - data.fiducials[i].urot[1] = NINT32(M_MM(atof(tokens[13*i+18]))); - data.fiducials[i].urot[2] = NINT32(M_MM(atof(tokens[13*i+19]))); + fiducial_count = atoi(tokens[7]); + + data.fiducials_count = fiducial_count; + //data.fiducials = (player_fiducial_item_t *) malloc(data.fiducials_count * sizeof(data.fiducials[0])); + data.fiducials = new player_fiducial_item_t[data.fiducials_count]; + + for (int i = 0; i < fiducial_count; i++) { + data.fiducials[i].id = atof(tokens[13 * i + 8]); + data.fiducials[i].pose.px = atof(tokens[13 * i + 9]); + data.fiducials[i].pose.py = atof(tokens[13 * i + 10]); + data.fiducials[i].pose.pz = atof(tokens[13 * i + 11]); + data.fiducials[i].pose.proll = atof(tokens[13 * i + 12]); + data.fiducials[i].pose.ppitch = atof(tokens[13 * i + 13]); + data.fiducials[i].pose.pyaw = atof(tokens[13 * i + 14]); + data.fiducials[i].upose.px = atof(tokens[13 * i + 15]); + data.fiducials[i].upose.py = atof(tokens[13 * i + 16]); + data.fiducials[i].upose.pz = atof(tokens[13 * i + 17]); + data.fiducials[i].upose.proll = atof(tokens[13 * i + 18]); + data.fiducials[i].upose.ppitch = atof(tokens[13 * i + 19]); + data.fiducials[i].upose.pyaw = atof(tokens[13 * i + 20]); + } + this->Publish(id, type, subtype, + (void*) & data, sizeof (data), &time); + + delete [] data.fiducials; + return (0); + default: + PLAYER_ERROR1("unimplemented fiducial data subtype %d\n", subtype); + return(-1); + } + case PLAYER_MSGTYPE_RESP_ACK: + switch (subtype) { + case PLAYER_FIDUCIAL_REQ_GET_GEOM: + + if (token_count < 17) { + PLAYER_ERROR2("incomplete line at %s:%d", + this->filename, linenum); + return -1; + } + player_fiducial_geom_t* geom; + + geom = (player_fiducial_geom_t*) calloc(1, sizeof (player_fiducial_geom_t)); + assert(geom); + + geom->pose.px = atof(tokens[7]); + geom->pose.py = atof(tokens[8]); + geom->pose.pz = atof(tokens[9]); + geom->pose.proll = atof(tokens[10]); + geom->pose.ppitch = atof(tokens[11]); + geom->pose.pyaw = atof(tokens[12]); + geom->size.sl = atof(tokens[13]); + geom->size.sw = atof(tokens[14]); + geom->size.sh = atof(tokens[15]); + geom->fiducial_size.sl = atof(tokens[16]); + geom->fiducial_size.sw = atof(tokens[17]); + + // Find the right place to put it + int j; + for (j = 0; j<this->provide_count; j++) { + if (Device::MatchDeviceAddress(this->provide_ids[j], id)) + break; + } + assert(j<this->provide_count); + + if (this->provide_metadata[j]) + free(this->provide_metadata[j]); + + this->provide_metadata[j] = (void*) geom; + + // nothing to publish + return (0); + + default: + PLAYER_ERROR1("unimplemented fiducial data subtype %d\n", subtype); + return(-1); + } + default: + PLAYER_ERROR1("unimplemented fiducial data type %d\n", type); + return (-1); + } - this->PutMsg(id,NULL,PLAYER_MSGTYPE_DATA,0, &data, sizeof(data), &time); - return 0; } - +#if 0 //////////////////////////////////////////////////////////////////////////// // Parse GPS data int ReadLog::ParseGps(player_devaddr_t id, int linenum, Modified: code/player/trunk/server/drivers/shell/writelog.cc =================================================================== --- code/player/trunk/server/drivers/shell/writelog.cc 2009-06-06 09:39:40 UTC (rev 7801) +++ code/player/trunk/server/drivers/shell/writelog.cc 2009-06-06 09:51:04 UTC (rev 7802) @@ -80,12 +80,12 @@ - @ref interface_pointcloud3d - @ref interface_actarray - @ref interface_camera +- @ref interface_fiducial The following interfaces are supported in principle but are currently disabled because they need to be updated: - @ref interface_blobfinder -- @ref interface_fiducial - @ref interface_gps - @ref interface_joystick - @ref interface_position3d @@ -274,13 +274,13 @@ // Write camera data to file private: int WriteCamera(WriteLogDevice *device, player_msghdr_t* hdr, void *data); + + // Write fiducial data to file + private: int WriteFiducial(player_msghdr_t* hdr, void *data); #if 0 // Write blobfinder data to file private: void WriteBlobfinder(player_blobfinder_data_t *data); - // Write fiducial data to file - private: void WriteFiducial(player_fiducial_data_t *data); - // Write GPS data to file private: void WriteGps(player_gps_data_t *data); @@ -876,13 +876,13 @@ case PLAYER_CAMERA_CODE: retval = this->WriteCamera(device, hdr, data); break; + case PLAYER_FIDUCIAL_CODE: + this->WriteFiducial(hdr, data); + break; #if 0 case PLAYER_BLOBFINDER_CODE: this->WriteBlobfinder((player_blobfinder_data_t*) data); break; - case PLAYER_FIDUCIAL_CODE: - this->WriteFiducial((player_fiducial_data_t*) data); - break; case PLAYER_GPS_CODE: this->WriteGps((player_gps_data_t*) data); break; @@ -2226,14 +2226,14 @@ return -1; } -#if 0 - /** @ingroup tutorial_datalog - * @defgroup player_driver_writelog_fiducial Fiducial format + * @defgroup player_driver_writelog_fiducial fiducial format @brief fiducial log format -The format for each @ref interface_fiducial message is: +The following messages from @ref interface_fiducial interface are logged: + +PLAYER_MSGTYPE_DATA:PLAYER_FIDUCIAL_DATA_SCAN (1:1) has the folowing format: - count (int): number of fiducials to follow - list of fiducials; for each fiducial: - id (int): fiducial ID @@ -2250,34 +2250,83 @@ - upitch (float): uncertainty in relative pitch orientation, in radians - uyaw (float): uncertainty in relative yaw orientation, in radians +PLAYER_MSGTYPE_RESP_ACK:PLAYER_FIDUCIAL_REQ_GET_GEOM (4:1) has the following format: + - x (float): relative X position, in meters + - y (float): relative Y position, in meters + - z (float): relative Z position, in meters + - roll (float): relative roll orientation, in radians + - pitch (float): relative pitch orientation, in radians + - yaw (float): relative yaw orientation, in radians + - length (float): fiducial finder length, in meters + - width (float): fiducial finder width, in meters + - height (float): fiducial finder height, in meters + - fiducial_length (float): fiducial marker length, in meters + - fiducial_width (float): fiducial marker width, in meters */ -void WriteLog::WriteFiducial(player_fiducial_data_t *data) +int WriteLog::WriteFiducial(player_msghdr_t* hdr, void *data) { - // format: <count> [<id> <x> <y> <z> <roll> <pitch> <yaw> <ux> <uy> <uz> etc] ... - fprintf(this->file, "%d", HUINT16(data->count)); - for(int i=0;i<HUINT16(data->count);i++) - { - fprintf(this->file, " %d" - " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f" - " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f", - HINT16(data->fiducials[i].id), - MM_M(HINT32(data->fiducials[i].pos[0])), - MM_M(HINT32(data->fiducials[i].pos[1])), - MM_M(HINT32(data->fiducials[i].pos[2])), - MM_M(HINT32(data->fiducials[i].rot[0])), - MM_M(HINT32(data->fiducials[i].rot[1])), - MM_M(HINT32(data->fiducials[i].rot[2])), - MM_M(HINT32(data->fiducials[i].upos[0])), - MM_M(HINT32(data->fiducials[i].upos[1])), - MM_M(HINT32(data->fiducials[i].upos[2])), - MM_M(HINT32(data->fiducials[i].urot[0])), - MM_M(HINT32(data->fiducials[i].urot[1])), - MM_M(HINT32(data->fiducials[i].urot[2]))); - } + player_fiducial_data_t *fiducial_data; + player_fiducial_geom_t *fiducial_geom; - return; + switch (hdr->type) { + case PLAYER_MSGTYPE_DATA: + switch (hdr->subtype) { + case PLAYER_FIDUCIAL_DATA_SCAN: + fiducial_data = (player_fiducial_data_t*) data; + // format: <count> [<id> <x> <y> <z> <roll> <pitch> <yaw> <ux> <uy> <uz> <uroll> <upitch> <uyaw>] ... + fprintf(this->file, "%d", fiducial_data->fiducials_count); + for (int i = 0; i < fiducial_data->fiducials_count; i++) { + fprintf(this->file, " %d" + " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f" + " %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f %+07.3f", + fiducial_data->fiducials[i].id, + fiducial_data->fiducials[i].pose.px, + fiducial_data->fiducials[i].pose.py, + fiducial_data->fiducials[i].pose.pz, + fiducial_data->fiducials[i].pose.proll, + fiducial_data->fiducials[i].pose.ppitch, + fiducial_data->fiducials[i].pose.pyaw, + fiducial_data->fiducials[i].upose.px, + fiducial_data->fiducials[i].upose.py, + fiducial_data->fiducials[i].upose.pz, + fiducial_data->fiducials[i].upose.proll, + fiducial_data->fiducials[i].upose.ppitch, + fiducial_data->fiducials[i].upose.pyaw); + } + return(0); + default: + return(-1); + } + case PLAYER_MSGTYPE_RESP_ACK: + switch(hdr->subtype) { + case PLAYER_FIDUCIAL_REQ_GET_GEOM: + fiducial_geom = (player_fiducial_geom_t*) data; + //format: <x> <y> <z> <roll> <pitch> <yaw> <length> ... + // <width> <height> <fiducial_length> <fiducial_width> + fprintf(this->file, "%+7.3f %+7.3f %+7.3f %+7.3f %+7.3f" + "%+7.3f %+7.3f %+7.3f %+7.3f %+7.3f %+7.3f", + fiducial_geom->pose.px, + fiducial_geom->pose.py, + fiducial_geom->pose.pz, + fiducial_geom->pose.proll, + fiducial_geom->pose.ppitch, + fiducial_geom->pose.pyaw, + fiducial_geom->size.sl, + fiducial_geom->size.sw, + fiducial_geom->size.sh, + fiducial_geom->fiducial_size.sl, + fiducial_geom->fiducial_size.sw); + + return(0); + default: + return(-1); + } + default: + return(-1); + } } +#if 0 /** @ingroup tutorial_datalog * @defgroup player_driver_writelog_gps GPS format This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-24 14:22:02
|
Revision: 8125 http://playerstage.svn.sourceforge.net/playerstage/?rev=8125&view=rev Author: thjc Date: 2009-07-24 14:21:55 +0000 (Fri, 24 Jul 2009) Log Message: ----------- explicit copyright headers for some misc files Modified Paths: -------------- code/player/trunk/server/drivers/shell/encode.cc code/player/trunk/server/drivers/shell/encode.h code/player/trunk/server/drivers/shell/test/test_passthrough.py Modified: code/player/trunk/server/drivers/shell/encode.cc =================================================================== --- code/player/trunk/server/drivers/shell/encode.cc 2009-07-24 13:54:27 UTC (rev 8124) +++ code/player/trunk/server/drivers/shell/encode.cc 2009-07-24 14:21:55 UTC (rev 8125) @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2004 + * Andrew Howard + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ /* * Desc: Useful encoding/decoding routines Modified: code/player/trunk/server/drivers/shell/encode.h =================================================================== --- code/player/trunk/server/drivers/shell/encode.h 2009-07-24 13:54:27 UTC (rev 8124) +++ code/player/trunk/server/drivers/shell/encode.h 2009-07-24 14:21:55 UTC (rev 8125) @@ -1,3 +1,23 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2004 + * Andrew Howard + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ /* * Desc: Useful encoding/decoding routines Modified: code/player/trunk/server/drivers/shell/test/test_passthrough.py =================================================================== --- code/player/trunk/server/drivers/shell/test/test_passthrough.py 2009-07-24 13:54:27 UTC (rev 8124) +++ code/player/trunk/server/drivers/shell/test/test_passthrough.py 2009-07-24 14:21:55 UTC (rev 8125) @@ -1,3 +1,30 @@ +#Copyright (c) 2009, Toby Collett +#All rights reserved. +# +#Redistribution and use in source and binary forms, with or without +#modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of the Player Project nor the names of its contributors +# may be used to endorse or promote products derived from this software +# without specific prior written permission. +# +#THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +#ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +#WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +#DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +#ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +#(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +#LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +#ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +#(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +#SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + """ Manual integration test for passthrough. Requires player server and blackboard driver, and python client. This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-08-07 08:08:43
|
Revision: 8182 http://playerstage.svn.sourceforge.net/playerstage/?rev=8182&view=rev Author: gbiggs Date: 2009-08-07 08:08:34 +0000 (Fri, 07 Aug 2009) Log Message: ----------- Applied patch #2832969: Update to writelog and 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-06 23:32:05 UTC (rev 8181) +++ code/player/trunk/server/drivers/shell/readlog.cc 2009-08-07 08:08:34 UTC (rev 8182) @@ -2458,6 +2458,39 @@ (void*)&data, sizeof(data), &time); return (0); } + + case PLAYER_IMU_DATA_FULLSTATE: + { + if (token_count < 22) + { + PLAYER_ERROR2("invalid line at %s:%d", this->filename, linenum); + return -1; + } + player_imu_data_fullstate_t data; + + data.pose.px = static_cast<float> (atof (tokens[7])); + data.pose.py = static_cast<float> (atof (tokens[8])); + data.pose.pz = static_cast<float> (atof (tokens[9])); + data.pose.proll = static_cast<float> (atof (tokens[10])); + data.pose.ppitch = static_cast<float> (atof (tokens[11])); + data.pose.pyaw = static_cast<float> (atof (tokens[12])); + data.vel.px = static_cast<float> (atof (tokens[13])); + data.vel.py = static_cast<float> (atof (tokens[14])); + data.vel.pz = static_cast<float> (atof (tokens[15])); + data.vel.proll = static_cast<float> (atof (tokens[16])); + data.vel.ppitch = static_cast<float> (atof (tokens[17])); + data.vel.pyaw = static_cast<float> (atof (tokens[18])); + data.acc.px = static_cast<float> (atof (tokens[19])); + data.acc.py = static_cast<float> (atof (tokens[20])); + data.acc.pz = static_cast<float> (atof (tokens[21])); + data.acc.proll = 0; + data.acc.ppitch = 0; + data.acc.pyaw = 0; + + this->Publish (id, static_cast<uint8_t> (type), static_cast<uint8_t> (subtype), + (void*)&data, sizeof(data), &time); + return (0); + } default: PLAYER_ERROR1 ("unknown IMU data subtype %d\n", subtype); return (-1); Modified: code/player/trunk/server/drivers/shell/writelog.cc =================================================================== --- code/player/trunk/server/drivers/shell/writelog.cc 2009-08-06 23:32:05 UTC (rev 8181) +++ code/player/trunk/server/drivers/shell/writelog.cc 2009-08-07 08:08:34 UTC (rev 8182) @@ -1636,6 +1636,22 @@ -> proll (float): roll angle -> ppitch (float): pitch angle -> pyaw (float): yaw angle + - for PLAYER_IMU_DATA_FULLSTATE (player_imu_data_fullstate_t): + -> pose_px (float): X pose + -> pose_py (float): Y pose + -> pose_pz (float): Z pose + -> pose_proll (float): roll angle + -> pose_ppitch (float): pitch angle + -> pose_pyaw (float): yaw angle + -> vel_px (float): X velocity + -> vel_py (float): Y velocity + -> vel_pz (float): Z velocity + -> vel_proll (float): roll anglular velocity + -> vel_ppitch (float): pitch anglular velocity + -> vel_pyaw (float): yaw anglular velocity + -> acc_px (float): X acceleration + -> acc_py (float): Y acceleration + -> acc_pz (float): Z acceleration */ int WriteLog::WriteIMU (player_msghdr_t* hdr, void *data) @@ -1718,6 +1734,29 @@ idata->orientation.pyaw); return (0); } + + case PLAYER_IMU_DATA_FULLSTATE: + { + player_imu_data_fullstate_t* idata; + idata = (player_imu_data_fullstate_t*)data; + fprintf (this->file,"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f", + idata->pose.px, + idata->pose.py, + idata->pose.pz, + idata->pose.proll, + idata->pose.ppitch, + idata->pose.pyaw, + idata->vel.px, + idata->vel.py, + idata->vel.pz, + idata->vel.proll, + idata->vel.ppitch, + idata->vel.pyaw, + idata->acc.px, + idata->acc.py, + idata->acc.pz); + return (0); + } default: return (-1); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
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. |