Update of /cvsroot/playerstage/code/player/server/drivers/shell
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22549
Modified Files:
readlog.cc writelog.cc
Log Message:
applied patch to log localize data
Index: writelog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/writelog.cc,v
retrieving revision 1.81
retrieving revision 1.82
diff -C2 -d -r1.81 -r1.82
*** writelog.cc 23 Aug 2007 19:58:48 -0000 1.81
--- writelog.cc 5 Sep 2007 18:57:43 -0000 1.82
***************
*** 190,193 ****
--- 190,196 ----
private: void CloseFile();
+ // Write localize particles to file
+ private: void WriteLocalizeParticles();
+
// Write data to file
private: void Write(WriteLogDevice *device,
***************
*** 197,200 ****
--- 200,206 ----
private: int WriteLaser(player_msghdr_t* hdr, void *data);
+ // Write localize data to file
+ private: int WriteLocalize(player_msghdr_t* hdr, void *data);
+
// Write position data to file
private: int WritePosition(player_msghdr_t* hdr, void *data);
***************
*** 275,278 ****
--- 281,290 ----
private: WriteLogDevice devices[1024];
+ // Log particles (in case a localize interface is provided)
+ public: bool write_particles;
+
+ private: bool write_particles_now;
+ private: WriteLogDevice* localize_device;
+
// Is writing enabled? (client can start/stop)
private: bool enable;
***************
*** 342,345 ****
--- 354,364 ----
this->device_count = 0;
+ //write particles in case the localize interface is provided
+ this->write_particles = cf->ReadInt(section, "write_particles", 0);
+
+ this->write_particles_now = false;
+
+
+ localize_device = NULL;
// Get a list of input devices
for (i = 0; i < cf->GetTupleCount(section, "requires"); i++)
***************
*** 571,574 ****
--- 590,597 ----
}
}
+ else if (device->addr.interf == PLAYER_LOCALIZE_CODE)
+ {
+ localize_device = device;
+ }
}
***************
*** 725,728 ****
--- 748,757 ----
this->Wait();
+
+ if (write_particles_now){
+ WriteLocalizeParticles();
+ write_particles_now = false;
+ }
+
// Process all new messages (calls ProcessMessage on each)
this->ProcessMessages();
***************
*** 763,766 ****
--- 792,798 ----
retval = this->WriteLaser(hdr, data);
break;
+ case PLAYER_LOCALIZE_CODE:
+ retval = this->WriteLocalize(hdr, data);
+ break;
case PLAYER_POSITION2D_CODE:
retval = this->WritePosition(hdr, data);
***************
*** 850,853 ****
--- 882,911 ----
}
+
+ void
+ WriteLog::WriteLocalizeParticles()
+
+ {
+ Message* msg;
+
+ assert(localize_device != NULL);
+
+ if(!(msg = localize_device->device->Request(this->InQueue,
+ PLAYER_MSGTYPE_REQ,
+
PLAYER_LOCALIZE_REQ_GET_PARTICLES,
+ NULL, 0, NULL, true)))
+ {
+ // oh well.
+ PLAYER_WARN("unable to get localize particles");
+ }
+ else
+ {
+ // log it
+ this->Write(localize_device, msg->GetHeader(), msg->GetPayload());
+ delete msg;
+ }
+ }
+
+
/** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_laser laser format
***************
*** 958,961 ****
--- 1016,1120 ----
/** @ingroup tutorial_datalog
+ * @defgroup player_driver_writelog_localize localize format
+
+ @brief laser log format
+
+ The following type:subtype laser 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
+ - hypoths_coun (int): number of pose hypotheses
+ - list of hypotheses; for each hypothesis
+ - px (float): X coordinate of the mean value of the pose estimate (in m)
+ - py (float): Y coordinate of the mean value of the pose estimate (in m)
+ - pa (float): yaw coordinate of the mean value of the pose estimate (in
rad)
+ - cov[0] (float): X coordinate of the covariance matrix pose estimate
+ - cov[1] (float): Y coordinate of the covariance matrix pose estimate
+ - cov[2] (float): yaw coordinate of the covariance matrix pose estimate
+ - alpha (float): weight coefficient for linear combination
+
+ - 4:2 (PLAYER_LOCALIZE_REQ_GET_PARTICLES) - Current particle set. The format
is:
+ - px (float): X coordinate of best (?) pose (in m?)
+ - py (float): Y coordinate of best (?) pose (in m?)
+ - pa (float): yaw coordinate of best (?) pose (in ??)
+ - variance (float): variance of the best(?) pose
+ - particles_count (int): number of particles; for each particle:
+ - px (float: X coordinate of particle's pose (in m)
+ - py (float: Y coordinate of particle's pose (in m)
+ - pa (float: yaw coordinate of particle's pose (in rad)
+ - alpha (float): weight coefficient for linear combination
+ */
+
+
+ int
+ WriteLog::WriteLocalize(player_msghdr_t* hdr, void *data)
+ {
+ size_t i;
+ player_localize_data_t* hypoths;
+ player_localize_get_particles_t* particles;
+
+
+ // Check the type
+ switch(hdr->type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ // Check the subtype
+ switch(hdr->subtype)
+ {
+ case PLAYER_LOCALIZE_DATA_HYPOTHS:
+ hypoths = (player_localize_data_t*)data;
+ // Note that, in this format, we need a lot of precision in the
+ // resolution field.
+
+
+ fprintf(this->file, "%10d %+07.3f %2d ",
+ hypoths->pending_count, hypoths->pending_time,
+ hypoths->hypoths_count);
+
+ for (i = 0; i < hypoths->hypoths_count; i++)
+ fprintf(this->file, "%+7.3f %+7.3f %7.3f %7.3f %7.3f %7.3f %7.3f
",
+ hypoths->hypoths[i].mean.px,
+ hypoths->hypoths[i].mean.py,
+ hypoths->hypoths[i].mean.pa,
+ hypoths->hypoths[i].cov[0],
+ hypoths->hypoths[i].cov[1],
+ hypoths->hypoths[i].cov[2],
+ hypoths->hypoths[i].alpha);
+ if (write_particles)
+ // every time we receive localize data also write localize particles
+ write_particles_now = true;
+ return(0);
+
+ default:
+ return(-1);
+ }
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(hdr->subtype)
+ {
+ case PLAYER_LOCALIZE_REQ_GET_PARTICLES:
+ particles = (player_localize_get_particles_t*)data;
+ fprintf(this->file, "%+7.3f %+7.3f %7.3f %7.3f %10d ",
+ particles->mean.px,
+ particles->mean.py,
+ particles->mean.pa,
+ particles->variance,
+ particles->particles_count);
+
+ for (i = 0; i < particles->particles_count; i++)
+ fprintf(this->file, "%+7.3f %+7.3f %7.3f %7.3f ",
+ particles->particles[i].pose.px,
+ particles->particles[i].pose.py,
+ particles->particles[i].pose.pa,
+ particles->particles[i].alpha);
+ return(0);
+ default:
+ return(-1);
+ }
+ default:
+ return(-1);
+ }
+ }
+
+ /** @ingroup tutorial_datalog
* @defgroup player_driver_writelog_position position2d format
Index: readlog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/readlog.cc,v
retrieving revision 1.51
retrieving revision 1.52
diff -C2 -d -r1.51 -r1.52
*** readlog.cc 23 Aug 2007 19:58:48 -0000 1.51
--- readlog.cc 5 Sep 2007 18:57:43 -0000 1.52
***************
*** 257,260 ****
--- 257,266 ----
int token_count, char **tokens, double time);
+ // Parse localize data
+ private: int ParseLocalize(player_devaddr_t id, unsigned short type,
+ unsigned short subtype,
+ int linenum, int token_count,
+ char **tokens, double time);
+
// Parse sonar data
private: int ParseSonar(player_devaddr_t id,
***************
*** 346,349 ****
--- 352,361 ----
#endif
+ // localize particles
+ private: player_localize_get_particles_t particles;
+ private: bool particles_set;
+ private: player_devaddr_t localize_addr;
+
+
// Input buffer
private: size_t line_size;
***************
*** 406,409 ****
--- 418,423 ----
memset(this->provide_metadata,0,sizeof(this->provide_metadata));
+ particles_set = false;
+
// Get a list of devices to provide
for (i = 0; i < 1024; i++)
***************
*** 450,453 ****
--- 464,472 ----
assert((this->provide_metadata[i] =
calloc(sizeof(player_sonar_geom_t),1)));
+
+ // if it's localize, remember address
+ if(this->provide_ids[i].interf == PLAYER_LOCALIZE_CODE){
+ this->localize_addr = this->provide_ids[i];
+ }
}
***************
*** 1083,1086 ****
--- 1102,1125 ----
return(this->ProcessPositionConfig(resp_queue, hdr, data));
}
+ else if(particles_set &&
+ Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_LOCALIZE_REQ_GET_PARTICLES,
+ this->localize_addr))
+ {
+ if(hdr->size != 0)
+ {
+ PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",
+ hdr->size, 0);
+ return(PLAYER_MSGTYPE_RESP_NACK);
+ }
+
+
+
+ this->Publish(this->localize_addr, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_LOCALIZE_REQ_GET_PARTICLES,
+ (void*)&particles, sizeof(particles), NULL);
+ return(0);
+ }
else
return -1;
***************
*** 1167,1170 ****
--- 1206,1212 ----
return this->ParseLaser(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);
else if (id.interf == PLAYER_SONAR_CODE)
return this->ParseSonar(id, type, subtype, linenum,
***************
*** 1558,1561 ****
--- 1600,1721 ----
}
+
+
+ ////////////////////////////////////////////////////////////////////////////
+ // Parse localize data
+ int ReadLog::ParseLocalize(player_devaddr_t id,
+ unsigned short type, unsigned short subtype,
+ int linenum,
+ int token_count, char **tokens, double time)
+ {
+ int i, count;
+
+ switch(type)
+ {
+ case PLAYER_MSGTYPE_DATA:
+ switch(subtype)
+ {
+ case PLAYER_LOCALIZE_DATA_HYPOTHS:
+ {
+ player_localize_data_t hypoths;
+
+ if (token_count < 10)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+
+ hypoths.pending_count = atoi(tokens[7]);
+ hypoths.pending_time = atof(tokens[8]);
+ hypoths.hypoths_count = atoi(tokens[9]);
+
+ count = 0;
+ for (i = 10; i < token_count; i += 7)
+ {
+ hypoths.hypoths[count].mean.px = atof(tokens[i + 0]);
+ hypoths.hypoths[count].mean.py = atof(tokens[i + 1]);
+ hypoths.hypoths[count].mean.pa = atof(tokens[i + 2]);
+ hypoths.hypoths[count].cov[0] = atof(tokens[i + 3]);
+ hypoths.hypoths[count].cov[1] = atof(tokens[i + 4]);
+ hypoths.hypoths[count].cov[2] = atof(tokens[i + 5]);
+ hypoths.hypoths[count].alpha = atof(tokens[i + 6]);
+ count += 1;
+ }
+
+ if (count != (int)hypoths.hypoths_count)
+ {
+ PLAYER_ERROR2("hypoths count mismatch at %s:%d",
+ this->filename, linenum);
+ return -1;
+
+ }
+
+ this->Publish(id, type, subtype,
+ (void*)&hypoths, sizeof(hypoths), &time);
+ return(0);
+ }
+
+
+ default:
+ PLAYER_ERROR1("unknown localize data subtype %d\n", subtype);
+ return(-1);
+ }
+ break;
+
+ case PLAYER_MSGTYPE_RESP_ACK:
+ switch(subtype)
+ {
+ case PLAYER_LOCALIZE_REQ_GET_PARTICLES:
+ {
+ if(token_count < 12)
+ {
+ PLAYER_ERROR2("incomplete line at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+
+
+ particles.mean.px = atof(tokens[7]);
+ particles.mean.py = atof(tokens[8]);
+ particles.mean.pa = atof(tokens[9]);
+ particles.variance = atof(tokens[10]);
+ particles.particles_count = atoi(tokens[11]);
+
+ count = 0;
+ for (i = 12; i < token_count; i += 4)
+ {
+ particles.particles[count].pose.px = atof(tokens[i + 0]);
+ particles.particles[count].pose.py = atof(tokens[i + 1]);
+ particles.particles[count].pose.pa = atof(tokens[i + 2]);
+ particles.particles[count].alpha = atof(tokens[i + 3]);
+ count += 1;
+ }
+
+ if (count != (int)particles.particles_count)
+ {
+ PLAYER_ERROR2("particles count mismatch at %s:%d",
+ this->filename, linenum);
+ return -1;
+ }
+ particles_set = true;
+
+ return(0);
+ }
+
+ default:
+ PLAYER_ERROR1("unknown localize reply subtype %d\n", subtype);
+ return(-1);
+ }
+ break;
+
+ default:
+ PLAYER_ERROR1("unknown localize msg type %d\n", type);
+ return(-1);
+ }
+ }
+
+
////////////////////////////////////////////////////////////////////////////
// Parse sonar data
***************
*** 2128,2131 ****
--- 2288,2292 ----
data.motor_state = atoi (tokens[data.actuators_count*5 +
8]);
+
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
