joint_data.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 #ifndef FLATHEADERS
35 #else
36 #include "joint_data.h"
37 #include "shared_types.h"
38 #include "log_wrapper.h"
39 #endif
40 
41 using namespace industrial::shared_types;
42 
43 namespace industrial
44 {
45 namespace joint_data
46 {
47 
48 JointData::JointData(void)
49 {
50  this->init();
51 }
52 JointData::~JointData(void)
53 {
54 
55 }
56 
57 void JointData::init()
58 {
59  for (int i = 0; i < this->getMaxNumJoints(); i++)
60  {
61  this->setJoint(i, 0.0);
62  }
63 }
64 
65 bool JointData::setJoint(shared_int index, shared_real value)
66 {
67  bool rtn = false;
68 
69  if (index < this->getMaxNumJoints())
70  {
71  this->joints_[index] = value;
72  rtn = true;
73  }
74  else
75  {
76  LOG_ERROR("Joint index: %d, is greater than size: %d", index, this->getMaxNumJoints());
77  rtn = false;
78  }
79  return rtn;
80 }
81 
82 bool JointData::getJoint(shared_int index, shared_real & value) const
83 {
84  bool rtn = false;
85 
86  if (index < this->getMaxNumJoints())
87  {
88  value = this->joints_[index];
89  rtn = true;
90  }
91  else
92  {
93  LOG_ERROR("Joint index: %d, is greater than size: %d", index, this->getMaxNumJoints());
94  rtn = false;
95  }
96  return rtn;
97 }
98 
99 shared_real JointData::getJoint(shared_int index) const
100 {
101  shared_real rtn = 0.0;
102  this->getJoint(index, rtn);
103  return rtn;
104 }
105 
106 
107 void JointData::copyFrom(JointData &src)
108 {
109  shared_real value = 0.0;
110 
111  for (int i = 0; i < this->getMaxNumJoints(); i++)
112  {
113  src.getJoint(i, value);
114  this->setJoint(i, value);
115  }
116 }
117 
118 bool JointData::operator==(JointData &rhs)
119 {
120  bool rtn = true;
121 
122  shared_real lhsvalue, rhsvalue;
123 
124  for (int i = 0; i < this->getMaxNumJoints(); i++)
125  {
126  this->getJoint(i, lhsvalue);
127  rhs.getJoint(i, rhsvalue);
128  if (lhsvalue != rhsvalue)
129  {
130  rtn = false;
131  break;
132  }
133  }
134  return rtn;
135 
136 }
137 
138 bool JointData::load(industrial::byte_array::ByteArray *buffer)
139 {
140  bool rtn = false;
141  shared_real value = 0.0;
142 
143  LOG_COMM("Executing joint position load");
144  for (int i = 0; i < this->getMaxNumJoints(); i++)
145  {
146  this->getJoint(i, value);
147  rtn = buffer->load(value);
148  if (!rtn)
149  {
150  LOG_ERROR("Failed to load joint position data");
151  break;
152  }
153  }
154  return rtn;
155 }
156 
157 bool JointData::unload(industrial::byte_array::ByteArray *buffer)
158 {
159  bool rtn = false;
160  shared_real value = 0.0;
161 
162  LOG_COMM("Executing joint position unload");
163  for (int i = this->getMaxNumJoints() - 1; i >= 0; i--)
164  {
165  rtn = buffer->unload(value);
166  if (!rtn)
167  {
168  LOG_ERROR("Failed to unload message joint: %d from data[%d]", i, buffer->getBufferSize());
169  break;
170  }
171  this->setJoint(i, value);
172  }
173  return rtn;
174 }
175 
176 }
177 }
178 
industrial::joint_data::JointData::getJoint
bool getJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real &value) const
Gets a joint value within the buffer.
Definition: joint_data.cpp:82
industrial::joint_data::JointData
Class encapsulated joint data (positions, accelerations, velocity, torque, and/or effort).
Definition: joint_data.h:68
industrial::shared_types
Contains platform specific type definitions that guarantee the size of primitive data types.
Definition: shared_types.h:52
industrial
Definition: byte_array.h:45
industrial::byte_array::ByteArray::unload
bool unload(industrial::shared_types::shared_bool &value)
unloads a boolean value from the byte array
Definition: byte_array.cpp:233
LOG_ERROR
#define LOG_ERROR(format,...)
Definition: log_wrapper.h:134
industrial::shared_types::shared_real
float shared_real
Definition: shared_types.h:66
industrial::byte_array::ByteArray
The byte array wraps a dynamic array of bytes (i.e. char).
Definition: byte_array.h:80
LOG_COMM
#define LOG_COMM(format,...)
Definition: log_wrapper.h:130
shared_types.h
log_wrapper.h
industrial::shared_types::shared_int
int shared_int
Definition: shared_types.h:62
industrial::byte_array::ByteArray::load
bool load(industrial::shared_types::shared_bool value)
loads a boolean into the byte array
Definition: byte_array.cpp:142
joint_data.h
init
void init(const M_string &remappings)
industrial::byte_array::ByteArray::getBufferSize
unsigned int getBufferSize()
gets current buffer size
Definition: byte_array.cpp:387


simple_message
Author(s): Shaun Edwards
autogenerated on Wed Mar 2 2022 00:24:53