Update of /cvsroot/playerstage/code/player/server/drivers/mixed/p2os
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv10946/server/drivers/mixed/p2os
Modified Files:
.cvsignore p2os.cc p2os.h sip.cc sip.h
Log Message:
applied Toby's patch to replace fixed-size arrays
Index: .cvsignore
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/p2os/.cvsignore,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** .cvsignore 17 Sep 2007 02:18:55 -0000 1.2
--- .cvsignore 1 Nov 2007 22:16:20 -0000 1.3
***************
*** 4,5 ****
--- 4,7 ----
*.la
*.a
+ .libs
+ *.lo
Index: p2os.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/p2os.cc,v
retrieving revision 1.84
retrieving revision 1.85
diff -C2 -d -r1.84 -r1.85
*** p2os.cc 30 Oct 2007 20:39:38 -0000 1.84
--- p2os.cc 1 Nov 2007 22:16:20 -0000 1.85
***************
*** 326,329 ****
--- 326,330 ----
#include "p2os.h"
+ #include <libplayerxdr/playerxdr.h>
Driver*
***************
*** 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);
--- 616,642 ----
}
// 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);
***************
*** 1302,1305 ****
--- 1303,1320 ----
P2OS::~P2OS (void)
{
+ player_position2d_data_t_cleanup(&p2os_data.position);
+ player_sonar_data_t_cleanup (&p2os_data.sonar);
+ player_gripper_data_t_cleanup (&p2os_data.gripper);
+ player_gripper_data_t_cleanup (&p2os_data.armGripper);
+ player_power_data_t_cleanup (&p2os_data.power);
+ player_bumper_data_t_cleanup (&p2os_data.bumper);
+ player_dio_data_t_cleanup (&p2os_data.dio);
+ player_aio_data_t_cleanup (&p2os_data.aio);
+ player_blobfinder_data_t_cleanup (&p2os_data.blobfinder);
+ player_position2d_data_t_cleanup (&p2os_data.compass);
+ player_position2d_data_t_cleanup (&p2os_data.gyro);
+ player_actarray_data_t_cleanup (&p2os_data.lift);
+ player_actarray_data_t_cleanup (&p2os_data.actArray);
+
if (kineCalc)
{
***************
*** 1372,1376 ****
// put odometry data
! this->Publish(this->position_id,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
--- 1387,1391 ----
// 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,
--- 1395,1399 ----
// 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,
--- 1401,1408 ----
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,
--- 1412,1416 ----
// 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,
--- 1420,1424 ----
// 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,
--- 1436,1440 ----
// 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,
--- 1444,1448 ----
// 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,
--- 1452,1456 ----
// 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,
--- 1460,1464 ----
// 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,
--- 1468,1472 ----
// 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,
--- 1476,1480 ----
// 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,
--- 1482,1489 ----
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,
***************
*** 2262,2265 ****
--- 2279,2283 ----
player_sonar_geom_t geom;
geom.poses_count = PlayerRobotParams[param_idx].SonarNum;
+ geom.poses = new player_pose3d_t[geom.poses_count];
for(int i = 0; i < PlayerRobotParams[param_idx].SonarNum; i++)
{
***************
*** 2272,2276 ****
this->Publish(this->sonar_id, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_SONAR_REQ_GET_GEOM,
! (void*)&geom, sizeof(geom), NULL);
return(0);
}
--- 2290,2295 ----
this->Publish(this->sonar_id, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_SONAR_REQ_GET_GEOM,
! (void*)&geom);
! delete [] geom.poses;
return(0);
}
***************
*** 2384,2389 ****
--- 2403,2416 ----
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++)
***************
*** 2413,2416 ****
--- 2440,2444 ----
this->Publish(this->actarray_id, resp_queue, PLAYER_MSGTYPE_RESP_ACK,
PLAYER_ACTARRAY_REQ_GET_GEOM, &aaGeom, sizeof (aaGeom), NULL);
+ delete[] actuators;
return 0;
}
***************
*** 2471,2474 ****
--- 2499,2503 ----
player_bumper_geom_t geom;
geom.bumper_def_count = PlayerRobotParams[param_idx].NumFrontBumpers +
PlayerRobotParams[param_idx].NumRearBumpers;
+ geom.bumper_def = new player_bumper_define_t[geom.bumper_def_count];
for(unsigned int ii = 0; ii < geom.bumper_def_count; ii++)
{
***************
*** 2483,2487 ****
this->Publish(this->bumper_id, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_BUMPER_REQ_GET_GEOM,
! (void*)&geom, sizeof(geom), NULL);
return(0);
}
--- 2512,2517 ----
this->Publish(this->bumper_id, resp_queue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_BUMPER_REQ_GET_GEOM,
! (void*)&geom);
! delete [] geom.bumper_def;
return(0);
}
***************
*** 2489,2495 ****
{
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;
--- 2519,2527 ----
{
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.35
diff -C2 -d -r1.34 -r1.35
*** p2os.h 23 Aug 2007 19:58:45 -0000 1.34
--- p2os.h 1 Nov 2007 22:16:21 -0000 1.35
***************
*** 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.25
diff -C2 -d -r1.24 -r1.25
*** sip.cc 20 Aug 2007 06:37:29 -0000 1.24
--- sip.cc 1 Nov 2007 22:16:21 -0000 1.25
***************
*** 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;
***************
*** 133,143 ****
///////////////////////////////////////////////////////////////
// bumper
! data->bumper.bumpers_count = PlayerRobotParams[param_idx].NumFrontBumpers +
PlayerRobotParams[param_idx].NumRearBumpers;
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);
--- 140,156 ----
///////////////////////////////////////////////////////////////
// bumper
! int bump_count = PlayerRobotParams[param_idx].NumFrontBumpers +
PlayerRobotParams[param_idx].NumRearBumpers;
! if (data->bumper.bumpers_count != bump_count)
! {
! data->bumper.bumpers_count = bump_count;
! delete [] data->bumper.bumpers;
! data->bumper.bumpers = new uint8_t[bump_count];
! }
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);
***************
*** 158,161 ****
--- 171,176 ----
//TODO: should do this smarter, based on which analog input is selected
data->aio.voltages_count = (unsigned char)1;
+ if (!data->aio.voltages)
+ data->aio.voltages = new float[1];
data->aio.voltages[0] = (this->analog / 255.0) * 5.0;
***************
*** 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;
--- 182,188 ----
** 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;
***************
*** 175,178 ****
--- 191,196 ----
{
data->blobfinder.blobs_count = 1;
+ if (!data->blobfinder.blobs)
+ data->blobfinder.blobs = new player_blobfinder_blob_t[1];
data->blobfinder.blobs[0].color = this->blobcolor;
data->blobfinder.blobs[0].x = this->blobmx;
***************
*** 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++)
{
--- 208,214 ----
///////////////////////////////////////////////////////////////
// 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 ****
--- 418,439 ----
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.9
diff -C2 -d -r1.8 -r1.9
*** sip.h 10 Jul 2006 16:55:38 -0000 1.8
--- sip.h 1 Nov 2007 22:16:21 -0000 1.9
***************
*** 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: 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
