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


simple_message
Author(s): Shaun Edwards
autogenerated on Fri Jan 3 2014 11:26:56