joint_traj_pt.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_pt.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
47 {
48 
49 JointTrajPt::JointTrajPt(void)
50 {
51  this->init();
52 }
53 JointTrajPt::~JointTrajPt(void)
54 {
55 
56 }
57 
58 void JointTrajPt::init()
59 {
60  this->joint_position_.init();
61  this->sequence_ = 0;
62  this->velocity_ = 0.0;
63  this->duration_ = 0.0;
64 }
65 
66 void JointTrajPt::init(shared_int sequence, JointData & position, shared_real velocity, shared_real duration)
67 {
68  this->setJointPosition(position);
69  this->setSequence(sequence);
70  this->setVelocity(velocity);
71  this->setDuration(duration);
72 }
73 
74 void JointTrajPt::copyFrom(JointTrajPt &src)
75 {
76  this->setSequence(src.getSequence());
77  src.getJointPosition(this->joint_position_);
78  this->setVelocity(src.getVelocity());
79  this->setDuration(src.getDuration());
80 }
81 
82 bool JointTrajPt::operator==(JointTrajPt &rhs)
83 {
84  return this->joint_position_ == rhs.joint_position_ && this->sequence_ == rhs.sequence_
85  && this->velocity_ == rhs.velocity_ && this->duration_ == rhs.duration_;
86 
87 }
88 
89 bool JointTrajPt::load(industrial::byte_array::ByteArray *buffer)
90 {
91  bool rtn = false;
92 
93  LOG_COMM("Executing joint trajectory point load");
94 
95  if (buffer->load(this->sequence_))
96  {
97  if (this->joint_position_.load(buffer))
98  {
99  if (buffer->load(this->velocity_))
100  {
101  if (buffer->load(this->duration_))
102  {
103  LOG_COMM("Trajectory point successfully loaded");
104  rtn = true;
105  }
106  else
107  {
108  rtn = false;
109  LOG_ERROR("Failed to load joint traj pt. duration");
110  }
111  }
112  else
113  {
114  rtn = false;
115  LOG_ERROR("Failed to load joint traj pt. velocity");
116  }
117 
118  }
119  else
120  {
121  rtn = false;
122  LOG_ERROR("Failed to load joint traj. pt. position data");
123  }
124  }
125  else
126  {
127  rtn = false;
128  LOG_ERROR("Failed to load joint traj. pt. sequence number");
129  }
130 
131  return rtn;
132 }
133 
134 bool JointTrajPt::unload(industrial::byte_array::ByteArray *buffer)
135 {
136  bool rtn = false;
137 
138  LOG_COMM("Executing joint traj. pt. unload");
139  if (buffer->unload(this->duration_))
140  {
141  if (buffer->unload(this->velocity_))
142  {
143  if (this->joint_position_.unload(buffer))
144  {
145  if (buffer->unload(this->sequence_))
146  {
147  rtn = true;
148  LOG_COMM("Joint traj. pt successfully unloaded");
149  }
150  else
151  {
152  LOG_ERROR("Failed to unload joint traj. pt. sequence number");
153  rtn = false;
154  }
155  }
156  else
157  {
158  LOG_ERROR("Failed to unload joint traj. pt. position data");
159  rtn = false;
160  }
161 
162  }
163  else
164  {
165  LOG_ERROR("Failed to unload joint traj. pt. velocity");
166  rtn = false;
167  }
168  }
169  else
170  {
171  LOG_ERROR("Failed to unload joint traj. pt. duration");
172  rtn = false;
173  }
174 
175  return rtn;
176 }
177 
178 }
179 }
180 
industrial::joint_traj_pt::JointTrajPt::getJointPosition
void getJointPosition(industrial::joint_data::JointData &dest)
Returns a copy of the position data.
Definition: joint_traj_pt.h:136
industrial::joint_data::JointData
Class encapsulated joint data (positions, accelerations, velocity, torque, and/or effort).
Definition: joint_data.h:68
industrial::joint_traj_pt::JointTrajPt
Class encapsulated joint trajectory point data. The point data serves as a waypoint along a trajector...
Definition: joint_traj_pt.h:92
industrial::joint_traj_pt::JointTrajPt::sequence_
industrial::shared_types::shared_int sequence_
trajectory sequence number
Definition: joint_traj_pt.h:237
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
industrial::joint_traj_pt::JointTrajPt::velocity_
industrial::shared_types::shared_real velocity_
joint point velocity
Definition: joint_traj_pt.h:233
industrial::joint_data
Definition: joint_data.h:47
LOG_ERROR
#define LOG_ERROR(format,...)
Definition: log_wrapper.h:134
joint_traj_pt.h
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
industrial::joint_traj_pt::JointTrajPt::getVelocity
industrial::shared_types::shared_real getVelocity()
Returns joint trajectory point velocity.
Definition: joint_traj_pt.h:176
shared_types.h
log_wrapper.h
industrial::shared_types::shared_int
int shared_int
Definition: shared_types.h:62
industrial::joint_traj_pt::JointTrajPt::getDuration
industrial::shared_types::shared_real getDuration()
Returns joint trajectory point duration.
Definition: joint_traj_pt.h:196
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
industrial::joint_traj_pt::JointTrajPt::joint_position_
industrial::joint_data::JointData joint_position_
joint point positional data
Definition: joint_traj_pt.h:229
industrial::joint_traj_pt::JointTrajPt::duration_
industrial::shared_types::shared_real duration_
joint move duration
Definition: joint_traj_pt.h:242
init
void init(const M_string &remappings)
industrial::joint_traj_pt::JointTrajPt::getSequence
industrial::shared_types::shared_int getSequence()
Returns joint trajectory point sequence number.
Definition: joint_traj_pt.h:156


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