Update of /cvsroot/playerstage/code/player/server/drivers/mixed/p2os
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv17948/drivers/mixed/p2os
Modified Files:
Tag: b_thjc_dynamic_arrays
p2os.cc p2os.h sip.cc sip.h
Log Message:
Fixes to handle dynamic arrays in interfaces
Index: p2os.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/p2os.cc,v
retrieving revision 1.83
retrieving revision 1.83.2.1
diff -C2 -d -r1.83 -r1.83.2.1
*** p2os.cc 23 Aug 2007 19:58:45 -0000 1.83
--- p2os.cc 14 Sep 2007 00:21:10 -0000 1.83.2.1
***************
*** 615,641 ****
}
// Joint 0 default: (0, 0, 1)
! aaAxes[0] = cf->ReadTupleFloat(section, "aa_axes", 0, 0.0f);
! aaAxes[1] = cf->ReadTupleFloat(section, "aa_axes", 1, 0.0f);
! aaAxes[2] = cf->ReadTupleFloat(section, "aa_axes", 2, -1.0f);
// Joint 1 default: (0, 1, 0)
! aaAxes[3] = cf->ReadTupleFloat(section, "aa_axes", 3, 0.0f);
! aaAxes[4] = cf->ReadTupleFloat(section, "aa_axes", 4, -1.0f);
! aaAxes[5] = cf->ReadTupleFloat(section, "aa_axes", 5, 0.0f);
// Joint 2 default: (0, 1, 0)
! aaAxes[6] = cf->ReadTupleFloat(section, "aa_axes", 6, 0.0f);
! aaAxes[7] = cf->ReadTupleFloat(section, "aa_axes", 7, -1.0f);
! aaAxes[8] = cf->ReadTupleFloat(section, "aa_axes", 8, 0.0f);
// Joint 3 default: (1, 0, 0)
! aaAxes[9] = cf->ReadTupleFloat(section, "aa_axes", 9, 1.0f);
! aaAxes[10] = cf->ReadTupleFloat(section, "aa_axes", 10, 0.0f);
! aaAxes[11] = cf->ReadTupleFloat(section, "aa_axes", 11, 0.0f);
// Joint 4 default: (0, 1, 0)
! aaAxes[12] = cf->ReadTupleFloat(section, "aa_axes", 12, 0.0f);
! aaAxes[13] = cf->ReadTupleFloat(section, "aa_axes", 13, 1.0f);
! aaAxes[14] = cf->ReadTupleFloat(section, "aa_axes", 14, 0.0f);
// Joint 5 default: (0, 0, 1)
! aaAxes[15] = cf->ReadTupleFloat(section, "aa_axes", 15, 0.0f);
! aaAxes[16] = cf->ReadTupleFloat(section, "aa_axes", 16, 0.0f);
! aaAxes[17] = cf->ReadTupleFloat(section, "aa_axes", 17, 1.0f);
// Joint base position, orientation
aaBasePos.px = cf->ReadTupleFloat(section, "aa_basepos", 0, 0.105f);
--- 615,641 ----
}
// Joint 0 default: (0, 0, 1)
! aaAxes[0] = cf->ReadTupleFloat(section, "aa_axes", 0, 0.0f);
! aaAxes[1] = cf->ReadTupleFloat(section, "aa_axes", 1, 0.0f);
! aaAxes[2] = cf->ReadTupleFloat(section, "aa_axes", 2, -1.0f);
// Joint 1 default: (0, 1, 0)
! aaAxes[3] = cf->ReadTupleFloat(section, "aa_axes", 3, 0.0f);
! aaAxes[4] = cf->ReadTupleFloat(section, "aa_axes", 4, -1.0f);
! aaAxes[5] = cf->ReadTupleFloat(section, "aa_axes", 5, 0.0f);
// Joint 2 default: (0, 1, 0)
! aaAxes[6] = cf->ReadTupleFloat(section, "aa_axes", 6, 0.0f);
! aaAxes[7] = cf->ReadTupleFloat(section, "aa_axes", 7, -1.0f);
! aaAxes[8] = cf->ReadTupleFloat(section, "aa_axes", 8, 0.0f);
// Joint 3 default: (1, 0, 0)
! aaAxes[9] = cf->ReadTupleFloat(section, "aa_axes", 9, 1.0f);
! aaAxes[10] = cf->ReadTupleFloat(section, "aa_axes", 10, 0.0f);
! aaAxes[11] = cf->ReadTupleFloat(section, "aa_axes", 11, 0.0f);
// Joint 4 default: (0, 1, 0)
! aaAxes[12] = cf->ReadTupleFloat(section, "aa_axes", 12, 0.0f);
! aaAxes[13] = cf->ReadTupleFloat(section, "aa_axes", 13, 1.0f);
! aaAxes[14] = cf->ReadTupleFloat(section, "aa_axes", 14, 0.0f);
// Joint 5 default: (0, 0, 1)
! aaAxes[15] = cf->ReadTupleFloat(section, "aa_axes", 15, 0.0f);
! aaAxes[16] = cf->ReadTupleFloat(section, "aa_axes", 16, 0.0f);
! aaAxes[17] = cf->ReadTupleFloat(section, "aa_axes", 17, 1.0f);
// Joint base position, orientation
aaBasePos.px = cf->ReadTupleFloat(section, "aa_basepos", 0, 0.105f);
***************
*** 1372,1376 ****
// put odometry data
! this->Publish(this->position_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
--- 1372,1376 ----
// put odometry data
! this->Publish(this->position_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
***************
*** 1380,1384 ****
// put sonar data
! this->Publish(this->sonar_id,
PLAYER_MSGTYPE_DATA,
PLAYER_SONAR_DATA_RANGES,
--- 1380,1384 ----
// put sonar data
! this->Publish(this->sonar_id,
PLAYER_MSGTYPE_DATA,
PLAYER_SONAR_DATA_RANGES,
***************
*** 1386,1392 ****
sizeof(player_sonar_data_t),
NULL);
// put aio data
! this->Publish(this->aio_id,
PLAYER_MSGTYPE_DATA,
PLAYER_AIO_DATA_STATE,
--- 1386,1393 ----
sizeof(player_sonar_data_t),
NULL);
+ delete this->p2os_data.sonar.ranges;
// put aio data
! this->Publish(this->aio_id,
PLAYER_MSGTYPE_DATA,
PLAYER_AIO_DATA_STATE,
***************
*** 1396,1400 ****
// put dio data
! this->Publish(this->dio_id,
PLAYER_MSGTYPE_DATA,
PLAYER_DIO_DATA_VALUES,
--- 1397,1401 ----
// put dio data
! this->Publish(this->dio_id,
PLAYER_MSGTYPE_DATA,
PLAYER_DIO_DATA_VALUES,
***************
*** 1404,1408 ****
// put gripper data
! this->Publish(this->gripper_id,
PLAYER_MSGTYPE_DATA,
PLAYER_GRIPPER_DATA_STATE,
--- 1405,1409 ----
// put gripper data
! this->Publish(this->gripper_id,
PLAYER_MSGTYPE_DATA,
PLAYER_GRIPPER_DATA_STATE,
***************
*** 1420,1424 ****
// put bumper data
! this->Publish(this->bumper_id,
PLAYER_MSGTYPE_DATA,
PLAYER_BUMPER_DATA_STATE,
--- 1421,1425 ----
// put bumper data
! this->Publish(this->bumper_id,
PLAYER_MSGTYPE_DATA,
PLAYER_BUMPER_DATA_STATE,
***************
*** 1428,1432 ****
// put power data
! this->Publish(this->power_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POWER_DATA_STATE,
--- 1429,1433 ----
// put power data
! this->Publish(this->power_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POWER_DATA_STATE,
***************
*** 1436,1440 ****
// put compass data
! this->Publish(this->compass_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
--- 1437,1441 ----
// put compass data
! this->Publish(this->compass_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
***************
*** 1444,1448 ****
// put gyro data
! this->Publish(this->gyro_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
--- 1445,1449 ----
// put gyro data
! this->Publish(this->gyro_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
***************
*** 1452,1456 ****
// put blobfinder data
! this->Publish(this->blobfinder_id,
PLAYER_MSGTYPE_DATA,
PLAYER_BLOBFINDER_DATA_BLOBS,
--- 1453,1457 ----
// put blobfinder data
! this->Publish(this->blobfinder_id,
PLAYER_MSGTYPE_DATA,
PLAYER_BLOBFINDER_DATA_BLOBS,
***************
*** 1460,1464 ****
// put actarray data
! this->Publish(this->actarray_id,
PLAYER_MSGTYPE_DATA,
PLAYER_ACTARRAY_DATA_STATE,
--- 1461,1465 ----
// put actarray data
! this->Publish(this->actarray_id,
PLAYER_MSGTYPE_DATA,
PLAYER_ACTARRAY_DATA_STATE,
***************
*** 1466,1472 ****
sizeof(player_actarray_data_t),
NULL);
// put limb data
! this->Publish(this->limb_id,
PLAYER_MSGTYPE_DATA,
PLAYER_LIMB_DATA_STATE,
--- 1467,1474 ----
sizeof(player_actarray_data_t),
NULL);
+ delete[] this->p2os_data.actArray.actuators;
// put limb data
! this->Publish(this->limb_id,
PLAYER_MSGTYPE_DATA,
PLAYER_LIMB_DATA_STATE,
***************
*** 2387,2392 ****
--- 2389,2402 ----
player_actarray_geom_t aaGeom;
+ player_actarray_actuatorgeom_t *actuators;
aaGeom.actuators_count = sippacket->armNumJoints;
+ actuators = new player_actarray_actuatorgeom_t[sippacket->armNumJoints];
+ if (actuators == NULL)
+ {
+ PLAYER_ERROR ("Failed to allocate memory for actuator data");
+ return -1;
+ }
+ aaGeom.actuators = actuators;
for (int ii = 0; ii < sippacket->armNumJoints; ii++)
***************
*** 2416,2419 ****
--- 2426,2430 ----
this->Publish(this->actarray_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_ACTARRAY_REQ_GET_GEOM, &aaGeom, sizeof (aaGeom), NULL);
+ delete[] actuators;
return 0;
}
***************
*** 2492,2498 ****
{
player_actarray_geom_t aaGeom;
aaGeom.actuators_count = 1;
! memset (aaGeom.actuators, 0, sizeof (player_actarray_actuator_t) *
PLAYER_ACTARRAY_NUM_ACTUATORS);
aaGeom.actuators[0].type = PLAYER_ACTARRAY_TYPE_LINEAR;
--- 2503,2511 ----
{
player_actarray_geom_t aaGeom;
+ player_actarray_actuatorgeom_t actuator;
+ aaGeom.actuators = &actuator;
aaGeom.actuators_count = 1;
! memset (aaGeom.actuators, 0, sizeof (player_actarray_actuator_t));
aaGeom.actuators[0].type = PLAYER_ACTARRAY_TYPE_LINEAR;
Index: p2os.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/p2os.h,v
retrieving revision 1.34
retrieving revision 1.34.2.1
diff -C2 -d -r1.34 -r1.34.2.1
*** p2os.h 23 Aug 2007 19:58:45 -0000 1.34
--- p2os.h 14 Sep 2007 00:21:10 -0000 1.34.2.1
***************
*** 324,329 ****
// MessageHandler
! virtual int ProcessMessage(QueuePointer & resp_queue,
! player_msghdr * hdr,
void * data);
--- 324,329 ----
// MessageHandler
! virtual int ProcessMessage(QueuePointer & resp_queue,
! player_msghdr * hdr,
void * data);
Index: sip.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/sip.cc,v
retrieving revision 1.24
retrieving revision 1.24.2.1
diff -C2 -d -r1.24 -r1.24.2.1
*** sip.cc 20 Aug 2007 06:37:29 -0000 1.24
--- sip.cc 14 Sep 2007 00:21:10 -0000 1.24.2.1
***************
*** 38,41 ****
--- 38,46 ----
#include "sip.h"
+ // Variable with constant lifetime to store lift actuator data
+ static player_actarray_actuator_t liftActuator;
+ // Same thing for blobs
+ static player_blobfinder_blob_t cmucamBlob;
+
void SIP::Fill(player_p2os_data_t* data)
{
***************
*** 82,85 ****
--- 87,91 ----
// sonar
data->sonar.ranges_count = PlayerRobotParams[param_idx].SonarNum;
+ data->sonar.ranges = new float[data->sonar.ranges_count];
for(int
i=0;i<MIN(PlayerRobotParams[param_idx].SonarNum,ARRAYSIZE(sonars));i++)
data->sonar.ranges[i] = this->sonars[i] / 1e3;
***************
*** 100,103 ****
--- 106,110 ----
// lift
data->lift.actuators_count = 1;
+ data->lift.actuators = &liftActuator;
data->lift.actuators[0].speed = 0;
data->lift.actuators[0].acceleration = -1;
***************
*** 136,143 ****
int j = 0;
for(int i=PlayerRobotParams[param_idx].NumFrontBumpers-1;i>=0;i--)
! data->bumper.bumpers[j++] =
(unsigned char)((this->frontbumpers >> i) & 0x01);
for(int i=PlayerRobotParams[param_idx].NumRearBumpers-1;i>=0;i--)
! data->bumper.bumpers[j++] =
(unsigned char)((this->rearbumpers >> i) & 0x01);
--- 143,150 ----
int j = 0;
for(int i=PlayerRobotParams[param_idx].NumFrontBumpers-1;i>=0;i--)
! data->bumper.bumpers[j++] =
(unsigned char)((this->frontbumpers >> i) & 0x01);
for(int i=PlayerRobotParams[param_idx].NumRearBumpers-1;i>=0;i--)
! data->bumper.bumpers[j++] =
(unsigned char)((this->rearbumpers >> i) & 0x01);
***************
*** 167,172 ****
** since CMUcam doesn't have range information, but does have a
** confidence value, I'm passing it back as range. */
memset(data->blobfinder.blobs,0,
! sizeof(player_blobfinder_blob_t)*PLAYER_BLOBFINDER_MAX_BLOBS);
data->blobfinder.width = CMUCAM_IMAGE_WIDTH;
data->blobfinder.height = CMUCAM_IMAGE_HEIGHT;
--- 174,180 ----
** since CMUcam doesn't have range information, but does have a
** confidence value, I'm passing it back as range. */
+ data->blobfinder.blobs = &cmucamBlob;
memset(data->blobfinder.blobs,0,
! sizeof(player_blobfinder_blob_t));
data->blobfinder.width = CMUCAM_IMAGE_WIDTH;
data->blobfinder.height = CMUCAM_IMAGE_HEIGHT;
***************
*** 190,195 ****
///////////////////////////////////////////////////////////////
// Fill in arm data
- memset (data->actArray.actuators, 0, sizeof (player_actarray_actuator_t) *
PLAYER_ACTARRAY_NUM_ACTUATORS);
data->actArray.actuators_count = armNumJoints;
for (int ii = 0; ii < armNumJoints; ii++)
{
--- 198,204 ----
///////////////////////////////////////////////////////////////
// Fill in arm data
data->actArray.actuators_count = armNumJoints;
+ data->actArray.actuators = new player_actarray_actuator_t[armNumJoints];
// This will be cleaned up in P2OS::PutData
+ memset (data->actArray.actuators, 0, sizeof (player_actarray_actuator_t) *
armNumJoints);
for (int ii = 0; ii < armNumJoints; ii++)
{
***************
*** 399,402 ****
--- 408,429 ----
cnt += sizeof(unsigned char);
+ if (buffer[cnt] == 0 && sonars != NULL)
+ {
+ // No sonar readings
+ delete[] sonars;
+ sonars = NULL;
+ }
+ else if (sonars == NULL)
+ {
+ // No space for sonar readings yet but need some
+ sonars = new unsigned short[buffer[cnt]];
+ }
+ else if (buffer[cnt] != sonarreadings)
+ {
+ // Sonar readings count has changed, reallocate the sonar readings array
+ if (sonars != NULL)
+ delete[] sonars;
+ sonars = new unsigned short[buffer[cnt]];
+ }
sonarreadings = buffer[cnt];
cnt += sizeof(unsigned char);
Index: sip.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/sip.h,v
retrieving revision 1.8
retrieving revision 1.8.2.1
diff -C2 -d -r1.8 -r1.8.2.1
*** sip.h 10 Jul 2006 16:55:38 -0000 1.8
--- sip.h 14 Sep 2007 00:21:10 -0000 1.8.2.1
***************
*** 57,61 ****
unsigned short rawypos, frontbumpers, rearbumpers;
short angle, lvel, rvel, control;
! unsigned short sonars[PLAYER_SONAR_MAX_SAMPLES];
int xpos, ypos;
int x_offset,y_offset,angle_offset;
--- 57,61 ----
unsigned short rawypos, frontbumpers, rearbumpers;
short angle, lvel, rvel, control;
! unsigned short *sonars;
int xpos, ypos;
int x_offset,y_offset,angle_offset;
***************
*** 99,104 ****
{
param_idx = idx;
! for(int i=0;i<ARRAYSIZE(sonars);i++)
! sonars[i] = 0;
xpos = INT_MAX;
--- 99,104 ----
{
param_idx = idx;
! sonarreadings = 0;
! sonars = NULL;
xpos = INT_MAX;
***************
*** 119,122 ****
--- 119,128 ----
}
}
+
+ ~SIP(void)
+ {
+ if (sonars != NULL)
+ delete[] sonars;
+ }
};
-------------------------------------------------------------------------
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
