Packet.cpp
Go to the documentation of this file.
00001 /*
00002  * Packet.cpp
00003  *
00004  *  Created on: 21.08.2013
00005  *      Author: G√ľnther Cwioro
00006  *
00007  *
00008  */
00009 
00010 #include <string>
00011 #include <list>
00012 
00013 #include "Packet.h"
00014 #include "functions.h"
00015 #include "RoutedFrame.h"
00016 #include "EthernetFrame.h"
00017 
00018 Packet::Packet(RoutedFrame frame)
00019 {
00020     this->id_ = frame.header_.packet_id;
00021     this->size_ = frame.header_.packet_size;
00022     this->data_type_ = frame.header_.payload_type;
00023     this->ts_ = getMillisecondsTime();
00024     this->frames_l_.clear();
00025     this->topic_ = frame.topic_;
00026     this->hostname_source_ = frame.hostname_source_;
00027     this->mc_group_ = frame.mc_g_name_;
00028     this->missed_sequences_l_.clear();
00029     highest_seq = 0;
00030     ts_last_frame_request = 0;
00031 }
00032 
00033 Packet::~Packet()
00034 {
00035 }
00036 
00037 Packet::Packet()
00038 {
00039 }
00040 
00041 unsigned long Packet::getSize()
00042 {
00043     unsigned long size = 0;
00044     for (std::list<RoutedFrame>::iterator it = frames_l_.begin(); it != frames_l_.end(); ++it)
00045     {
00046         size += (*it).getSize();
00047     }
00048 
00049     size += sizeof (id_);
00050     size += sizeof (size_);
00051     size += sizeof (data_type_);
00052     size += sizeof (ts_);
00053     size += sizeof (topic_);
00054     size += sizeof (hostname_source_);
00055     size += sizeof (mc_group_);
00056     size += sizeof (uint32_t) * missed_sequences_l_.size();
00057     return size;
00058 }
00059 
00060 bool Packet::frameAlreadyExsits(RoutedFrame f)
00061 {
00062     for (std::list<RoutedFrame>::iterator it = frames_l_.begin(); it != frames_l_.end(); ++it)
00063     {
00064         RoutedFrame & frame(*it);
00065 
00066         if (frame.header_.packet_sequence_num == f.header_.packet_sequence_num)
00067         {
00068             return true;
00069         }
00070     }
00071     return false;
00072 }
00073 
00074 void Packet::sortFrameList()
00075 {
00076     if (this->wrong_sequence_)
00077     {
00078         /* Simple sort algo */
00079         list<RoutedFrame> ordered_frames;
00080         for (uint32_t i = 0; i < this->size_; i++)
00081         {
00082             bool found = false;
00083             for (std::list<RoutedFrame>::iterator it = frames_l_.begin();
00084                     it != frames_l_.end(); ++it)
00085             {
00086                 RoutedFrame & frame(*it);
00087                 bool doppelt = false;
00088 
00089                 if (frame.header_.packet_sequence_num == i)
00090                 {
00091                     if (doppelt)
00092                         ROS_ERROR("doppelt: %u", i);
00093                     else
00094                     {
00095                         ordered_frames.push_back(frame);
00096                         found = true;
00097                         doppelt = true;
00098                     }
00099                 }
00100             }
00101             if (!found)
00102                 ROS_ERROR("missing frame!!! %u", i);
00103         }
00104         this->frames_l_ = ordered_frames;
00105     }
00106     else
00107     {
00108         /* Fast sort algo */
00109         RoutedFrame first = RoutedFrame(frames_l_.front());
00110 
00111         while (first.header_.packet_sequence_num != 0)
00112         {
00113             frames_l_.pop_front();
00114             for (std::list<RoutedFrame>::iterator it = frames_l_.begin();
00115                     it != frames_l_.end(); ++it)
00116             {
00117                 RoutedFrame & frame(*it);
00118                 if (frame.header_.packet_sequence_num + 1
00119                     == first.header_.packet_sequence_num)
00120                 {
00121                     frames_l_.insert(++it, first);
00122                     break;
00123                 }
00124             }
00125             first = RoutedFrame(frames_l_.front());
00126         }
00127     }
00128 }
00129 
00130 void Packet::refreshLists()
00131 {
00132     this->wrong_sequence_ = true;
00133     list<RoutedFrame> ordered_frames;
00134     /* Generates a new ordered frame list, in which every seq num is only included once*/
00135     for (uint32_t i = 0; i < this->size_; i++)
00136     {
00137         bool multiple_occurrences = false;
00138         for (std::list<RoutedFrame>::iterator it = frames_l_.begin();
00139                 it != frames_l_.end(); ++it)
00140         {
00141             RoutedFrame & frame(*it);
00142 
00143             if (frame.header_.packet_sequence_num == i)
00144             {
00145                 if (!multiple_occurrences)
00146                 {
00147                     ordered_frames.push_back(frame);
00148                     multiple_occurrences = true; //"" Occurrences
00149                 }
00150             }
00151         }
00152     }
00153     frames_l_ = ordered_frames;
00154 
00155     /* Generates a new missed sequences list */
00156     std::list<uint32_t> missed_sequences;
00157     for (uint32_t count = 0; count < this->size_; count++)
00158     {
00159 
00160         RoutedFrame f;
00161         f.header_.packet_sequence_num = count;
00162 
00163         if (!this->frameAlreadyExsits(f))
00164             missed_sequences.push_back(count);
00165 
00166     }
00167     if (missed_sequences.empty())
00168     {
00169         for (uint32_t count = 0; count < this->size_; count++)
00170         {
00171 
00172             RoutedFrame f;
00173             f.header_.packet_sequence_num = count;
00174 
00175             if (!this->frameAlreadyExsits(f))
00176                 missed_sequences.push_back(count);
00177         }
00178     }
00179     this->missed_sequences_l_ = missed_sequences;
00180 
00181     //ROS_ERROR("list refreshed %u %u %u", this->size_, missed_sequences.size(),
00182     //  frames_l_.size());
00183 }
00184 
00185 inline bool Packet::isMcFrame()
00186 {
00187     return mc_group_.compare("") != 0;
00188 }
00189 
00190 inline bool Packet::isNack()
00191 {
00192     return isMcFrame() && size_ >= Packet::NACK_THRESHOLD;
00193 }
00194 
00195 bool Packet::addFrame(RoutedFrame f)
00196 {
00197     /* Description:
00198      * Add the frame to the frame list of the packet, if the frame belongs to the packet.
00199      * This method is needed to build up the packet on the receiver side.
00200      * The method also checks, if frames are received in the right order.
00201      *
00202      *
00203      * Returns:
00204      *          (true) if packet is complete and ready to publish
00205      *
00206      *          (false) in every other case
00207      */
00208 
00209     this->ts_ = getMillisecondsTime();
00210     
00211     if(this->size_ == this->frames_l_.size())
00212         return true;
00213 
00214     if (this->highest_seq < f.header_.packet_sequence_num)
00215         this->highest_seq = f.header_.packet_sequence_num;
00216 
00217     if (this->isMcFrame() && this->frameAlreadyExsits(f) == true)
00218     {
00219         this->refreshLists();
00220         return false;
00221     }
00222 
00223     if (frames_l_.size() == 0 && f.header_.packet_sequence_num != 0)
00224     {
00225         for (uint32_t mis_seq = 0; mis_seq < f.header_.packet_sequence_num; mis_seq++)
00226             missed_sequences_l_.push_back(mis_seq);
00227     }
00228 
00229     if (f.header_.packet_id != this->id_)
00230     {
00231         ROS_ERROR("packet error");
00232         exit(22);
00233     }
00234 
00235     if ((unsigned) frames_l_.size() < size_)
00236     {
00237         bool missed_sequence_frame = false;
00238 
00239         /* check if frame is on with a missed sequence*/
00240         for (std::list<uint32_t>::iterator it = missed_sequences_l_.begin(); it != missed_sequences_l_.end(); ++it)
00241         {
00242             if (f.header_.packet_sequence_num == *it)
00243             {
00244                 //ROS_ERROR("Frame was in missed squ %u",f.header_.packet_sequence_num);
00245                 missed_sequence_frame = true;
00246                 missed_sequences_l_.erase(it); //todo try remove
00247                 break;
00248             }
00249         }
00250 
00251         if (!missed_sequence_frame)
00252         {
00253             //std::list<RoutedFrame>::reverse_iterator iter = frames_l_.rbegin(); //points to the last instered frame
00254 
00255             RoutedFrame & last_frame(*frames_l_.rbegin());
00256 
00257             /*Check if the new frame has the expected sequence*/
00258             if (last_frame.header_.packet_sequence_num == f.header_.packet_sequence_num - 1 || frames_l_.size() == 0)
00259             {
00260                 frames_l_.push_back(f);
00261             }
00262             else if (last_frame.header_.packet_sequence_num < f.header_.packet_sequence_num)
00263             {
00264 
00265                 /*Insert all sequence numbers between the current incoming frame and the last inserted one*/
00266                 for (uint32_t i = 1; i < f.header_.packet_sequence_num - last_frame.header_.packet_sequence_num; i++)
00267                 {
00268                     missed_sequences_l_.push_front(last_frame.header_.packet_sequence_num + i);
00269 
00270                 }
00271 
00272                 frames_l_.push_back(f);
00273             }
00274             else
00275             {
00276 
00277                 //      this->refreshLists();
00278                 //return this->addFrame(f);
00279             }
00280         }
00281             /*Missed sequence frame*/
00282         else
00283         {
00284             frames_l_.push_front(f);
00285         }
00286 
00287         if (frames_l_.size() == size_)
00288         {
00289             this->wrong_sequence_ = true;
00290             sortFrameList();
00291 
00292             return frames_l_.size() == size_;
00293         }
00294         else
00295             return false;
00296     }
00297 
00298     return false;
00299 }
00300 
00301 std::string Packet::getPayload()
00302 {
00303     /* Description:
00304      * Build the full payload string from the single frames
00305      *
00306      * Returns:
00307      *          "" if packet is incomplete
00308      *
00309      *          [Full Payload] if packet is complete
00310      */
00311 
00312     //  this->refreshLists();
00313     std::string payload = "";
00314     if (frames_l_.size() >= size_)
00315     {
00316 
00317         this->sortFrameList();
00318 
00319         /* build payload from the sorted list*/
00320         uint32_t expected_sequence = 0;
00321         for (std::list<RoutedFrame>::iterator it = frames_l_.begin();
00322                 it != frames_l_.end(); ++it)
00323         {
00324             RoutedFrame frame = *it;
00325 
00326             if (frame.header_.packet_sequence_num == expected_sequence++) //doublecheck if frame is in the right order
00327                 payload.append(frame.payload_);
00328             else
00329             {
00330                 ROS_ERROR("PACKET SEG FAULT! %u %u", frame.header_.packet_sequence_num ,expected_sequence );
00331                 return std::string("");
00332             }
00333         }
00334     }
00335 
00336     return payload;
00337 }
00338 


adhoc_communication
Author(s): Guenter Cwioro , Torsten Andre
autogenerated on Thu Jun 6 2019 20:59:43