Update of /cvsroot/playerstage/code/player/server/drivers/shell
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv17948/drivers/shell
Modified Files:
Tag: b_thjc_dynamic_arrays
readlog.cc writelog.cc
Log Message:
Fixes to handle dynamic arrays in interfaces
Index: writelog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/writelog.cc,v
retrieving revision 1.82
retrieving revision 1.82.2.1
diff -C2 -d -r1.82 -r1.82.2.1
*** writelog.cc 5 Sep 2007 18:57:43 -0000 1.82
--- writelog.cc 14 Sep 2007 00:21:10 -0000 1.82.2.1
***************
*** 883,887 ****
! void
WriteLog::WriteLocalizeParticles()
--- 883,887 ----
! void
WriteLog::WriteLocalizeParticles()
***************
*** 1067,1071 ****
// resolution field.
!
fprintf(this->file, "%10d %+07.3f %2d ",
hypoths->pending_count, hypoths->pending_time,
--- 1067,1071 ----
// resolution field.
!
fprintf(this->file, "%10d %+07.3f %2d ",
hypoths->pending_count, hypoths->pending_time,
***************
*** 1074,1082 ****
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);
--- 1074,1082 ----
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);
***************
*** 1103,1108 ****
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);
--- 1103,1108 ----
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);
***************
*** 1661,1690 ****
switch(hdr->type)
{
! case PLAYER_MSGTYPE_DATA:
! // Check the subtype
! switch(hdr->subtype)
! {
! case PLAYER_ACTARRAY_DATA_STATE:
! {
! player_actarray_data_t* pdata;
! pdata = (player_actarray_data_t*)data;
! fprintf (this->file, "%d ", pdata->actuators_count);
! for (i = 0; i < pdata->actuators_count; i++)
! fprintf (this->file,"%f %f %f %f %d ",
! pdata->actuators[i].position,
! pdata->actuators[i].speed,
! pdata->actuators[i].acceleration,
! pdata->actuators[i].current,
! pdata->actuators[i].state);
! fprintf (this->file, "%d ", pdata->motor_state);
! return (0);
! }
!
! default:
! return (-1);
! }
!
! default:
return (-1);
}
}
--- 1661,1688 ----
switch(hdr->type)
{
! case PLAYER_MSGTYPE_DATA:
! // Check the subtype
! switch(hdr->subtype)
! {
! case PLAYER_ACTARRAY_DATA_STATE:
! player_actarray_data_t* pdata;
! pdata = (player_actarray_data_t*)data;
! fprintf (this->file, "%d ", pdata->actuators_count);
! for (i = 0; i < pdata->actuators_count; i++)
! fprintf (this->file,"%f %f %f %f %d ",
! pdata->actuators[i].position,
! pdata->actuators[i].speed,
! pdata->actuators[i].acceleration,
! pdata->actuators[i].current,
! pdata->actuators[i].state);
! fprintf (this->file, "%d ", pdata->motor_state);
! delete[] pdata->actuators;
! return (0);
! default:
return (-1);
+ }
+ break;
+ default:
+ return (-1);
}
}
Index: readlog.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/shell/readlog.cc,v
retrieving revision 1.52
retrieving revision 1.52.2.1
diff -C2 -d -r1.52 -r1.52.2.1
*** readlog.cc 5 Sep 2007 18:57:43 -0000 1.52
--- readlog.cc 14 Sep 2007 00:21:10 -0000 1.52.2.1
***************
*** 257,264 ****
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);
--- 257,264 ----
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);
***************
*** 356,360 ****
private: bool particles_set;
private: player_devaddr_t localize_addr;
!
// Input buffer
--- 356,360 ----
private: bool particles_set;
private: player_devaddr_t localize_addr;
!
// Input buffer
***************
*** 1631,1643 ****
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]);
--- 1631,1643 ----
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]);
***************
*** 2072,2076 ****
data.pose.ppitch = atof (tokens[11]);
data.pose.pyaw = atof (tokens[12]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
--- 2072,2076 ----
data.pose.ppitch = atof (tokens[11]);
data.pose.pyaw = atof (tokens[12]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
***************
*** 2096,2100 ****
data.magn_y = atof (tokens[14]);
data.magn_z = atof (tokens[15]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
--- 2096,2100 ----
data.magn_y = atof (tokens[14]);
data.magn_z = atof (tokens[15]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
***************
*** 2124,2128 ****
data.q2 = atof (tokens[18]);
data.q3 = atof (tokens[19]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
--- 2124,2128 ----
data.q2 = atof (tokens[18]);
data.q3 = atof (tokens[19]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
***************
*** 2151,2155 ****
data.orientation.ppitch = atof (tokens[17]);
data.orientation.pyaw = atof (tokens[18]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
--- 2151,2155 ----
data.orientation.ppitch = atof (tokens[17]);
data.orientation.pyaw = atof (tokens[18]);
!
this->Publish (id, type, subtype,
(void*)&data, sizeof(data), &time);
***************
*** 2264,2304 ****
switch(type)
{
! case PLAYER_MSGTYPE_DATA:
! switch(subtype)
{
! case PLAYER_ACTARRAY_DATA_STATE:
! {
! player_actarray_data_t data;
! data.actuators_count = atoi (tokens[7]);
! if (token_count < (int)(7+data.actuators_count))
! {
! PLAYER_ERROR2("invalid line at %s:%d",
this->filename, linenum);
! return -1;
! }
! for (i = 0; i < data.actuators_count; i++)
! {
! player_actarray_actuator actuator;
! actuator.position = atof (tokens[5*i+8]);
! actuator.speed = atof (tokens[5*i+9]);
! actuator.acceleration = atof (tokens[5*i+10]);
! actuator.current = atof (tokens[5*i+11]);
! actuator.state = atoi (tokens[5*i+12]);
! data.actuators[i] = actuator;
! }
! data.motor_state = atoi (tokens[data.actuators_count*5 +
8]);
!
!
! this->Publish (id, type, subtype,
! (void*)&data, sizeof(data), &time);
! return (0);
! }
!
! default:
! PLAYER_ERROR1 ("unknown Actarray data subtype %d\n",
subtype);
! return (-1);
}
! default:
! PLAYER_ERROR1 ("unknown Actarray message type %d\n", type);
return (-1);
}
}
--- 2264,2302 ----
switch(type)
{
! case PLAYER_MSGTYPE_DATA:
! switch(subtype)
! {
! case PLAYER_ACTARRAY_DATA_STATE:
! player_actarray_data_t data;
! data.actuators_count = atoi (tokens[7]);
! data.actuators = new
player_actarray_actuator[data.actuators_count];
! if (token_count < (int)(7+data.actuators_count))
{
! PLAYER_ERROR2("invalid line at %s:%d", this->filename,
linenum);
! return -1;
}
! for (i = 0; i < data.actuators_count; i++)
! {
! player_actarray_actuator actuator;
! actuator.position = atof (tokens[5*i+8]);
! actuator.speed = atof (tokens[5*i+9]);
! actuator.acceleration = atof (tokens[5*i+10]);
! actuator.current = atof (tokens[5*i+11]);
! actuator.state = atoi (tokens[5*i+12]);
! data.actuators[i] = actuator;
! }
! data.motor_state = atoi (tokens[data.actuators_count*5 + 8]);
!
! this->Publish (id, type, subtype, (void*)&data, sizeof(data),
&time);
! delete[] data.actuators;
! return (0);
! default:
! PLAYER_ERROR1 ("unknown Actarray data subtype %d\n", subtype);
return (-1);
+ }
+ break;
+ default:
+ PLAYER_ERROR1 ("unknown Actarray message type %d\n", type);
+ return (-1);
}
}
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2005.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
