packet_finder.cpp
Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Includes
00012 *****************************************************************************/
00013 
00014 #include <sstream>
00015 #include "../../include/kobuki_driver/packet_handler/packet_finder.hpp"
00016 
00017 /*****************************************************************************
00018 ** Namespaces
00019 *****************************************************************************/
00020 
00021 namespace kobuki {
00022 
00023 /*****************************************************************************
00024 ** Implementation
00025 *****************************************************************************/
00026 
00027 PacketFinderBase::PacketFinderBase() :
00028     state(waitingForStx), verbose(false)
00029 {
00030 }
00031 
00032 
00033 /*****************************************************************************
00034 ** Public
00035 *****************************************************************************/
00036 
00037 void PacketFinderBase::configure(const std::string &sigslots_namespace,
00038                const BufferType & putStx, const BufferType & putEtx, unsigned int sizeLengthField,
00039                unsigned int sizeMaxPayload, unsigned int sizeChecksumField, bool variableSizePayload)
00040 {
00041   size_stx = putStx.size();
00042   size_etx = putEtx.size();
00043   size_length_field = sizeLengthField;
00044   variable_size_payload = variableSizePayload;
00045   size_max_payload = sizeMaxPayload;
00046   size_payload = variable_size_payload ? 0 : sizeMaxPayload;
00047   size_checksum_field = sizeChecksumField;
00048   STX = putStx;
00049   ETX = putEtx;
00050   buffer = BufferType(size_stx + size_length_field + size_max_payload + size_checksum_field + size_etx);
00051   state = waitingForStx;
00052 
00053   sig_warn.connect(sigslots_namespace + std::string("/ros_warn"));
00054   sig_error.connect(sigslots_namespace + std::string("/ros_error"));
00055 
00056   //todo; exception
00057   // Problem1: size_length_field = 1, vairable_size_payload = false
00058 
00059   clear();
00060 }
00061 
00062 void PacketFinderBase::clear()
00063 {
00064   state = waitingForStx;
00065   buffer.clear();
00066 }
00067 
00068 void PacketFinderBase::enableVerbose()
00069 {
00070   verbose = true;
00071 }
00072 
00073 bool PacketFinderBase::checkSum()
00074 {
00075   return true;
00076 }
00077 
00078 unsigned int PacketFinderBase::numberOfDataToRead()
00079 {
00080   unsigned int num(0);
00081 
00082   switch (state)
00083   {
00084     case waitingForEtx:
00085       num = 1;
00086       break;
00087 
00088     case waitingForPayloadToEtx:
00089       num = size_payload + size_etx + size_checksum_field;
00090       break;
00091 
00092     case waitingForPayloadSize:
00093       num = size_checksum_field;
00094       break;
00095 
00096     case waitingForStx:
00097     case clearBuffer:
00098     default:
00099       num = 1;
00100       break;
00101   }
00102 
00103   if (verbose)
00104   {
00105     printf("[state(%d):%02d]", state, num);
00106   }
00107   return num;
00108 }
00109 
00110 void PacketFinderBase::getBuffer(BufferType & bufferRef)
00111 {
00112   bufferRef = buffer;
00113 }
00114 
00115 void PacketFinderBase::getPayload(BufferType & bufferRef)
00116 {
00117   bufferRef.clear();
00118   bufferRef.resize( buffer.size() - size_stx - size_etx - size_length_field - size_checksum_field );
00119   for (unsigned int i = size_stx + size_length_field; i < buffer.size() - size_etx - size_checksum_field; i++) {
00120     bufferRef.push_back(buffer[i]);
00121   }
00122 }
00123 
00131 bool PacketFinderBase::update(const unsigned char * incoming, unsigned int numberOfIncoming)
00132 {
00133   // clearBuffer = 0, waitingForStx, waitingForPayloadSize, waitingForPayloadToEtx, waitingForEtx,
00134   // std::cout << "update [" << numberOfIncoming << "][" << state << "]" << std::endl;
00135   if (!(numberOfIncoming > 0))
00136     return false;
00137 
00138   bool found_packet(false);
00139 
00140   if ( state == clearBuffer ) {
00141     buffer.clear();
00142     state = waitingForStx;
00143   }
00144   switch (state)
00145   {
00146     case waitingForStx:
00147       if (WaitForStx(incoming[0]))
00148       {
00149         if (size_length_field)
00150         {
00151           state = waitingForPayloadSize; // kobukibot
00152         }
00153         else
00154         {
00155           if (variable_size_payload)
00156           {
00157             // e.g. stargazer
00158             state = waitingForEtx;
00159           }
00160           else
00161           {
00162             // e.g. iroboQ
00163             //Todo; should put correct state
00164             state = waitingForPayloadToEtx;
00165           }
00166         }
00167       }
00168       break;
00169     case waitingForEtx:
00170       if (waitForEtx(incoming[0], found_packet))
00171       {
00172         state = clearBuffer;
00173       }
00174       break;
00175 
00176     case waitingForPayloadSize:
00177       if (waitForPayloadSize(incoming, numberOfIncoming))
00178       {
00179         state = waitingForPayloadToEtx;
00180       }
00181       break;
00182 
00183     case waitingForPayloadToEtx:
00184       if (waitForPayloadAndEtx(incoming, numberOfIncoming, found_packet))
00185       {
00186         state = clearBuffer;
00187       }
00188       break;
00189 
00190     default:
00191       state = waitingForStx;
00192       break;
00193   }
00194   if ( found_packet ) {
00195     return checkSum();  //what happen if checksum is equal to false(== -1)?
00196   } else {
00197     return false;
00198   }
00199 }
00200 /*****************************************************************************
00201 ** Protected
00202 *****************************************************************************/
00203 
00204 bool PacketFinderBase::WaitForStx(const unsigned char datum)
00205 {
00206   bool found_stx(true);
00207 
00208   // add incoming datum
00209   buffer.push_back(datum);
00210 
00211   // check whether we have STX
00212   for (unsigned int i = 0; i < buffer.size() && i < STX.size(); i++)
00213   {
00214     if (buffer[i] != STX[i])
00215     {
00216       found_stx = false;
00217       buffer.pop_front();
00218       break;
00219     }
00220   }
00221 
00222   return (found_stx && buffer.size() == STX.size());
00223 }
00224 
00225 bool PacketFinderBase::waitForPayloadSize(const unsigned char * incoming, unsigned int numberOfIncoming)
00226 {
00227   // push data
00228   unsigned char first_byte;
00229   for (unsigned int i = 0; i < numberOfIncoming; i++) {
00230     first_byte = incoming[i];
00231     buffer.push_back(incoming[i]);
00232   }
00233 
00234   if (verbose)
00235   {
00236     for (unsigned int i = 0; i < buffer.size(); i++)
00237       printf("%02x ", buffer[i]);
00238     printf("\n");
00239   }
00240 
00241   // check when we need to wait for etx
00242   if (buffer.size() < size_stx + size_length_field)
00243   {
00244     return false;
00245   }
00246   else
00247   {
00248     switch (size_length_field)
00249     {
00250       case 1: // kobuki
00251         size_payload = buffer[size_stx];
00252         break;
00253       case 2:
00254         size_payload = buffer[size_stx];
00255         size_payload |= buffer[size_stx + 1] << 8;
00256         break;
00257       case 4:
00258         size_payload = buffer[size_stx];
00259         size_payload |= buffer[size_stx + 1] << 8;
00260         size_payload |= buffer[size_stx + 2] << 16;
00261         size_payload |= buffer[size_stx + 3] << 24;
00262         break;
00263       default:
00264         // put assertion failure
00265         size_payload = 1;
00266         break;
00267     }
00268 
00269     if (verbose)
00270     {
00271       printf("[payloadSize: %d]\n", size_payload);
00272     }
00273 
00274     return true;
00275   }
00276 }
00277 
00278 bool PacketFinderBase::waitForEtx(const unsigned char incoming, bool & foundPacket)
00279 {
00280   // push data
00281   buffer.push_back(incoming);
00282 
00283   // check when we need to wait for etx
00284   // if minimum payload size is 1
00285   if (buffer.size() < size_stx + size_etx + 1)
00286   {
00287     return false;
00288   }
00289   else
00290   {
00291     unsigned int number_of_match(0);
00292     for (unsigned int i = 0; i < ETX.size(); i++)
00293     {
00294       if (buffer[buffer.size() - ETX.size() + i] == ETX[i])
00295       {
00296         number_of_match++;
00297       }
00298     }
00299 
00300     if (number_of_match == ETX.size())
00301     {
00302       foundPacket = true;
00303       return true;
00304     }
00305 
00306     if (buffer.size() >= size_stx + size_max_payload + size_etx)
00307       return true;
00308     else
00309       return false;
00310   }
00311 }
00312 
00313 bool PacketFinderBase::waitForPayloadAndEtx(const unsigned char * incoming, unsigned int numberOfIncoming, bool & foundPacket)
00314 {
00315   // push data
00316   for (unsigned int i = 0; i < numberOfIncoming; i++)
00317   {
00318     buffer.push_back(incoming[i]);
00319   }
00320   /*********************
00321   ** Error Handling
00322   **********************/
00323   if ( size_payload > size_max_payload ) {
00324     state = clearBuffer;
00325     std::ostringstream ostream;
00326     ostream << "abnormally sized payload retrieved, clearing [" << size_max_payload << "][" << size_payload << "]";
00327 
00328     ostream << std::setfill('0') << std::uppercase; //declare once, use everytime
00329 //      ostream.fill('0') //call once, affects everytime
00330 //      ostream.width(2); //need to call everytime
00331 
00332 /*
00333     ostream << "[";
00334     for (unsigned int i = 0; i < numberOfIncoming; ++i ) {
00335       ostream.width(2); // need to declare evertime
00336       ostream << std::hex << static_cast<int>(*(incoming+i)) << " " << std::dec;
00337     }
00338     ostream << "\b]";
00339 */
00340     ostream << ", buffer: [" << std::setw(2) << buffer.size() << "][";
00341     for (unsigned int i = 0; i < buffer.size(); ++i ) {
00342       ostream << std::setw(2) << std::hex << static_cast<int>(buffer[i]) << " " << std::dec;
00343     }
00344     ostream << "\b]";
00345 
00346     sig_error.emit(ostream.str());
00347     return false;
00348   }
00349   // check when we need to wait for etx
00350   if (buffer.size() < size_stx + size_length_field + size_payload + size_checksum_field + size_etx)
00351   {
00352     return false;
00353   }
00354   else
00355   {
00356     if (verbose) {
00357       std::cout << "Start check etx " << std::endl;
00358       for (unsigned int i = 0; i < numberOfIncoming; ++i ) {
00359         std::cout << std::hex << static_cast<int>(*(incoming+i)) << " ";
00360       }
00361       std::cout << std::dec << std::endl;
00362     }
00363     foundPacket = true;
00364 
00365     for (unsigned int i = (size_stx + size_length_field + size_payload + size_checksum_field);
00366         i < (size_stx + size_length_field + size_payload + size_checksum_field + size_etx); i++)
00367     {
00368       if (buffer[i] != ETX[i])
00369       {
00370         foundPacket = false;
00371       }
00372     }
00373     if (verbose)
00374       std::cout << "End of checking etx " << std::endl;
00375     return true;
00376   }
00377 }
00378 
00379 
00380 } // namespace kobuki


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37