00001 """autogenerated by genpy from control_msgs/FollowJointTrajectoryGoal.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import trajectory_msgs.msg
00008 import control_msgs.msg
00009 import genpy
00010 import std_msgs.msg
00011
00012 class FollowJointTrajectoryGoal(genpy.Message):
00013 _md5sum = "01f6d702507b59bae3fc1e7149e6210c"
00014 _type = "control_msgs/FollowJointTrajectoryGoal"
00015 _has_header = False
00016 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00017 # The joint trajectory to follow
00018 trajectory_msgs/JointTrajectory trajectory
00019
00020 # Tolerances for the trajectory. If the measured joint values fall
00021 # outside the tolerances the trajectory goal is aborted. Any
00022 # tolerances that are not specified (by being omitted or set to 0) are
00023 # set to the defaults for the action server (often taken from the
00024 # parameter server).
00025
00026 # Tolerances applied to the joints as the trajectory is executed. If
00027 # violated, the goal aborts with error_code set to
00028 # PATH_TOLERANCE_VIOLATED.
00029 JointTolerance[] path_tolerance
00030
00031 # To report success, the joints must be within goal_tolerance of the
00032 # final trajectory value. The goal must be achieved by time the
00033 # trajectory ends plus goal_time_tolerance. (goal_time_tolerance
00034 # allows some leeway in time, so that the trajectory goal can still
00035 # succeed even if the joints reach the goal some time after the
00036 # precise end time of the trajectory).
00037 #
00038 # If the joints are not within goal_tolerance after "trajectory finish
00039 # time" + goal_time_tolerance, the goal aborts with error_code set to
00040 # GOAL_TOLERANCE_VIOLATED
00041 JointTolerance[] goal_tolerance
00042 duration goal_time_tolerance
00043
00044
00045 ================================================================================
00046 MSG: trajectory_msgs/JointTrajectory
00047 Header header
00048 string[] joint_names
00049 JointTrajectoryPoint[] points
00050 ================================================================================
00051 MSG: std_msgs/Header
00052 # Standard metadata for higher-level stamped data types.
00053 # This is generally used to communicate timestamped data
00054 # in a particular coordinate frame.
00055 #
00056 # sequence ID: consecutively increasing ID
00057 uint32 seq
00058 #Two-integer timestamp that is expressed as:
00059 # * stamp.secs: seconds (stamp_secs) since epoch
00060 # * stamp.nsecs: nanoseconds since stamp_secs
00061 # time-handling sugar is provided by the client library
00062 time stamp
00063 #Frame this data is associated with
00064 # 0: no frame
00065 # 1: global frame
00066 string frame_id
00067
00068 ================================================================================
00069 MSG: trajectory_msgs/JointTrajectoryPoint
00070 float64[] positions
00071 float64[] velocities
00072 float64[] accelerations
00073 duration time_from_start
00074 ================================================================================
00075 MSG: control_msgs/JointTolerance
00076 # The tolerances specify the amount the position, velocity, and
00077 # accelerations can vary from the setpoints. For example, in the case
00078 # of trajectory control, when the actual position varies beyond
00079 # (desired position + position tolerance), the trajectory goal may
00080 # abort.
00081 #
00082 # There are two special values for tolerances:
00083 # * 0 - The tolerance is unspecified and will remain at whatever the default is
00084 # * -1 - The tolerance is "erased". If there was a default, the joint will be
00085 # allowed to move without restriction.
00086
00087 string name
00088 float64 position # in radians or meters (for a revolute or prismatic joint, respectively)
00089 float64 velocity # in rad/sec or m/sec
00090 float64 acceleration # in rad/sec^2 or m/sec^2
00091
00092 """
00093 __slots__ = ['trajectory','path_tolerance','goal_tolerance','goal_time_tolerance']
00094 _slot_types = ['trajectory_msgs/JointTrajectory','control_msgs/JointTolerance[]','control_msgs/JointTolerance[]','duration']
00095
00096 def __init__(self, *args, **kwds):
00097 """
00098 Constructor. Any message fields that are implicitly/explicitly
00099 set to None will be assigned a default value. The recommend
00100 use is keyword arguments as this is more robust to future message
00101 changes. You cannot mix in-order arguments and keyword arguments.
00102
00103 The available fields are:
00104 trajectory,path_tolerance,goal_tolerance,goal_time_tolerance
00105
00106 :param args: complete set of field values, in .msg order
00107 :param kwds: use keyword arguments corresponding to message field names
00108 to set specific fields.
00109 """
00110 if args or kwds:
00111 super(FollowJointTrajectoryGoal, self).__init__(*args, **kwds)
00112
00113 if self.trajectory is None:
00114 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00115 if self.path_tolerance is None:
00116 self.path_tolerance = []
00117 if self.goal_tolerance is None:
00118 self.goal_tolerance = []
00119 if self.goal_time_tolerance is None:
00120 self.goal_time_tolerance = genpy.Duration()
00121 else:
00122 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00123 self.path_tolerance = []
00124 self.goal_tolerance = []
00125 self.goal_time_tolerance = genpy.Duration()
00126
00127 def _get_types(self):
00128 """
00129 internal API method
00130 """
00131 return self._slot_types
00132
00133 def serialize(self, buff):
00134 """
00135 serialize message into buffer
00136 :param buff: buffer, ``StringIO``
00137 """
00138 try:
00139 _x = self
00140 buff.write(_struct_3I.pack(_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00141 _x = self.trajectory.header.frame_id
00142 length = len(_x)
00143 if python3 or type(_x) == unicode:
00144 _x = _x.encode('utf-8')
00145 length = len(_x)
00146 buff.write(struct.pack('<I%ss'%length, length, _x))
00147 length = len(self.trajectory.joint_names)
00148 buff.write(_struct_I.pack(length))
00149 for val1 in self.trajectory.joint_names:
00150 length = len(val1)
00151 if python3 or type(val1) == unicode:
00152 val1 = val1.encode('utf-8')
00153 length = len(val1)
00154 buff.write(struct.pack('<I%ss'%length, length, val1))
00155 length = len(self.trajectory.points)
00156 buff.write(_struct_I.pack(length))
00157 for val1 in self.trajectory.points:
00158 length = len(val1.positions)
00159 buff.write(_struct_I.pack(length))
00160 pattern = '<%sd'%length
00161 buff.write(struct.pack(pattern, *val1.positions))
00162 length = len(val1.velocities)
00163 buff.write(_struct_I.pack(length))
00164 pattern = '<%sd'%length
00165 buff.write(struct.pack(pattern, *val1.velocities))
00166 length = len(val1.accelerations)
00167 buff.write(_struct_I.pack(length))
00168 pattern = '<%sd'%length
00169 buff.write(struct.pack(pattern, *val1.accelerations))
00170 _v1 = val1.time_from_start
00171 _x = _v1
00172 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00173 length = len(self.path_tolerance)
00174 buff.write(_struct_I.pack(length))
00175 for val1 in self.path_tolerance:
00176 _x = val1.name
00177 length = len(_x)
00178 if python3 or type(_x) == unicode:
00179 _x = _x.encode('utf-8')
00180 length = len(_x)
00181 buff.write(struct.pack('<I%ss'%length, length, _x))
00182 _x = val1
00183 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration))
00184 length = len(self.goal_tolerance)
00185 buff.write(_struct_I.pack(length))
00186 for val1 in self.goal_tolerance:
00187 _x = val1.name
00188 length = len(_x)
00189 if python3 or type(_x) == unicode:
00190 _x = _x.encode('utf-8')
00191 length = len(_x)
00192 buff.write(struct.pack('<I%ss'%length, length, _x))
00193 _x = val1
00194 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration))
00195 _x = self
00196 buff.write(_struct_2i.pack(_x.goal_time_tolerance.secs, _x.goal_time_tolerance.nsecs))
00197 except struct.error as se: self._check_types(se)
00198 except TypeError as te: self._check_types(te)
00199
00200 def deserialize(self, str):
00201 """
00202 unpack serialized message in str into this message instance
00203 :param str: byte array of serialized message, ``str``
00204 """
00205 try:
00206 if self.trajectory is None:
00207 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00208 if self.path_tolerance is None:
00209 self.path_tolerance = None
00210 if self.goal_tolerance is None:
00211 self.goal_tolerance = None
00212 if self.goal_time_tolerance is None:
00213 self.goal_time_tolerance = genpy.Duration()
00214 end = 0
00215 _x = self
00216 start = end
00217 end += 12
00218 (_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00219 start = end
00220 end += 4
00221 (length,) = _struct_I.unpack(str[start:end])
00222 start = end
00223 end += length
00224 if python3:
00225 self.trajectory.header.frame_id = str[start:end].decode('utf-8')
00226 else:
00227 self.trajectory.header.frame_id = str[start:end]
00228 start = end
00229 end += 4
00230 (length,) = _struct_I.unpack(str[start:end])
00231 self.trajectory.joint_names = []
00232 for i in range(0, length):
00233 start = end
00234 end += 4
00235 (length,) = _struct_I.unpack(str[start:end])
00236 start = end
00237 end += length
00238 if python3:
00239 val1 = str[start:end].decode('utf-8')
00240 else:
00241 val1 = str[start:end]
00242 self.trajectory.joint_names.append(val1)
00243 start = end
00244 end += 4
00245 (length,) = _struct_I.unpack(str[start:end])
00246 self.trajectory.points = []
00247 for i in range(0, length):
00248 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00249 start = end
00250 end += 4
00251 (length,) = _struct_I.unpack(str[start:end])
00252 pattern = '<%sd'%length
00253 start = end
00254 end += struct.calcsize(pattern)
00255 val1.positions = struct.unpack(pattern, str[start:end])
00256 start = end
00257 end += 4
00258 (length,) = _struct_I.unpack(str[start:end])
00259 pattern = '<%sd'%length
00260 start = end
00261 end += struct.calcsize(pattern)
00262 val1.velocities = struct.unpack(pattern, str[start:end])
00263 start = end
00264 end += 4
00265 (length,) = _struct_I.unpack(str[start:end])
00266 pattern = '<%sd'%length
00267 start = end
00268 end += struct.calcsize(pattern)
00269 val1.accelerations = struct.unpack(pattern, str[start:end])
00270 _v2 = val1.time_from_start
00271 _x = _v2
00272 start = end
00273 end += 8
00274 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00275 self.trajectory.points.append(val1)
00276 start = end
00277 end += 4
00278 (length,) = _struct_I.unpack(str[start:end])
00279 self.path_tolerance = []
00280 for i in range(0, length):
00281 val1 = control_msgs.msg.JointTolerance()
00282 start = end
00283 end += 4
00284 (length,) = _struct_I.unpack(str[start:end])
00285 start = end
00286 end += length
00287 if python3:
00288 val1.name = str[start:end].decode('utf-8')
00289 else:
00290 val1.name = str[start:end]
00291 _x = val1
00292 start = end
00293 end += 24
00294 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end])
00295 self.path_tolerance.append(val1)
00296 start = end
00297 end += 4
00298 (length,) = _struct_I.unpack(str[start:end])
00299 self.goal_tolerance = []
00300 for i in range(0, length):
00301 val1 = control_msgs.msg.JointTolerance()
00302 start = end
00303 end += 4
00304 (length,) = _struct_I.unpack(str[start:end])
00305 start = end
00306 end += length
00307 if python3:
00308 val1.name = str[start:end].decode('utf-8')
00309 else:
00310 val1.name = str[start:end]
00311 _x = val1
00312 start = end
00313 end += 24
00314 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end])
00315 self.goal_tolerance.append(val1)
00316 _x = self
00317 start = end
00318 end += 8
00319 (_x.goal_time_tolerance.secs, _x.goal_time_tolerance.nsecs,) = _struct_2i.unpack(str[start:end])
00320 self.goal_time_tolerance.canon()
00321 return self
00322 except struct.error as e:
00323 raise genpy.DeserializationError(e)
00324
00325
00326 def serialize_numpy(self, buff, numpy):
00327 """
00328 serialize message with numpy array types into buffer
00329 :param buff: buffer, ``StringIO``
00330 :param numpy: numpy python module
00331 """
00332 try:
00333 _x = self
00334 buff.write(_struct_3I.pack(_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs))
00335 _x = self.trajectory.header.frame_id
00336 length = len(_x)
00337 if python3 or type(_x) == unicode:
00338 _x = _x.encode('utf-8')
00339 length = len(_x)
00340 buff.write(struct.pack('<I%ss'%length, length, _x))
00341 length = len(self.trajectory.joint_names)
00342 buff.write(_struct_I.pack(length))
00343 for val1 in self.trajectory.joint_names:
00344 length = len(val1)
00345 if python3 or type(val1) == unicode:
00346 val1 = val1.encode('utf-8')
00347 length = len(val1)
00348 buff.write(struct.pack('<I%ss'%length, length, val1))
00349 length = len(self.trajectory.points)
00350 buff.write(_struct_I.pack(length))
00351 for val1 in self.trajectory.points:
00352 length = len(val1.positions)
00353 buff.write(_struct_I.pack(length))
00354 pattern = '<%sd'%length
00355 buff.write(val1.positions.tostring())
00356 length = len(val1.velocities)
00357 buff.write(_struct_I.pack(length))
00358 pattern = '<%sd'%length
00359 buff.write(val1.velocities.tostring())
00360 length = len(val1.accelerations)
00361 buff.write(_struct_I.pack(length))
00362 pattern = '<%sd'%length
00363 buff.write(val1.accelerations.tostring())
00364 _v3 = val1.time_from_start
00365 _x = _v3
00366 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00367 length = len(self.path_tolerance)
00368 buff.write(_struct_I.pack(length))
00369 for val1 in self.path_tolerance:
00370 _x = val1.name
00371 length = len(_x)
00372 if python3 or type(_x) == unicode:
00373 _x = _x.encode('utf-8')
00374 length = len(_x)
00375 buff.write(struct.pack('<I%ss'%length, length, _x))
00376 _x = val1
00377 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration))
00378 length = len(self.goal_tolerance)
00379 buff.write(_struct_I.pack(length))
00380 for val1 in self.goal_tolerance:
00381 _x = val1.name
00382 length = len(_x)
00383 if python3 or type(_x) == unicode:
00384 _x = _x.encode('utf-8')
00385 length = len(_x)
00386 buff.write(struct.pack('<I%ss'%length, length, _x))
00387 _x = val1
00388 buff.write(_struct_3d.pack(_x.position, _x.velocity, _x.acceleration))
00389 _x = self
00390 buff.write(_struct_2i.pack(_x.goal_time_tolerance.secs, _x.goal_time_tolerance.nsecs))
00391 except struct.error as se: self._check_types(se)
00392 except TypeError as te: self._check_types(te)
00393
00394 def deserialize_numpy(self, str, numpy):
00395 """
00396 unpack serialized message in str into this message instance using numpy for array types
00397 :param str: byte array of serialized message, ``str``
00398 :param numpy: numpy python module
00399 """
00400 try:
00401 if self.trajectory is None:
00402 self.trajectory = trajectory_msgs.msg.JointTrajectory()
00403 if self.path_tolerance is None:
00404 self.path_tolerance = None
00405 if self.goal_tolerance is None:
00406 self.goal_tolerance = None
00407 if self.goal_time_tolerance is None:
00408 self.goal_time_tolerance = genpy.Duration()
00409 end = 0
00410 _x = self
00411 start = end
00412 end += 12
00413 (_x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00414 start = end
00415 end += 4
00416 (length,) = _struct_I.unpack(str[start:end])
00417 start = end
00418 end += length
00419 if python3:
00420 self.trajectory.header.frame_id = str[start:end].decode('utf-8')
00421 else:
00422 self.trajectory.header.frame_id = str[start:end]
00423 start = end
00424 end += 4
00425 (length,) = _struct_I.unpack(str[start:end])
00426 self.trajectory.joint_names = []
00427 for i in range(0, length):
00428 start = end
00429 end += 4
00430 (length,) = _struct_I.unpack(str[start:end])
00431 start = end
00432 end += length
00433 if python3:
00434 val1 = str[start:end].decode('utf-8')
00435 else:
00436 val1 = str[start:end]
00437 self.trajectory.joint_names.append(val1)
00438 start = end
00439 end += 4
00440 (length,) = _struct_I.unpack(str[start:end])
00441 self.trajectory.points = []
00442 for i in range(0, length):
00443 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00444 start = end
00445 end += 4
00446 (length,) = _struct_I.unpack(str[start:end])
00447 pattern = '<%sd'%length
00448 start = end
00449 end += struct.calcsize(pattern)
00450 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00451 start = end
00452 end += 4
00453 (length,) = _struct_I.unpack(str[start:end])
00454 pattern = '<%sd'%length
00455 start = end
00456 end += struct.calcsize(pattern)
00457 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00458 start = end
00459 end += 4
00460 (length,) = _struct_I.unpack(str[start:end])
00461 pattern = '<%sd'%length
00462 start = end
00463 end += struct.calcsize(pattern)
00464 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00465 _v4 = val1.time_from_start
00466 _x = _v4
00467 start = end
00468 end += 8
00469 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00470 self.trajectory.points.append(val1)
00471 start = end
00472 end += 4
00473 (length,) = _struct_I.unpack(str[start:end])
00474 self.path_tolerance = []
00475 for i in range(0, length):
00476 val1 = control_msgs.msg.JointTolerance()
00477 start = end
00478 end += 4
00479 (length,) = _struct_I.unpack(str[start:end])
00480 start = end
00481 end += length
00482 if python3:
00483 val1.name = str[start:end].decode('utf-8')
00484 else:
00485 val1.name = str[start:end]
00486 _x = val1
00487 start = end
00488 end += 24
00489 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end])
00490 self.path_tolerance.append(val1)
00491 start = end
00492 end += 4
00493 (length,) = _struct_I.unpack(str[start:end])
00494 self.goal_tolerance = []
00495 for i in range(0, length):
00496 val1 = control_msgs.msg.JointTolerance()
00497 start = end
00498 end += 4
00499 (length,) = _struct_I.unpack(str[start:end])
00500 start = end
00501 end += length
00502 if python3:
00503 val1.name = str[start:end].decode('utf-8')
00504 else:
00505 val1.name = str[start:end]
00506 _x = val1
00507 start = end
00508 end += 24
00509 (_x.position, _x.velocity, _x.acceleration,) = _struct_3d.unpack(str[start:end])
00510 self.goal_tolerance.append(val1)
00511 _x = self
00512 start = end
00513 end += 8
00514 (_x.goal_time_tolerance.secs, _x.goal_time_tolerance.nsecs,) = _struct_2i.unpack(str[start:end])
00515 self.goal_time_tolerance.canon()
00516 return self
00517 except struct.error as e:
00518 raise genpy.DeserializationError(e)
00519
00520 _struct_I = genpy.struct_I
00521 _struct_3I = struct.Struct("<3I")
00522 _struct_2i = struct.Struct("<2i")
00523 _struct_3d = struct.Struct("<3d")