joint_data.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_data.h"
00033 #include "simple_message/shared_types.h"
00034 #include "simple_message/log_wrapper.h"
00035 #endif
00036 
00037 #ifdef MOTOPLUS
00038 #include "joint_data.h"
00039 #include "shared_types.h"
00040 #include "log_wrapper.h"
00041 #endif
00042 
00043 using namespace industrial::shared_types;
00044 
00045 namespace industrial
00046 {
00047 namespace joint_data
00048 {
00049 
00050 JointData::JointData(void)
00051 {
00052   this->init();
00053 }
00054 JointData::~JointData(void)
00055 {
00056 
00057 }
00058 
00059 void JointData::init()
00060 {
00061   for (int i = 0; i < this->getMaxNumJoints(); i++)
00062   {
00063     this->setJoint(i, 0.0);
00064   }
00065 }
00066 
00067 bool JointData::setJoint(shared_int index, shared_real value)
00068 {
00069   bool rtn = false;
00070 
00071   if (index < this->getMaxNumJoints())
00072   {
00073     this->joints_[index] = value;
00074     rtn = true;
00075   }
00076   else
00077   {
00078     LOG_ERROR("Joint index: %d, is greater than size: %d", index, this->getMaxNumJoints());
00079     rtn = false;
00080   }
00081   return rtn;
00082 }
00083 
00084 bool JointData::getJoint(shared_int index, shared_real & value)
00085 {
00086   bool rtn = false;
00087 
00088   if (index < this->getMaxNumJoints())
00089   {
00090     value = this->joints_[index];
00091     rtn = true;
00092   }
00093   else
00094   {
00095     LOG_ERROR("Joint index: %d, is greater than size: %d", index, this->getMaxNumJoints());
00096     rtn = false;
00097   }
00098   return rtn;
00099 }
00100 
00101 shared_real JointData::getJoint(shared_int index)
00102 {
00103   shared_real rtn = 0.0;
00104   this->getJoint(index, rtn);
00105   return rtn;
00106 }
00107   
00108 
00109 void JointData::copyFrom(JointData &src)
00110 {
00111   shared_real value = 0.0;
00112 
00113   for (int i = 0; i < this->getMaxNumJoints(); i++)
00114   {
00115     src.getJoint(i, value);
00116     this->setJoint(i, value);
00117   }
00118 }
00119 
00120 bool JointData::operator==(JointData &rhs)
00121 {
00122   bool rtn = true;
00123 
00124   shared_real lhsvalue, rhsvalue;
00125 
00126   for (int i = 0; i < this->getMaxNumJoints(); i++)
00127   {
00128     this->getJoint(i, lhsvalue);
00129     rhs.getJoint(i, rhsvalue);
00130     if (lhsvalue != rhsvalue)
00131     {
00132       rtn = false;
00133       break;
00134     }
00135   }
00136   return rtn;
00137 
00138 }
00139 
00140 bool JointData::load(industrial::byte_array::ByteArray *buffer)
00141 {
00142   bool rtn = false;
00143   shared_real value = 0.0;
00144 
00145   LOG_COMM("Executing joint position load");
00146   for (int i = 0; i < this->getMaxNumJoints(); i++)
00147   {
00148     this->getJoint(i, value);
00149     rtn = buffer->load(value);
00150     if (!rtn)
00151     {
00152       LOG_ERROR("Failed to load joint position data");
00153       break;
00154     }
00155   }
00156   return rtn;
00157 }
00158 
00159 bool JointData::unload(industrial::byte_array::ByteArray *buffer)
00160 {
00161   bool rtn = false;
00162   shared_real value = 0.0;
00163 
00164   LOG_COMM("Executing joint position unload");
00165   for (int i = this->getMaxNumJoints() - 1; i >= 0; i--)
00166   {
00167     rtn = buffer->unload(value);
00168     if (!rtn)
00169     {
00170       LOG_ERROR("Failed to unload message joint: %d from data[%d]", i, buffer->getBufferSize());
00171       break;
00172     }
00173     this->setJoint(i, value);
00174   }
00175   return rtn;
00176 }
00177 
00178 }
00179 }
00180 


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