34 for(
int i=0;i<
size;i++) {
35 ROS_INFO(
"%u ",
packet[i]);
44 for(
int i=0;i<
size;i++) {
45 ROS_INFO(
"0x%.2x ",
packet[i]);
64 unsigned char *buffer = &
packet[3];
71 c+= (*(buffer)<<8) | *(buffer+1);
76 if (n>0) c = c^ (int)*(buffer++);
83 unsigned char prefix[3];
91 memset(prefix,0,
sizeof(prefix));
99 if ( (cnt+=read( fd, &prefix[2], 1 )) < 0 )
101 ROS_ERROR(
"Error reading packet header from robot connection: P2OSPacket():Receive():read():");
106 if (prefix[0]==0xFA && prefix[1]==0xFB)
break;
119 memcpy(
packet, prefix, 3);
122 while( cnt!=prefix[2] )
124 if ( (cnt+=read( fd, &
packet[3+cnt], prefix[2]-cnt )) < 0 )
126 ROS_ERROR(
"Error reading packet body from robot connection: P2OSPacket():Receive():read():");
144 ROS_ERROR(
"Packet to P2OS can't be larger than 200 bytes");
149 memcpy( &
packet[3], data, datasize );
152 packet[3+datasize] = chksum >> 8;
153 packet[3+datasize+1] = chksum & 0xFF;
unsigned char packet[PACKET_LEN]
int Build(unsigned char *data, unsigned char datasize)