simple_message: simple_message: byte

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2011, Southwest Research Institute
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *       * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *       * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *       * Neither the name of the Southwest Research Institute, nor the names
00016  *       of its contributors may be used to endorse or promote products derived
00017  *       from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 #ifdef ROS
00032 #include "simple_message/byte_array.h"
00033 #include "simple_message/simple_serialize.h"
00034 #include "simple_message/log_wrapper.h"
00035 #endif
00036 
00037 #ifdef MOTOPLUS
00038 #include "motoPlus.h"
00039 #include "byte_array.h"
00040 #include "simple_serialize.h"
00041 #include "log_wrapper.h"
00042 #endif
00043 
00044 #include "string.h"
00045 
00046 namespace industrial
00047 {
00048 namespace byte_array
00049 {
00050 
00051 using namespace industrial::simple_serialize;
00052 using namespace industrial::shared_types;
00053 using namespace industrial::byte_array;
00054 
00055 ByteArray::ByteArray(void)
00056 {
00057   this->init();
00058 }
00059 
00060 ByteArray::~ByteArray(void)
00061 {
00062 }
00063 
00064 void ByteArray::init()
00065 {
00066   memset(&(buffer_[0]), 0, this->MAX_SIZE);
00067   this->setBufferSize(0);
00068 }
00069 
00070 bool ByteArray::init(const char* buffer, const shared_int byte_size)
00071 {
00072   bool rtn;
00073 
00074   if (this->MAX_SIZE >= byte_size)
00075   {
00076     LOG_COMM("Initializing buffer to size: %d", byte_size);
00077     this->load((void*)buffer, byte_size);
00078     rtn = true;
00079   }
00080   else
00081   {
00082     LOG_ERROR("Failed to initialize byte array, buffer size: %u greater than max: %u",
00083               byte_size, this->getMaxBufferSize());
00084     rtn = false;
00085   }
00086   return rtn;
00087 }
00088 
00089 void ByteArray::copyFrom(ByteArray & buffer)
00090 {
00091   if (buffer.getBufferSize() != 0)
00092   {
00093     this->setBufferSize(buffer.getBufferSize());
00094     memcpy(this->getRawDataPtr(), buffer.getRawDataPtr(), this->buffer_size_);
00095   }
00096   else
00097   {
00098     LOG_WARN("Byte array copy not performed, buffer to copy is empty");
00099   }
00100 }
00101 
00102 char* ByteArray::getRawDataPtr()
00103 {
00104   return &this->buffer_[0];
00105 }
00106 
00107 /****************************************************************
00108  // load(*)
00109  //
00110  // Methods for loading various data types.
00111  //
00112  */
00113 bool ByteArray::load(shared_bool value)
00114 {
00115   return this->load(&value, sizeof(shared_bool));
00116 }
00117 
00118 bool ByteArray::load(shared_real value)
00119 {
00120   return this->load(&value, sizeof(shared_real));
00121 }
00122 
00123 bool ByteArray::load(shared_int value)
00124 {
00125   return this->load(&value, sizeof(shared_int));
00126 }
00127 
00128 bool ByteArray::load(simple_serialize::SimpleSerialize &value)
00129 {
00130   LOG_COMM("Executing byte array load through simple serialize");
00131   return value.load(this);
00132 }
00133 
00134 bool ByteArray::load(ByteArray &value)
00135 {
00136   LOG_COMM("Executing byte array load through byte array");
00137   return this->load(value.getRawDataPtr(), value.getBufferSize());
00138 }
00139 
00140 bool ByteArray::load(void* value, const shared_int byte_size)
00141 {
00142 
00143   bool rtn;
00144   // Get the load pointer before extending the buffer.
00145   char* loadPtr;
00146 
00147   LOG_COMM("Executing byte array load through void*, size: %d", byte_size);
00148   // Check inputs
00149   if (NULL == value)
00150   {
00151     LOG_ERROR("NULL point passed into load method");
00152     return false;
00153   }
00154 
00155   loadPtr = this->getLoadPtr();
00156 
00157   if (this->extendBufferSize(byte_size))
00158   {
00159     memcpy(loadPtr, value, byte_size);
00160     rtn = true;
00161   }
00162   else
00163   {
00164     LOG_ERROR("Failed to load byte array");
00165     rtn = false;
00166   }
00167 
00168   return rtn;
00169 }
00170 
00171 /****************************************************************
00172  // unload(*)
00173  //
00174  // Methods for unloading various data types.  Unloading data shortens
00175  // the internal buffer.  The resulting memory that holds the data is
00176  // lost.
00177  //
00178  */
00179 bool ByteArray::unload(shared_bool & value)
00180 {
00181   return this->unload(&value, sizeof(shared_bool));
00182 }
00183 
00184 bool ByteArray::unload(shared_real &value)
00185 {
00186   return this->unload(&value, sizeof(shared_real));
00187 }
00188 
00189 bool ByteArray::unload(shared_int &value)
00190 {
00191   return this->unload(&value, sizeof(shared_int));
00192 }
00193 
00194 bool ByteArray::unload(simple_serialize::SimpleSerialize &value)
00195 {
00196   LOG_COMM("Executing byte array unload through simple serialize");
00197   return value.unload(this);
00198 }
00199 
00200 bool ByteArray::unload(ByteArray &value, const shared_int byte_size)
00201 {
00202   LOG_COMM("Executing byte array unload through byte array");
00203   char* unloadPtr = this->getUnloadPtr(byte_size);
00204   bool rtn;
00205 
00206   if (NULL != unloadPtr)
00207   {
00208     if (this->shortenBufferSize(byte_size))
00209     {
00210 
00211       rtn = value.load(unloadPtr, byte_size);
00212       rtn = true;
00213     }
00214     else
00215     {
00216       LOG_ERROR("Failed to shorten array");
00217       rtn = false;
00218     }
00219   }
00220   else
00221   {
00222     LOG_ERROR("Unload pointer returned NULL");
00223     rtn = false;
00224   }
00225 
00226   return rtn;
00227 }
00228 
00229 bool ByteArray::unload(void* value, shared_int byteSize)
00230 {
00231   bool rtn;
00232   char* unloadPtr;
00233 
00234   LOG_COMM("Executing byte array unload through void*, size: %d", byteSize);
00235   // Check inputs
00236   if (NULL == value)
00237   {
00238     LOG_ERROR("NULL point passed into unload method");
00239     return false;
00240   }
00241 
00242   unloadPtr = this->getUnloadPtr(byteSize);
00243 
00244   if (NULL != unloadPtr)
00245   {
00246 
00247     if (this->shortenBufferSize(byteSize))
00248     {
00249       memcpy(value, unloadPtr, byteSize);
00250       rtn = true;
00251     }
00252     else
00253     {
00254       LOG_ERROR("Failed to shorten array");
00255       rtn = false;
00256     }
00257   }
00258   else
00259   {
00260     LOG_ERROR("Unload pointer returned NULL");
00261     rtn = false;
00262   }
00263 
00264   return rtn;
00265 }
00266 
00267 bool ByteArray::unloadFront(void* value, const industrial::shared_types::shared_int byteSize)
00268 {
00269   bool rtn;
00270   char* unloadPtr = NULL;
00271   char* nextPtr = NULL;
00272   shared_int sizeRemain;
00273 
00274   // Check inputs
00275   if (NULL == value)
00276   {
00277     LOG_ERROR("NULL point passed into unload method");
00278     return false;
00279   }
00280 
00281   unloadPtr = &this->buffer_[0];
00282 
00283   if (NULL != unloadPtr)
00284   {
00285     nextPtr = unloadPtr + byteSize;
00286     sizeRemain = this->getBufferSize() - byteSize;
00287 
00288     LOG_DEBUG("Unloading: %d bytes, %d bytes remain", byteSize, sizeRemain);
00289     if (this->shortenBufferSize(byteSize))
00290     {
00291       LOG_COMM("Preparing to copy value");
00292       memcpy(value, unloadPtr, byteSize);
00293       LOG_COMM("Value is unloaded, performing move");
00294       memmove(unloadPtr, nextPtr, sizeRemain);
00295       LOG_COMM("Move operation completed");
00296       rtn = true;
00297     }
00298     else
00299     {
00300       LOG_ERROR("Failed to shorten array");
00301       rtn = false;
00302     }
00303   }
00304   else
00305   {
00306     LOG_ERROR("Unload pointer returned NULL");
00307     rtn = false;
00308   }
00309 
00310   return rtn;
00311 }
00312 
00313 unsigned int ByteArray::getBufferSize()
00314 {
00315   return this->buffer_size_;
00316 }
00317 
00318 unsigned int ByteArray::getMaxBufferSize()
00319 {
00320   return this->MAX_SIZE;
00321 }
00322 
00323 bool ByteArray::setBufferSize(shared_int size)
00324 {
00325   bool rtn;
00326 
00327   if (this->MAX_SIZE >= size)
00328   {
00329     this->buffer_size_ = size;
00330     rtn = true;
00331   }
00332   else
00333   {
00334     LOG_ERROR("Set buffer size: %u, larger than MAX:, %u", size, this->MAX_SIZE);
00335     rtn = false;
00336   }
00337 
00338   return rtn;
00339 
00340 }
00341 
00342 bool ByteArray::extendBufferSize(shared_int size)
00343 {
00344   unsigned int newSize;
00345 
00346   newSize = this->getBufferSize() + size;
00347   return this->setBufferSize(newSize);
00348 
00349 }
00350 
00351 bool ByteArray::shortenBufferSize(shared_int size)
00352 {
00353   unsigned int newSize;
00354   bool rtn;
00355 
00356   // If the buffer is not larger than the size it is shortened by
00357   // we fail.  This is checked here (as opposed to setBufferSize)
00358   // because setBufferSize assumes a unsigned argument and therefore
00359   // wouldn't catch a negative size.
00360   if (size <= (shared_int)this->getBufferSize())
00361   {
00362     newSize = this->getBufferSize() - size;
00363     rtn = this->setBufferSize(newSize);
00364   }
00365   else
00366   {
00367     LOG_ERROR("Failed to shorten buffer by %u bytes, buffer too small, %u bytes", size, this->getBufferSize());
00368     rtn = false;
00369   }
00370 
00371   return rtn;
00372 
00373 }
00374 
00375 char* ByteArray::getLoadPtr()
00376 {
00377 
00378   return &this->buffer_[this->buffer_size_];
00379 }
00380 
00381 char* ByteArray::getUnloadPtr(shared_int byteSize)
00382 {
00383   char* rtn;
00384 
00385   if (byteSize <= (shared_int)this->getBufferSize())
00386   {
00387     rtn = this->getLoadPtr() - byteSize;
00388   }
00389   else
00390   {
00391     LOG_ERROR("Get unload pointer failed, buffer size: %d, smaller than byte size: %d",
00392       this->getBufferSize(), byteSize);
00393     rtn = NULL;
00394   }
00395 
00396   return rtn;
00397 }
00398 
00399 } // namespace byte_array
00400 } // namespace industrial
byte_array.cpp
