Update of /cvsroot/playerstage/code/player/server/drivers/mixed/p2os
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv26274/server/drivers/mixed/p2os
Modified Files:
p2os.cc p2os.h packet.cc packet.h sip.cc sip.h
Log Message:
applied patch
[ 1876540 ] Separation of publishing data in p2os interfaces.
Index: packet.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/packet.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** packet.h 29 Nov 2002 17:07:43 -0000 1.1
--- packet.h 15 Feb 2008 08:53:55 -0000 1.2
***************
*** 33,36 ****
--- 33,39 ----
#include <string.h>
+ #include <libplayercore/globals.h>
+ #include <libplayercore/wallclocktime.h>
+
#define PACKET_LEN 256
***************
*** 40,43 ****
--- 43,47 ----
unsigned char packet[PACKET_LEN];
unsigned char size;
+ double timestamp;
int CalcChkSum();
Index: p2os.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/p2os.cc,v
retrieving revision 1.86
retrieving revision 1.87
diff -C2 -d -r1.86 -r1.87
*** p2os.cc 7 Dec 2007 01:50:15 -0000 1.86
--- p2os.cc 15 Feb 2008 08:53:55 -0000 1.87
***************
*** 1382,1389 ****
void
! P2OS::PutData(void)
{
- // TODO: something smarter about timestamping.
-
// put odometry data
this->Publish(this->position_id,
--- 1382,1387 ----
void
! P2OS::StandardSIPPutData(double timestampStandardSIP)
{
// put odometry data
this->Publish(this->position_id,
***************
*** 1392,1396 ****
(void*)&(this->p2os_data.position),
sizeof(player_position2d_data_t),
! NULL);
// put sonar data
--- 1390,1394 ----
(void*)&(this->p2os_data.position),
sizeof(player_position2d_data_t),
! ×tampStandardSIP);
// put sonar data
***************
*** 1400,1404 ****
(void*)&(this->p2os_data.sonar),
sizeof(player_sonar_data_t),
! NULL);
delete this->p2os_data.sonar.ranges;
--- 1398,1402 ----
(void*)&(this->p2os_data.sonar),
sizeof(player_sonar_data_t),
! ×tampStandardSIP);
delete this->p2os_data.sonar.ranges;
***************
*** 1409,1413 ****
(void*)&(this->p2os_data.aio),
sizeof(player_aio_data_t),
! NULL);
// put dio data
--- 1407,1411 ----
(void*)&(this->p2os_data.aio),
sizeof(player_aio_data_t),
! ×tampStandardSIP);
// put dio data
***************
*** 1417,1421 ****
(void*)&(this->p2os_data.dio),
sizeof(player_dio_data_t),
! NULL);
// put gripper data
--- 1415,1419 ----
(void*)&(this->p2os_data.dio),
sizeof(player_dio_data_t),
! ×tampStandardSIP);
// put gripper data
***************
*** 1425,1429 ****
(void*)&(this->p2os_data.gripper),
sizeof(player_gripper_data_t),
! NULL);
// put lift data
--- 1423,1427 ----
(void*)&(this->p2os_data.gripper),
sizeof(player_gripper_data_t),
! ×tampStandardSIP);
// put lift data
***************
*** 1433,1437 ****
(void*)&(this->p2os_data.lift),
sizeof(player_actarray_data_t),
! NULL);
// put bumper data
--- 1431,1435 ----
(void*)&(this->p2os_data.lift),
sizeof(player_actarray_data_t),
! ×tampStandardSIP);
// put bumper data
***************
*** 1441,1445 ****
(void*)&(this->p2os_data.bumper),
sizeof(player_bumper_data_t),
! NULL);
// put power data
--- 1439,1443 ----
(void*)&(this->p2os_data.bumper),
sizeof(player_bumper_data_t),
! ×tampStandardSIP);
// put power data
***************
*** 1449,1453 ****
(void*)&(this->p2os_data.power),
sizeof(player_power_data_t),
! NULL);
// put compass data
--- 1447,1451 ----
(void*)&(this->p2os_data.power),
sizeof(player_power_data_t),
! ×tampStandardSIP);
// put compass data
***************
*** 1457,1462 ****
(void*)&(this->p2os_data.compass),
sizeof(player_position2d_data_t),
! NULL);
// put gyro data
this->Publish(this->gyro_id,
--- 1455,1464 ----
(void*)&(this->p2os_data.compass),
sizeof(player_position2d_data_t),
! ×tampStandardSIP);
! }
+ void
+ P2OS::GyroPutData(double timestampGyro)
+ {
// put gyro data
this->Publish(this->gyro_id,
***************
*** 1465,1470 ****
(void*)&(this->p2os_data.gyro),
sizeof(player_position2d_data_t),
! NULL);
// put blobfinder data
this->Publish(this->blobfinder_id,
--- 1467,1476 ----
(void*)&(this->p2os_data.gyro),
sizeof(player_position2d_data_t),
! ×tampGyro);
! }
+ void
+ P2OS::BlobfinderPutData(double timestampSERAUX)
+ {
// put blobfinder data
this->Publish(this->blobfinder_id,
***************
*** 1473,1478 ****
(void*)&(this->p2os_data.blobfinder),
sizeof(player_blobfinder_data_t),
! NULL);
// put actarray data
this->Publish(this->actarray_id,
--- 1479,1488 ----
(void*)&(this->p2os_data.blobfinder),
sizeof(player_blobfinder_data_t),
! ×tampSERAUX);
! }
+ void
+ P2OS::ActarrayPutData(double timestampArm)
+ {
// put actarray data
this->Publish(this->actarray_id,
***************
*** 1481,1485 ****
(void*)&(this->p2os_data.actArray),
sizeof(player_actarray_data_t),
! NULL);
delete[] this->p2os_data.actArray.actuators;
--- 1491,1495 ----
(void*)&(this->p2os_data.actArray),
sizeof(player_actarray_data_t),
! ×tampArm);
delete[] this->p2os_data.actArray.actuators;
***************
*** 1490,1494 ****
(void*)&(this->limb_data),
sizeof(player_limb_data_t),
! NULL);
// put arm gripper data
--- 1500,1504 ----
(void*)&(this->limb_data),
sizeof(player_limb_data_t),
! ×tampArm);
// put arm gripper data
***************
*** 1498,1502 ****
(void*)&(this->p2os_data.armGripper),
sizeof(player_gripper_data_t),
! NULL);
}
--- 1508,1512 ----
(void*)&(this->p2os_data.armGripper),
sizeof(player_gripper_data_t),
! ×tampArm);
}
***************
*** 1605,1611 ****
{
P2OSPacket packet;
// zero the combined data buffer. it will be filled with the latest data
! // by SIP::Fill()
memset(&(this->p2os_data),0,sizeof(player_p2os_data_t));
if((this->psos_fd >= 0) && this->sippacket)
--- 1615,1622 ----
{
P2OSPacket packet;
+ double msgTime;
// zero the combined data buffer. it will be filled with the latest data
! // by corresponding SIP::Fill*()
memset(&(this->p2os_data),0,sizeof(player_p2os_data_t));
if((this->psos_fd >= 0) && this->sippacket)
***************
*** 1629,1637 ****
/* It is a server packet, so process it */
! this->sippacket->Parse( &packet.packet[3] );
! this->sippacket->Fill(&(this->p2os_data));
if(publish_data)
! this->PutData();
}
else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
--- 1640,1648 ----
/* It is a server packet, so process it */
! this->sippacket->ParseStandard( &packet.packet[3] );
! this->sippacket->FillStandard(&(this->p2os_data));
if(publish_data)
! this->StandardSIPPutData(packet.timestamp);
}
else if(packet.packet[0] == 0xFA && packet.packet[1] == 0xFB &&
***************
*** 1644,1647 ****
--- 1655,1659 ----
{
// This is an AUX2 serial packet
+
if(blobfinder_id.interf)
{
***************
*** 1649,1656 ****
/* Be sure to pass data size too (packet[2])! */
this->sippacket->ParseSERAUX( &packet.packet[2] );
! this->sippacket->Fill(&(this->p2os_data));
!
if(publish_data)
! this->PutData();
P2OSPacket cam_packet;
--- 1661,1668 ----
/* Be sure to pass data size too (packet[2])! */
this->sippacket->ParseSERAUX( &packet.packet[2] );
! this->sippacket->FillSERAUX(&(this->p2os_data));
!
if(publish_data)
! this->BlobfinderPutData(packet.timestamp);
P2OSPacket cam_packet;
***************
*** 1700,1706 ****
/* It's a set of gyro measurements */
this->sippacket->ParseGyro(&packet.packet[2]);
! this->sippacket->Fill(&(this->p2os_data));
if(publish_data)
! this->PutData();
/* Now, the manual says that we get one gyro packet each cycle,
--- 1712,1719 ----
/* It's a set of gyro measurements */
this->sippacket->ParseGyro(&packet.packet[2]);
! this->sippacket->FillGyro(&(this->p2os_data));
!
if(publish_data)
! this->GyroPutData(packet.timestamp);
/* Now, the manual says that we get one gyro packet each cycle,
***************
*** 1730,1734 ****
joints[ii] = sippacket->armJointPosRads[ii];
}
! sippacket->Fill(&p2os_data);
if(kineCalc)
{
--- 1743,1747 ----
joints[ii] = sippacket->armJointPosRads[ii];
}
! sippacket->FillArm(&p2os_data);
if(kineCalc)
{
***************
*** 1754,1760 ****
}
}
}
! if(publish_data)
! this->PutData();
// Go for another SIP - there had better be one or things will probably
go boom
SendReceive(NULL,publish_data);
--- 1767,1774 ----
}
}
+ if(publish_data)
+ this->ActarrayPutData(packet.timestamp);
}
!
// Go for another SIP - there had better be one or things will probably
go boom
SendReceive(NULL,publish_data);
***************
*** 1773,1776 ****
--- 1787,1791 ----
// Go for another SIP - there had better be one or things will
probably go boom
}
+
SendReceive(NULL,publish_data);
}
***************
*** 1781,1784 ****
--- 1796,1800 ----
}
}
+
return(0);
}
Index: packet.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/packet.cc,v
retrieving revision 1.5
retrieving revision 1.6
diff -C2 -d -r1.5 -r1.6
*** packet.cc 2 Mar 2006 21:39:28 -0000 1.5
--- packet.cc 15 Feb 2008 08:53:55 -0000 1.6
***************
*** 112,115 ****
--- 112,117 ----
if (prefix[0]==0xFA && prefix[1]==0xFB) break;
+ GlobalTime->GetTimeDouble(×tamp);
+
prefix[0]=prefix[1];
prefix[1]=prefix[2];
Index: sip.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/sip.h,v
retrieving revision 1.10
retrieving revision 1.11
diff -C2 -d -r1.10 -r1.11
*** sip.h 27 Nov 2007 22:20:36 -0000 1.10
--- sip.h 15 Feb 2008 08:53:55 -0000 1.11
***************
*** 83,89 ****
// Need this value to calculate approx position of lift when in between up
and down
double lastLiftPos;
/* returns 0 if Parsed correctly otherwise 1 */
! void Parse( unsigned char *buffer );
void ParseSERAUX( unsigned char *buffer );
void ParseGyro(unsigned char* buffer);
--- 83,92 ----
// Need this value to calculate approx position of lift when in between up
and down
double lastLiftPos;
+
+ //Timestamping SIP packets
+ //double timeStandardSIP, timeGyro, timeSERAUX, timeArm;
/* returns 0 if Parsed correctly otherwise 1 */
! void ParseStandard( unsigned char *buffer );
void ParseSERAUX( unsigned char *buffer );
void ParseGyro(unsigned char* buffer);
***************
*** 94,98 ****
void PrintArm ();
void PrintArmInfo ();
! void Fill(player_p2os_data_t* data);
SIP(int idx)
--- 97,104 ----
void PrintArm ();
void PrintArmInfo ();
! void FillStandard(player_p2os_data_t* data);
! void FillSERAUX(player_p2os_data_t* data);
! void FillGyro(player_p2os_data_t* data);
! void FillArm(player_p2os_data_t* data);
SIP(int idx)
Index: p2os.h
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/p2os.h,v
retrieving revision 1.35
retrieving revision 1.36
diff -C2 -d -r1.35 -r1.36
*** p2os.h 1 Nov 2007 22:16:21 -0000 1.35
--- p2os.h 15 Feb 2008 08:53:55 -0000 1.36
***************
*** 146,162 ****
typedef struct player_p2os_data
{
player_position2d_data_t position;
player_sonar_data_t sonar;
player_gripper_data_t gripper;
! player_gripper_data_t armGripper;
player_power_data_t power;
player_bumper_data_t bumper;
player_dio_data_t dio;
player_aio_data_t aio;
player_blobfinder_data_t blobfinder;
! player_position2d_data_t compass;
player_position2d_data_t gyro;
! player_actarray_data_t lift;
player_actarray_data_t actArray;
} __attribute__ ((packed)) player_p2os_data_t;
--- 146,169 ----
typedef struct player_p2os_data
{
+ //Standard SIP
player_position2d_data_t position;
player_sonar_data_t sonar;
player_gripper_data_t gripper;
! player_actarray_data_t lift;
player_power_data_t power;
player_bumper_data_t bumper;
+ player_position2d_data_t compass;
player_dio_data_t dio;
player_aio_data_t aio;
+
+ //Blobfinder
player_blobfinder_data_t blobfinder;
!
! //Gyro
player_position2d_data_t gyro;
!
! //ARMPAC
player_actarray_data_t actArray;
+ player_gripper_data_t armGripper;
} __attribute__ ((packed)) player_p2os_data_t;
***************
*** 226,230 ****
void* data);
int HandleCommand(player_msghdr * hdr, void * data);
! void PutData(void);
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
int HandleGripperCommand (player_msghdr *hdr, void *data);
--- 233,240 ----
void* data);
int HandleCommand(player_msghdr * hdr, void * data);
! void StandardSIPPutData(double timestampStandardSIP);
! void GyroPutData(double timestampGyro);
! void BlobfinderPutData(double timestampSERAUX);
! void ActarrayPutData(double timestampArm);
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
int HandleGripperCommand (player_msghdr *hdr, void *data);
Index: sip.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/sip.cc,v
retrieving revision 1.29
retrieving revision 1.30
diff -C2 -d -r1.29 -r1.30
*** sip.cc 7 Dec 2007 01:50:15 -0000 1.29
--- sip.cc 15 Feb 2008 08:53:55 -0000 1.30
***************
*** 43,47 ****
static player_blobfinder_blob_t cmucamBlob;
! void SIP::Fill(player_p2os_data_t* data)
{
///////////////////////////////////////////////////////////////
--- 43,47 ----
static player_blobfinder_blob_t cmucamBlob;
! void SIP::FillStandard(player_p2os_data_t* data)
{
///////////////////////////////////////////////////////////////
***************
*** 80,88 ****
///////////////////////////////////////////////////////////////
- // gyro
- memset(&(data->gyro),0,sizeof(data->gyro));
- data->gyro.pos.pa = DTOR(this->gyro_rate);
-
- ///////////////////////////////////////////////////////////////
// sonar
data->sonar.ranges_count = PlayerRobotParams[param_idx].SonarNum;
--- 80,83 ----
***************
*** 174,178 ****
--- 169,184 ----
data->aio.voltages = new float[1];
data->aio.voltages[0] = (this->analog / 255.0) * 5.0;
+ }
+ void SIP::FillGyro(player_p2os_data_t* data)
+ {
+ ///////////////////////////////////////////////////////////////
+ // gyro
+ memset(&(data->gyro),0,sizeof(data->gyro));
+ data->gyro.pos.pa = DTOR(this->gyro_rate);
+ }
+
+ void SIP::FillSERAUX(player_p2os_data_t* data)
+ {
/* CMUcam blob tracking interface. The CMUcam only supports one blob
** (and therefore one channel too), so everything else is zero. All
***************
*** 205,209 ****
--- 211,218 ----
else
data->blobfinder.blobs_count = 0;
+ }
+ void SIP::FillArm(player_p2os_data_t* data)
+ {
///////////////////////////////////////////////////////////////
// Fill in arm data
***************
*** 329,333 ****
}
! void SIP::Parse( unsigned char *buffer )
{
int cnt = 0, change;
--- 338,342 ----
}
! void SIP::ParseStandard( unsigned char *buffer )
{
int cnt = 0, change;
-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
