Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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