packet.cc
Go to the documentation of this file.
00001 /*
00002  *  P2OS for ROS
00003  *  Copyright (C) 2000
00004  *      Hunter Allen, David Feil-Seifer, Brian Gerkey, Kasper Stoy,
00005  *      Richard Vaughan, & Andrew Howard
00006  *
00007  *
00008  *  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU General Public License for more details.
00017  *
00018  *  You should have received a copy of the GNU General Public License
00019  *  along with this program; if not, write to the Free Software
00020  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  */
00023 
00024 #include <stdio.h>
00025 #include <errno.h>
00026 #include <string.h>
00027 #include <p2os_driver/packet.h>
00028 #include <unistd.h>
00029 #include <stdlib.h> /* for exit() */
00030 #include <ros/ros.h>
00031 
00032 void P2OSPacket::Print()
00033 {
00034     if (packet) {
00035         ROS_INFO("\"");
00036         for(int i=0;i<size;i++)
00037             ROS_INFO("%u ", packet[i]);
00038         ROS_INFO("\"");
00039     }
00040 }
00041 
00042 void P2OSPacket::PrintHex()
00043 {
00044     if (packet) {
00045         ROS_INFO("\"");
00046         for(int i=0;i<size;i++)
00047             ROS_INFO("0x%.2x ", packet[i]);
00048         ROS_INFO("\"");
00049     }
00050 }
00051 
00052 
00053 bool P2OSPacket::Check() {
00054     short chksum;
00055     chksum = CalcChkSum();
00056 
00057     if ((chksum == packet[size-2] << 8) | packet[size-1])
00058         return(true);
00059 
00060 
00061     return(false);
00062 }
00063 
00064 int P2OSPacket::CalcChkSum() {
00065     unsigned char *buffer = &packet[3];
00066     int c = 0;
00067     int n;
00068 
00069     n = size - 5;
00070 
00071     while (n > 1) {
00072         c+= (*(buffer)<<8) | *(buffer+1);
00073         c = c & 0xffff;
00074         n -= 2;
00075         buffer += 2;
00076     }
00077     if (n>0) c = c^ (int)*(buffer++);
00078 
00079     return(c);
00080 }
00081 
00082 int P2OSPacket::Receive(int fd)
00083 {
00084     unsigned char prefix[3];
00085     int cnt;
00086 
00087     memset(packet,0,sizeof(packet));
00088 
00089     do
00090     {
00091         memset(prefix,0,sizeof(prefix));
00092 
00093         while(1)
00094         {
00095             cnt = 0;
00096             while( cnt!=1 )
00097             {
00098                 if ( (cnt+=read( fd, &prefix[2], 1 )) < 0 )
00099                 {
00100                     ROS_ERROR("Error reading packet header from robot connection: P2OSPacket():Receive():read():");
00101                     return(1);
00102                 }
00103             }
00104 
00105             if (prefix[0] == 0xFA && prefix[1] == 0xFB)
00106                 break;
00107 
00108             timestamp = ros::Time::now();
00109 
00110             //GlobalTime->GetTimeDouble(&timestamp);
00111 
00112             prefix[0] = prefix[1];
00113             prefix[1] = prefix[2];
00114             //skipped++;
00115         }
00116         //if (skipped>3) ROS_INFO("Skipped %d bytes\n", skipped);
00117 
00118         size = prefix[2]+3;
00119         memcpy( packet, prefix, 3);
00120 
00121         cnt = 0;
00122         while( cnt!=prefix[2] )
00123         {
00124             if ((cnt+=read(fd, &packet[3+cnt],  prefix[2]-cnt)) < 0 )
00125             {
00126                 ROS_ERROR("Error reading packet body from robot connection: P2OSPacket():Receive():read():");
00127                 return(1);
00128             }
00129         }
00130     } while (!Check());
00131     return(0);
00132 }
00133 
00134 int P2OSPacket::Build(unsigned char *data, unsigned char datasize) {
00135     short chksum;
00136 
00137     size = datasize + 5;
00138 
00139     /* header */
00140     packet[0]=0xFA;
00141     packet[1]=0xFB;
00142 
00143     if ( size > 198 ) {
00144         ROS_ERROR("Packet to P2OS can't be larger than 200 bytes");
00145         return(1);
00146     }
00147     packet[2] = datasize + 2;
00148 
00149     memcpy(&packet[3], data, datasize);
00150 
00151     chksum = CalcChkSum();
00152     packet[3+datasize] = chksum >> 8;
00153     packet[3+datasize+1] = chksum & 0xFF;
00154 
00155     if (!Check()) {
00156         ROS_ERROR("DAMN");
00157         return(1);
00158     }
00159     return(0);
00160 }
00161 
00162 int P2OSPacket::Send(int fd)
00163 {
00164     int cnt=0;
00165 
00166     while (cnt != size)
00167     {
00168         if((cnt += write( fd, packet, size )) < 0)
00169         {
00170             ROS_ERROR("Send");
00171             return(1);
00172         }
00173     }
00174     return(0);
00175 }


p2os_driver
Author(s): Hunter L. Allen , David Feil-Seifer , Aris Synodinos , Brian Gerkey, Kasper Stoy, Richard Vaughan, Andrew Howard, Tucker Hermans, ActivMedia Robotics LLC, MobileRobots Inc.
autogenerated on Sat Aug 5 2017 02:20:17