packet.cc
Go to the documentation of this file.
1 /*
2  * P2OS for ROS
3  * Copyright (C) 2000
4  * David Feil-Seifer, Brian Gerkey, Kasper Stoy,
5  * Richard Vaughan, & Andrew Howard
6  *
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
21  *
22  */
23 
24 #include <stdio.h>
25 #include <errno.h>
26 #include <string.h>
27 #include <p2os_driver/packet.h>
28 #include <unistd.h>
29 #include <stdlib.h> /* for exit() */
30 #include <ros/ros.h>
32  if (packet) {
33  ROS_INFO("\"");
34  for(int i=0;i<size;i++) {
35  ROS_INFO("%u ", packet[i]);
36  }
37  ROS_INFO("\"");
38  }
39 }
40 
42  if (packet) {
43  ROS_INFO("\"");
44  for(int i=0;i<size;i++) {
45  ROS_INFO("0x%.2x ", packet[i]);
46  }
47  ROS_INFO("\"");
48  }
49 }
50 
51 
53  short chksum;
54  chksum = CalcChkSum();
55 
56  if ( (chksum == packet[size-2] << 8) | packet[size-1])
57  return(true);
58 
59 
60  return(false);
61 }
62 
64  unsigned char *buffer = &packet[3];
65  int c = 0;
66  int n;
67 
68  n = size - 5;
69 
70  while (n > 1) {
71  c+= (*(buffer)<<8) | *(buffer+1);
72  c = c & 0xffff;
73  n -= 2;
74  buffer += 2;
75  }
76  if (n>0) c = c^ (int)*(buffer++);
77 
78  return(c);
79 }
80 
81 int P2OSPacket::Receive( int fd )
82 {
83  unsigned char prefix[3];
84  //int skipped=0;
85  int cnt;
86 
87  memset(packet,0,sizeof(packet));
88 
89  do
90  {
91  memset(prefix,0,sizeof(prefix));
92  //memset( prefix, 0, 3);
93 
94  while(1)
95  {
96  cnt = 0;
97  while( cnt!=1 )
98  {
99  if ( (cnt+=read( fd, &prefix[2], 1 )) < 0 )
100  {
101  ROS_ERROR("Error reading packet header from robot connection: P2OSPacket():Receive():read():");
102  return(1);
103  }
104  }
105 
106  if (prefix[0]==0xFA && prefix[1]==0xFB) break;
107 
108  timestamp = ros::Time::now();
109 
110  //GlobalTime->GetTimeDouble(&timestamp);
111 
112  prefix[0]=prefix[1];
113  prefix[1]=prefix[2];
114  //skipped++;
115  }
116  //if (skipped>3) ROS_INFO("Skipped %d bytes\n", skipped);
117 
118  size = prefix[2]+3;
119  memcpy( packet, prefix, 3);
120 
121  cnt = 0;
122  while( cnt!=prefix[2] )
123  {
124  if ( (cnt+=read( fd, &packet[3+cnt], prefix[2]-cnt )) < 0 )
125  {
126  ROS_ERROR("Error reading packet body from robot connection: P2OSPacket():Receive():read():");
127  return(1);
128  }
129  }
130  } while (!Check());
131  return(0);
132 }
133 
134 int P2OSPacket::Build( unsigned char *data, unsigned char datasize ) {
135  short chksum;
136 
137  size = datasize + 5;
138 
139  /* header */
140  packet[0]=0xFA;
141  packet[1]=0xFB;
142 
143  if ( size > 198 ) {
144  ROS_ERROR("Packet to P2OS can't be larger than 200 bytes");
145  return(1);
146  }
147  packet[2] = datasize + 2;
148 
149  memcpy( &packet[3], data, datasize );
150 
151  chksum = CalcChkSum();
152  packet[3+datasize] = chksum >> 8;
153  packet[3+datasize+1] = chksum & 0xFF;
154 
155  if (!Check()) {
156  ROS_ERROR("DAMN");
157  return(1);
158  }
159  return(0);
160 }
161 
162 int P2OSPacket::Send( int fd)
163 {
164  int cnt=0;
165 
166  //ROS_INFO("Send(): ");
167  //PrintHex();
168  while(cnt!=size)
169  {
170  if((cnt += write( fd, packet, size )) < 0)
171  {
172  ROS_ERROR("Send");
173  return(1);
174  }
175  }
176  return(0);
177 }
void PrintHex()
Definition: packet.cc:41
unsigned char packet[PACKET_LEN]
Definition: packet.h:36
void Print()
Definition: packet.cc:31
ros::Time timestamp
Definition: packet.h:38
bool Check()
Definition: packet.cc:52
int Build(unsigned char *data, unsigned char datasize)
Definition: packet.cc:134
int Receive(int fd)
Definition: packet.cc:81
unsigned char size
Definition: packet.h:37
int CalcChkSum()
Definition: packet.cc:63
int Send(int fd)
Definition: packet.cc:162


p2os_driver
Author(s): Hunter Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Wed Jun 25 2014 09:37:15