joint_traj_pt_full.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, 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_pt_full.h"
37 #include "shared_types.h"
38 #include "log_wrapper.h"
39 #endif
40 
41 using namespace industrial::joint_data;
42 using namespace industrial::shared_types;
43 
44 namespace industrial
45 {
46 namespace joint_traj_pt_full
47 {
48 
49 JointTrajPtFull::JointTrajPtFull(void)
50 {
51  this->init();
52 }
53 JointTrajPtFull::~JointTrajPtFull(void)
54 {
55 
56 }
57 
58 void JointTrajPtFull::init()
59 {
60  this->robot_id_ = 0;
61  this->sequence_ = 0;
62  this->valid_fields_ = 0;
63  this->time_ = 0.0;
64  this->positions_.init();
65  this->velocities_.init();
66  this->accelerations_.init();
67 }
68 
69 void JointTrajPtFull::init(industrial::shared_types::shared_int robot_id,
75  industrial::joint_data::JointData & accelerations)
76 {
77  this->setRobotID(robot_id);
78  this->setSequence(sequence);
79  this->setTime(time);
80  this->setPositions(positions);
81  this->setVelocities(velocities);
82  this->setAccelerations(accelerations);
83  this->valid_fields_ = valid_fields; // must happen after others are set
84 }
85 
86 void JointTrajPtFull::copyFrom(JointTrajPtFull &src)
87 {
88  this->setRobotID(src.getRobotID());
89  this->setSequence(src.getSequence());
90  src.getTime(this->time_);
91  src.getPositions(this->positions_);
92  src.getVelocities(this->velocities_);
93  src.getAccelerations(this->accelerations_);
94  this->valid_fields_ = src.valid_fields_;
95 }
96 
97 bool JointTrajPtFull::operator==(JointTrajPtFull &rhs)
98 {
99  return this->robot_id_ == rhs.robot_id_ &&
100  this->sequence_ == rhs.sequence_ &&
101  this->valid_fields_ == rhs.valid_fields_ &&
102  ( !is_valid(ValidFieldTypes::TIME) || (this->time_ == rhs.time_) ) &&
103  ( !is_valid(ValidFieldTypes::POSITION) || (this->positions_ == rhs.positions_) ) &&
104  ( !is_valid(ValidFieldTypes::VELOCITY) || (this->velocities_ == rhs.velocities_) ) &&
105  ( !is_valid(ValidFieldTypes::ACCELERATION) || (this->accelerations_ == rhs.accelerations_) );
106 }
107 
108 bool JointTrajPtFull::load(industrial::byte_array::ByteArray *buffer)
109 {
110  LOG_COMM("Executing joint trajectory point load");
111 
112  if (!buffer->load(this->robot_id_))
113  {
114  LOG_ERROR("Failed to load joint traj pt. robot_id");
115  return false;
116  }
117 
118  if (!buffer->load(this->sequence_))
119  {
120  LOG_ERROR("Failed to load joint traj. pt. sequence number");
121  return false;
122  }
123 
124  if (!buffer->load(this->valid_fields_))
125  {
126  LOG_ERROR("Failed to load joint traj. pt. valid fields");
127  return false;
128  }
129 
130  if (!buffer->load(this->time_))
131  {
132  LOG_ERROR("Failed to load joint traj. pt. time");
133  return false;
134  }
135 
136  if (!this->positions_.load(buffer))
137  {
138  LOG_ERROR("Failed to load joint traj. pt. positions");
139  return false;
140  }
141 
142  if (!this->velocities_.load(buffer))
143  {
144  LOG_ERROR("Failed to load joint traj. pt. velocities");
145  return false;
146  }
147 
148  if (!this->accelerations_.load(buffer))
149  {
150  LOG_ERROR("Failed to load joint traj. pt. accelerations");
151  return false;
152  }
153 
154  LOG_COMM("Trajectory point successfully loaded");
155  return true;
156 }
157 
158 bool JointTrajPtFull::unload(industrial::byte_array::ByteArray *buffer)
159 {
160  LOG_COMM("Executing joint traj. pt. unload");
161 
162  if (!this->accelerations_.unload(buffer))
163  {
164  LOG_ERROR("Failed to unload joint traj. pt. accelerations");
165  return false;
166  }
167 
168  if (!this->velocities_.unload(buffer))
169  {
170  LOG_ERROR("Failed to unload joint traj. pt. velocities");
171  return false;
172  }
173 
174  if (!this->positions_.unload(buffer))
175  {
176  LOG_ERROR("Failed to unload joint traj. pt. positions");
177  return false;
178  }
179 
180  if (!buffer->unload(this->time_))
181  {
182  LOG_ERROR("Failed to unload joint traj. pt. time");
183  return false;
184  }
185 
186  if (!buffer->unload(this->valid_fields_))
187  {
188  LOG_ERROR("Failed to unload joint traj. pt. valid fields");
189  return false;
190  }
191 
192  if (!buffer->unload(this->sequence_))
193  {
194  LOG_ERROR("Failed to unload joint traj. pt. sequence number");
195  return false;
196  }
197 
198  if (!buffer->unload(this->robot_id_))
199  {
200  LOG_ERROR("Faild to unload joint traj. pt. robot_id");
201  return false;
202  }
203 
204  LOG_COMM("Joint traj. pt successfully unloaded");
205  return true;
206 }
207 
208 }
209 }
210 
industrial::shared_types::shared_int robot_id_
robot group # (0-based) for controllers that support multiple axis-groups
bool getTime(industrial::shared_types::shared_real &time)
Returns joint trajectory point timestamp.
void init(const M_string &remappings)
Contains platform specific type definitions that guarantee the size of primitive data types...
Definition: shared_types.h:52
industrial::joint_data::JointData accelerations_
joint trajectory point acceleration data
industrial::shared_types::shared_int valid_fields_
bit-mask of (optional) fields that have been initialized with valid data
#define LOG_COMM(format,...)
Definition: log_wrapper.h:104
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:108
industrial::shared_types::shared_int getRobotID()
Gets robot_id. Robot group # (0-based), for controllers with multiple axis-groups.
bool getPositions(industrial::joint_data::JointData &dest)
Returns a copy of the position data.
industrial::joint_data::JointData velocities_
joint trajectory point velocity data
The byte array wraps a dynamic array of bytes (i.e. char).
Definition: byte_array.h:80
Class encapsulated joint data (positions, accelerations, velocity, torque, and/or effort)...
Definition: joint_data.h:68
Class encapsulated joint trajectory point data. The point data serves as a waypoint along a trajector...
bool getAccelerations(industrial::joint_data::JointData &dest)
Returns a copy of the acceleration data.
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 sequence_
trajectory sequence number
industrial::shared_types::shared_int getSequence()
Returns joint trajectory point sequence number.
industrial::shared_types::shared_real time_
joint trajectory point timestamp Typically, time_from_start of this trajectory (in seconds) ...
industrial::joint_data::JointData positions_
joint trajectory point positional data
bool getVelocities(industrial::joint_data::JointData &dest)
Returns a copy of the velocity data.


simple_message
Author(s): Shaun Edwards
autogenerated on Sat Sep 21 2019 03:30:09