joint_traj.cpp
Go to the documentation of this file.
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 #ifndef FLATHEADERS
00032 #include "simple_message/joint_traj.h"
00033 #include "simple_message/shared_types.h"
00034 #include "simple_message/log_wrapper.h"
00035 #else
00036 #include "joint_traj.h"
00037 #include "shared_types.h"
00038 #include "log_wrapper.h"
00039 #endif
00040 
00041 using namespace industrial::shared_types;
00042 using namespace industrial::joint_traj_pt;
00043 
00044 namespace industrial
00045 {
00046 namespace joint_traj
00047 {
00048 
00049 JointTraj::JointTraj(void)
00050 {
00051         this->init();
00052 }
00053 JointTraj::~JointTraj(void)
00054 {
00055 
00056 }
00057 
00058 void JointTraj::init()
00059 {
00060         JointTrajPt empty;
00061 
00062         this->size_ = 0;
00063         for (shared_int i = 0; i < this->getMaxNumPoints(); i++)
00064         {
00065                 this->points_[i].copyFrom(empty);
00066         }
00067 }
00068 
00069 bool JointTraj::addPoint(JointTrajPt & point)
00070 {
00071         bool rtn = false;
00072 
00073         if (!this->isFull())
00074         {
00075                 this->points_[this->size()].copyFrom(point);
00076                 this->size_++;
00077                 rtn = true;
00078         }
00079         else
00080         {
00081                 rtn = false;
00082                 LOG_ERROR("Failed to add point, buffer is full");
00083         }
00084 
00085         return rtn;
00086 }
00087 
00088 bool JointTraj::getPoint(shared_int index, JointTrajPt & point)
00089 {
00090         bool rtn = false;
00091 
00092         if (index < this->size())
00093         {
00094                 point.copyFrom(this->points_[index]);
00095                 rtn = true;
00096         }
00097         else
00098         {
00099                 LOG_ERROR("Point index: %d, is greater than size: %d", index, this->size());
00100                 rtn = false;
00101         }
00102         return rtn;
00103 }
00104 
00105 void JointTraj::copyFrom(JointTraj &src)
00106 {
00107         JointTrajPt value;
00108 
00109         this->size_ = src.size();
00110         for (shared_int i = 0; i < this->size(); i++)
00111         {
00112                 src.getPoint(i, value);
00113                 this->points_[i].copyFrom(value);
00114         }
00115 }
00116 
00117 bool JointTraj::operator==(JointTraj &rhs)
00118 {
00119         bool rtn = true;
00120 
00121         if(this->size() == rhs.size())
00122         {
00123                 for(shared_int i = 0; i < this->size(); i++)
00124                 {
00125                         JointTrajPt value;
00126                         rhs.getPoint(i, value);
00127                         if(!(this->points_[i] == value))
00128                         {
00129                                 LOG_DEBUG("Joint trajectory point different");
00130                                 rtn = false;
00131                                 break;
00132                         }
00133                         else
00134                         {
00135                                 rtn = true;
00136                         }
00137                 }
00138         }
00139         else
00140         {
00141                 LOG_DEBUG("Joint trajectory compare failed, size mismatch");
00142                 rtn = false;
00143         }
00144 
00145         return rtn;
00146 }
00147 
00148 
00149 bool JointTraj::load(industrial::byte_array::ByteArray *buffer)
00150 {
00151         bool rtn = false;
00152         JointTrajPt value;
00153 
00154         LOG_COMM("Executing joint trajectory load");
00155         for (shared_int i = 0; i < this->size(); i++)
00156         {
00157                 this->getPoint(i, value);
00158                 rtn = buffer->load(value);
00159                 if (!rtn)
00160                 {
00161                         LOG_ERROR("Failed to load joint traj.pt. data");
00162                         rtn = false;
00163                         break;
00164                 }
00165                 rtn = true;
00166         }
00167 
00168         if (rtn)
00169         {
00170                 rtn = buffer->load(this->size());
00171         }
00172         return rtn;
00173 }
00174 
00175 bool JointTraj::unload(industrial::byte_array::ByteArray *buffer)
00176 {
00177         bool rtn = false;
00178         JointTrajPt value;
00179 
00180         LOG_COMM("Executing joint trajectory unload");
00181 
00182         rtn = buffer->unload(this->size_);
00183 
00184         if(rtn)
00185         {
00186                 for (int i = this->size() - 1; i >= 0; i--)
00187                 {
00188                         rtn = buffer->unload(value);
00189                         if (!rtn)
00190                         {
00191                                 LOG_ERROR("Failed to unload message point: %d from data[%d]", i, buffer->getBufferSize());
00192                                 break;
00193                         }
00194                         this->points_[i].copyFrom(value);
00195                 }
00196         }
00197         else
00198         {
00199                 LOG_ERROR("Failed to unload trajectory size");
00200         }
00201         return rtn;
00202 }
00203 
00204 }
00205 }
00206 


simple_message
Author(s): Shaun Edwards
autogenerated on Sat Jun 8 2019 20:43:23