joint_traj.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_traj.h"
37 #include "shared_types.h"
38 #include "log_wrapper.h"
39 #endif
40 
41 using namespace industrial::shared_types;
42 using namespace industrial::joint_traj_pt;
43 
44 namespace industrial
45 {
46 namespace joint_traj
47 {
48 
49 JointTraj::JointTraj(void)
50 {
51  this->init();
52 }
53 JointTraj::~JointTraj(void)
54 {
55 
56 }
57 
58 void JointTraj::init()
59 {
60  JointTrajPt empty;
61 
62  this->size_ = 0;
63  for (shared_int i = 0; i < this->getMaxNumPoints(); i++)
64  {
65  this->points_[i].copyFrom(empty);
66  }
67 }
68 
69 bool JointTraj::addPoint(JointTrajPt & point)
70 {
71  bool rtn = false;
72 
73  if (!this->isFull())
74  {
75  this->points_[this->size()].copyFrom(point);
76  this->size_++;
77  rtn = true;
78  }
79  else
80  {
81  rtn = false;
82  LOG_ERROR("Failed to add point, buffer is full");
83  }
84 
85  return rtn;
86 }
87 
88 bool JointTraj::getPoint(shared_int index, JointTrajPt & point)
89 {
90  bool rtn = false;
91 
92  if (index < this->size())
93  {
94  point.copyFrom(this->points_[index]);
95  rtn = true;
96  }
97  else
98  {
99  LOG_ERROR("Point index: %d, is greater than size: %d", index, this->size());
100  rtn = false;
101  }
102  return rtn;
103 }
104 
105 void JointTraj::copyFrom(JointTraj &src)
106 {
107  JointTrajPt value;
108 
109  this->size_ = src.size();
110  for (shared_int i = 0; i < this->size(); i++)
111  {
112  src.getPoint(i, value);
113  this->points_[i].copyFrom(value);
114  }
115 }
116 
117 bool JointTraj::operator==(JointTraj &rhs)
118 {
119  bool rtn = true;
120 
121  if(this->size() == rhs.size())
122  {
123  for(shared_int i = 0; i < this->size(); i++)
124  {
125  JointTrajPt value;
126  rhs.getPoint(i, value);
127  if(!(this->points_[i] == value))
128  {
129  LOG_DEBUG("Joint trajectory point different");
130  rtn = false;
131  break;
132  }
133  else
134  {
135  rtn = true;
136  }
137  }
138  }
139  else
140  {
141  LOG_DEBUG("Joint trajectory compare failed, size mismatch");
142  rtn = false;
143  }
144 
145  return rtn;
146 }
147 
148 
149 bool JointTraj::load(industrial::byte_array::ByteArray *buffer)
150 {
151  bool rtn = false;
152  JointTrajPt value;
153 
154  LOG_COMM("Executing joint trajectory load");
155  for (shared_int i = 0; i < this->size(); i++)
156  {
157  this->getPoint(i, value);
158  rtn = buffer->load(value);
159  if (!rtn)
160  {
161  LOG_ERROR("Failed to load joint traj.pt. data");
162  rtn = false;
163  break;
164  }
165  rtn = true;
166  }
167 
168  if (rtn)
169  {
170  rtn = buffer->load(this->size());
171  }
172  return rtn;
173 }
174 
175 bool JointTraj::unload(industrial::byte_array::ByteArray *buffer)
176 {
177  bool rtn = false;
178  JointTrajPt value;
179 
180  LOG_COMM("Executing joint trajectory unload");
181 
182  rtn = buffer->unload(this->size_);
183 
184  if(rtn)
185  {
186  for (int i = this->size() - 1; i >= 0; i--)
187  {
188  rtn = buffer->unload(value);
189  if (!rtn)
190  {
191  LOG_ERROR("Failed to unload message point: %d from data[%d]", i, buffer->getBufferSize());
192  break;
193  }
194  this->points_[i].copyFrom(value);
195  }
196  }
197  else
198  {
199  LOG_ERROR("Failed to unload trajectory size");
200  }
201  return rtn;
202 }
203 
204 }
205 }
206 
void init(const M_string &remappings)
Contains platform specific type definitions that guarantee the size of primitive data types...
Definition: shared_types.h:52
bool getPoint(industrial::shared_types::shared_int index, industrial::joint_traj_pt::JointTrajPt &point)
Gets a point value within the buffer.
Definition: joint_traj.cpp:88
#define LOG_COMM(format,...)
Definition: log_wrapper.h:130
bool load(industrial::shared_types::shared_bool value)
loads a boolean into the byte array
Definition: byte_array.cpp:142
#define LOG_ERROR(format,...)
Definition: log_wrapper.h:134
Class encapsulated joint trajectory point data. The point data serves as a waypoint along a trajector...
Definition: joint_traj_pt.h:92
The byte array wraps a dynamic array of bytes (i.e. char).
Definition: byte_array.h:80
void copyFrom(JointTrajPt &src)
Copies the passed in value.
Class encapsulated joint trajectory. A joint trajectory includes an array of JointTrajPt data types...
Definition: joint_traj.h:70
#define LOG_DEBUG(format,...)
Definition: log_wrapper.h:131
unsigned int getBufferSize()
gets current buffer size
Definition: byte_array.cpp:387
bool unload(industrial::shared_types::shared_bool &value)
unloads a boolean value from the byte array
Definition: byte_array.cpp:233
industrial::shared_types::shared_int size()
Gets a size of trajectory.
Definition: joint_traj.h:116


simple_message
Author(s): Shaun Edwards
autogenerated on Mon Feb 28 2022 22:34:35