00001 """autogenerated by genpy from nasa_r2_common_msgs/LabeledPoseTrajectory.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 geometry_msgs.msg
00008 import nasa_r2_common_msgs.msg
00009 import genpy
00010 import std_msgs.msg
00011
00012 class LabeledPoseTrajectory(genpy.Message):
00013 _md5sum = "6859ade925296749b6e37b0f7d35911a"
00014 _type = "nasa_r2_common_msgs/LabeledPoseTrajectory"
00015 _has_header = True
00016 _full_text = """Header header
00017 string originator
00018 # length of nodes must match length of poses in each point
00019 string[] nodes
00020 # priorities must have nodes length or be omitted which defaults all to HIGH
00021 PriorityArray[] node_priorities
00022 # refFrames must either have one item or be nodes length
00023 string[] refFrames
00024 PoseTrajectoryPoint[] points
00025
00026
00027 ================================================================================
00028 MSG: std_msgs/Header
00029 # Standard metadata for higher-level stamped data types.
00030 # This is generally used to communicate timestamped data
00031 # in a particular coordinate frame.
00032 #
00033 # sequence ID: consecutively increasing ID
00034 uint32 seq
00035 #Two-integer timestamp that is expressed as:
00036 # * stamp.secs: seconds (stamp_secs) since epoch
00037 # * stamp.nsecs: nanoseconds since stamp_secs
00038 # time-handling sugar is provided by the client library
00039 time stamp
00040 #Frame this data is associated with
00041 # 0: no frame
00042 # 1: global frame
00043 string frame_id
00044
00045 ================================================================================
00046 MSG: nasa_r2_common_msgs/PriorityArray
00047 # axis_priorities must be 0, 1, or 6 elements
00048 # empty defaults to high, 1 sets all axes equal priority, 6 sets each axis (x, y, z, r, p, y)
00049 uint8[] axis_priorities
00050
00051 uint8 IGNORE = 0 # Exclude from trajectory
00052 uint8 CRITICAL = 1 # Most important priority in trajectory
00053 uint8 HIGH = 2 # Second most important priority in trajectory
00054 uint8 MEDIUM = 3 # Medium prioirty in trajectory
00055 uint8 LOW = 4 # Low priority in trajectory
00056 uint8 OPT = 5 # Optimize trajectory (solve for, but do not iterate to achieve)
00057
00058 #uint8 IGNORE = 0
00059 #uint8 LOW = 1
00060 #uint8 MEDIUM = 128
00061 #uint8 HIGH = 254
00062 #uint8 CRITICAL = 255
00063
00064 ================================================================================
00065 MSG: nasa_r2_common_msgs/PoseTrajectoryPoint
00066 # currently, position only but could add velocity and acceleration later
00067 geometry_msgs/Pose[] positions
00068 geometry_msgs/Twist[] velocities
00069 geometry_msgs/Twist[] accelerations
00070 duration time_from_start
00071
00072 ================================================================================
00073 MSG: geometry_msgs/Pose
00074 # A representation of pose in free space, composed of postion and orientation.
00075 Point position
00076 Quaternion orientation
00077
00078 ================================================================================
00079 MSG: geometry_msgs/Point
00080 # This contains the position of a point in free space
00081 float64 x
00082 float64 y
00083 float64 z
00084
00085 ================================================================================
00086 MSG: geometry_msgs/Quaternion
00087 # This represents an orientation in free space in quaternion form.
00088
00089 float64 x
00090 float64 y
00091 float64 z
00092 float64 w
00093
00094 ================================================================================
00095 MSG: geometry_msgs/Twist
00096 # This expresses velocity in free space broken into its linear and angular parts.
00097 Vector3 linear
00098 Vector3 angular
00099
00100 ================================================================================
00101 MSG: geometry_msgs/Vector3
00102 # This represents a vector in free space.
00103
00104 float64 x
00105 float64 y
00106 float64 z
00107 """
00108 __slots__ = ['header','originator','nodes','node_priorities','refFrames','points']
00109 _slot_types = ['std_msgs/Header','string','string[]','nasa_r2_common_msgs/PriorityArray[]','string[]','nasa_r2_common_msgs/PoseTrajectoryPoint[]']
00110
00111 def __init__(self, *args, **kwds):
00112 """
00113 Constructor. Any message fields that are implicitly/explicitly
00114 set to None will be assigned a default value. The recommend
00115 use is keyword arguments as this is more robust to future message
00116 changes. You cannot mix in-order arguments and keyword arguments.
00117
00118 The available fields are:
00119 header,originator,nodes,node_priorities,refFrames,points
00120
00121 :param args: complete set of field values, in .msg order
00122 :param kwds: use keyword arguments corresponding to message field names
00123 to set specific fields.
00124 """
00125 if args or kwds:
00126 super(LabeledPoseTrajectory, self).__init__(*args, **kwds)
00127
00128 if self.header is None:
00129 self.header = std_msgs.msg.Header()
00130 if self.originator is None:
00131 self.originator = ''
00132 if self.nodes is None:
00133 self.nodes = []
00134 if self.node_priorities is None:
00135 self.node_priorities = []
00136 if self.refFrames is None:
00137 self.refFrames = []
00138 if self.points is None:
00139 self.points = []
00140 else:
00141 self.header = std_msgs.msg.Header()
00142 self.originator = ''
00143 self.nodes = []
00144 self.node_priorities = []
00145 self.refFrames = []
00146 self.points = []
00147
00148 def _get_types(self):
00149 """
00150 internal API method
00151 """
00152 return self._slot_types
00153
00154 def serialize(self, buff):
00155 """
00156 serialize message into buffer
00157 :param buff: buffer, ``StringIO``
00158 """
00159 try:
00160 _x = self
00161 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00162 _x = self.header.frame_id
00163 length = len(_x)
00164 if python3 or type(_x) == unicode:
00165 _x = _x.encode('utf-8')
00166 length = len(_x)
00167 buff.write(struct.pack('<I%ss'%length, length, _x))
00168 _x = self.originator
00169 length = len(_x)
00170 if python3 or type(_x) == unicode:
00171 _x = _x.encode('utf-8')
00172 length = len(_x)
00173 buff.write(struct.pack('<I%ss'%length, length, _x))
00174 length = len(self.nodes)
00175 buff.write(_struct_I.pack(length))
00176 for val1 in self.nodes:
00177 length = len(val1)
00178 if python3 or type(val1) == unicode:
00179 val1 = val1.encode('utf-8')
00180 length = len(val1)
00181 buff.write(struct.pack('<I%ss'%length, length, val1))
00182 length = len(self.node_priorities)
00183 buff.write(_struct_I.pack(length))
00184 for val1 in self.node_priorities:
00185 _x = val1.axis_priorities
00186 length = len(_x)
00187
00188 if type(_x) in [list, tuple]:
00189 buff.write(struct.pack('<I%sB'%length, length, *_x))
00190 else:
00191 buff.write(struct.pack('<I%ss'%length, length, _x))
00192 length = len(self.refFrames)
00193 buff.write(_struct_I.pack(length))
00194 for val1 in self.refFrames:
00195 length = len(val1)
00196 if python3 or type(val1) == unicode:
00197 val1 = val1.encode('utf-8')
00198 length = len(val1)
00199 buff.write(struct.pack('<I%ss'%length, length, val1))
00200 length = len(self.points)
00201 buff.write(_struct_I.pack(length))
00202 for val1 in self.points:
00203 length = len(val1.positions)
00204 buff.write(_struct_I.pack(length))
00205 for val2 in val1.positions:
00206 _v1 = val2.position
00207 _x = _v1
00208 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00209 _v2 = val2.orientation
00210 _x = _v2
00211 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00212 length = len(val1.velocities)
00213 buff.write(_struct_I.pack(length))
00214 for val2 in val1.velocities:
00215 _v3 = val2.linear
00216 _x = _v3
00217 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00218 _v4 = val2.angular
00219 _x = _v4
00220 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00221 length = len(val1.accelerations)
00222 buff.write(_struct_I.pack(length))
00223 for val2 in val1.accelerations:
00224 _v5 = val2.linear
00225 _x = _v5
00226 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00227 _v6 = val2.angular
00228 _x = _v6
00229 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00230 _v7 = val1.time_from_start
00231 _x = _v7
00232 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00233 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00234 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00235
00236 def deserialize(self, str):
00237 """
00238 unpack serialized message in str into this message instance
00239 :param str: byte array of serialized message, ``str``
00240 """
00241 try:
00242 if self.header is None:
00243 self.header = std_msgs.msg.Header()
00244 if self.node_priorities is None:
00245 self.node_priorities = None
00246 if self.points is None:
00247 self.points = None
00248 end = 0
00249 _x = self
00250 start = end
00251 end += 12
00252 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00253 start = end
00254 end += 4
00255 (length,) = _struct_I.unpack(str[start:end])
00256 start = end
00257 end += length
00258 if python3:
00259 self.header.frame_id = str[start:end].decode('utf-8')
00260 else:
00261 self.header.frame_id = str[start:end]
00262 start = end
00263 end += 4
00264 (length,) = _struct_I.unpack(str[start:end])
00265 start = end
00266 end += length
00267 if python3:
00268 self.originator = str[start:end].decode('utf-8')
00269 else:
00270 self.originator = str[start:end]
00271 start = end
00272 end += 4
00273 (length,) = _struct_I.unpack(str[start:end])
00274 self.nodes = []
00275 for i in range(0, length):
00276 start = end
00277 end += 4
00278 (length,) = _struct_I.unpack(str[start:end])
00279 start = end
00280 end += length
00281 if python3:
00282 val1 = str[start:end].decode('utf-8')
00283 else:
00284 val1 = str[start:end]
00285 self.nodes.append(val1)
00286 start = end
00287 end += 4
00288 (length,) = _struct_I.unpack(str[start:end])
00289 self.node_priorities = []
00290 for i in range(0, length):
00291 val1 = nasa_r2_common_msgs.msg.PriorityArray()
00292 start = end
00293 end += 4
00294 (length,) = _struct_I.unpack(str[start:end])
00295 start = end
00296 end += length
00297 val1.axis_priorities = str[start:end]
00298 self.node_priorities.append(val1)
00299 start = end
00300 end += 4
00301 (length,) = _struct_I.unpack(str[start:end])
00302 self.refFrames = []
00303 for i in range(0, length):
00304 start = end
00305 end += 4
00306 (length,) = _struct_I.unpack(str[start:end])
00307 start = end
00308 end += length
00309 if python3:
00310 val1 = str[start:end].decode('utf-8')
00311 else:
00312 val1 = str[start:end]
00313 self.refFrames.append(val1)
00314 start = end
00315 end += 4
00316 (length,) = _struct_I.unpack(str[start:end])
00317 self.points = []
00318 for i in range(0, length):
00319 val1 = nasa_r2_common_msgs.msg.PoseTrajectoryPoint()
00320 start = end
00321 end += 4
00322 (length,) = _struct_I.unpack(str[start:end])
00323 val1.positions = []
00324 for i in range(0, length):
00325 val2 = geometry_msgs.msg.Pose()
00326 _v8 = val2.position
00327 _x = _v8
00328 start = end
00329 end += 24
00330 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00331 _v9 = val2.orientation
00332 _x = _v9
00333 start = end
00334 end += 32
00335 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00336 val1.positions.append(val2)
00337 start = end
00338 end += 4
00339 (length,) = _struct_I.unpack(str[start:end])
00340 val1.velocities = []
00341 for i in range(0, length):
00342 val2 = geometry_msgs.msg.Twist()
00343 _v10 = val2.linear
00344 _x = _v10
00345 start = end
00346 end += 24
00347 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00348 _v11 = val2.angular
00349 _x = _v11
00350 start = end
00351 end += 24
00352 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00353 val1.velocities.append(val2)
00354 start = end
00355 end += 4
00356 (length,) = _struct_I.unpack(str[start:end])
00357 val1.accelerations = []
00358 for i in range(0, length):
00359 val2 = geometry_msgs.msg.Twist()
00360 _v12 = val2.linear
00361 _x = _v12
00362 start = end
00363 end += 24
00364 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00365 _v13 = val2.angular
00366 _x = _v13
00367 start = end
00368 end += 24
00369 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00370 val1.accelerations.append(val2)
00371 _v14 = val1.time_from_start
00372 _x = _v14
00373 start = end
00374 end += 8
00375 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00376 self.points.append(val1)
00377 return self
00378 except struct.error as e:
00379 raise genpy.DeserializationError(e)
00380
00381
00382 def serialize_numpy(self, buff, numpy):
00383 """
00384 serialize message with numpy array types into buffer
00385 :param buff: buffer, ``StringIO``
00386 :param numpy: numpy python module
00387 """
00388 try:
00389 _x = self
00390 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00391 _x = self.header.frame_id
00392 length = len(_x)
00393 if python3 or type(_x) == unicode:
00394 _x = _x.encode('utf-8')
00395 length = len(_x)
00396 buff.write(struct.pack('<I%ss'%length, length, _x))
00397 _x = self.originator
00398 length = len(_x)
00399 if python3 or type(_x) == unicode:
00400 _x = _x.encode('utf-8')
00401 length = len(_x)
00402 buff.write(struct.pack('<I%ss'%length, length, _x))
00403 length = len(self.nodes)
00404 buff.write(_struct_I.pack(length))
00405 for val1 in self.nodes:
00406 length = len(val1)
00407 if python3 or type(val1) == unicode:
00408 val1 = val1.encode('utf-8')
00409 length = len(val1)
00410 buff.write(struct.pack('<I%ss'%length, length, val1))
00411 length = len(self.node_priorities)
00412 buff.write(_struct_I.pack(length))
00413 for val1 in self.node_priorities:
00414 _x = val1.axis_priorities
00415 length = len(_x)
00416
00417 if type(_x) in [list, tuple]:
00418 buff.write(struct.pack('<I%sB'%length, length, *_x))
00419 else:
00420 buff.write(struct.pack('<I%ss'%length, length, _x))
00421 length = len(self.refFrames)
00422 buff.write(_struct_I.pack(length))
00423 for val1 in self.refFrames:
00424 length = len(val1)
00425 if python3 or type(val1) == unicode:
00426 val1 = val1.encode('utf-8')
00427 length = len(val1)
00428 buff.write(struct.pack('<I%ss'%length, length, val1))
00429 length = len(self.points)
00430 buff.write(_struct_I.pack(length))
00431 for val1 in self.points:
00432 length = len(val1.positions)
00433 buff.write(_struct_I.pack(length))
00434 for val2 in val1.positions:
00435 _v15 = val2.position
00436 _x = _v15
00437 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00438 _v16 = val2.orientation
00439 _x = _v16
00440 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00441 length = len(val1.velocities)
00442 buff.write(_struct_I.pack(length))
00443 for val2 in val1.velocities:
00444 _v17 = val2.linear
00445 _x = _v17
00446 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00447 _v18 = val2.angular
00448 _x = _v18
00449 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00450 length = len(val1.accelerations)
00451 buff.write(_struct_I.pack(length))
00452 for val2 in val1.accelerations:
00453 _v19 = val2.linear
00454 _x = _v19
00455 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00456 _v20 = val2.angular
00457 _x = _v20
00458 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00459 _v21 = val1.time_from_start
00460 _x = _v21
00461 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00462 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00463 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00464
00465 def deserialize_numpy(self, str, numpy):
00466 """
00467 unpack serialized message in str into this message instance using numpy for array types
00468 :param str: byte array of serialized message, ``str``
00469 :param numpy: numpy python module
00470 """
00471 try:
00472 if self.header is None:
00473 self.header = std_msgs.msg.Header()
00474 if self.node_priorities is None:
00475 self.node_priorities = None
00476 if self.points is None:
00477 self.points = None
00478 end = 0
00479 _x = self
00480 start = end
00481 end += 12
00482 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00483 start = end
00484 end += 4
00485 (length,) = _struct_I.unpack(str[start:end])
00486 start = end
00487 end += length
00488 if python3:
00489 self.header.frame_id = str[start:end].decode('utf-8')
00490 else:
00491 self.header.frame_id = str[start:end]
00492 start = end
00493 end += 4
00494 (length,) = _struct_I.unpack(str[start:end])
00495 start = end
00496 end += length
00497 if python3:
00498 self.originator = str[start:end].decode('utf-8')
00499 else:
00500 self.originator = str[start:end]
00501 start = end
00502 end += 4
00503 (length,) = _struct_I.unpack(str[start:end])
00504 self.nodes = []
00505 for i in range(0, length):
00506 start = end
00507 end += 4
00508 (length,) = _struct_I.unpack(str[start:end])
00509 start = end
00510 end += length
00511 if python3:
00512 val1 = str[start:end].decode('utf-8')
00513 else:
00514 val1 = str[start:end]
00515 self.nodes.append(val1)
00516 start = end
00517 end += 4
00518 (length,) = _struct_I.unpack(str[start:end])
00519 self.node_priorities = []
00520 for i in range(0, length):
00521 val1 = nasa_r2_common_msgs.msg.PriorityArray()
00522 start = end
00523 end += 4
00524 (length,) = _struct_I.unpack(str[start:end])
00525 start = end
00526 end += length
00527 val1.axis_priorities = str[start:end]
00528 self.node_priorities.append(val1)
00529 start = end
00530 end += 4
00531 (length,) = _struct_I.unpack(str[start:end])
00532 self.refFrames = []
00533 for i in range(0, length):
00534 start = end
00535 end += 4
00536 (length,) = _struct_I.unpack(str[start:end])
00537 start = end
00538 end += length
00539 if python3:
00540 val1 = str[start:end].decode('utf-8')
00541 else:
00542 val1 = str[start:end]
00543 self.refFrames.append(val1)
00544 start = end
00545 end += 4
00546 (length,) = _struct_I.unpack(str[start:end])
00547 self.points = []
00548 for i in range(0, length):
00549 val1 = nasa_r2_common_msgs.msg.PoseTrajectoryPoint()
00550 start = end
00551 end += 4
00552 (length,) = _struct_I.unpack(str[start:end])
00553 val1.positions = []
00554 for i in range(0, length):
00555 val2 = geometry_msgs.msg.Pose()
00556 _v22 = val2.position
00557 _x = _v22
00558 start = end
00559 end += 24
00560 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00561 _v23 = val2.orientation
00562 _x = _v23
00563 start = end
00564 end += 32
00565 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00566 val1.positions.append(val2)
00567 start = end
00568 end += 4
00569 (length,) = _struct_I.unpack(str[start:end])
00570 val1.velocities = []
00571 for i in range(0, length):
00572 val2 = geometry_msgs.msg.Twist()
00573 _v24 = val2.linear
00574 _x = _v24
00575 start = end
00576 end += 24
00577 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00578 _v25 = val2.angular
00579 _x = _v25
00580 start = end
00581 end += 24
00582 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00583 val1.velocities.append(val2)
00584 start = end
00585 end += 4
00586 (length,) = _struct_I.unpack(str[start:end])
00587 val1.accelerations = []
00588 for i in range(0, length):
00589 val2 = geometry_msgs.msg.Twist()
00590 _v26 = val2.linear
00591 _x = _v26
00592 start = end
00593 end += 24
00594 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00595 _v27 = val2.angular
00596 _x = _v27
00597 start = end
00598 end += 24
00599 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00600 val1.accelerations.append(val2)
00601 _v28 = val1.time_from_start
00602 _x = _v28
00603 start = end
00604 end += 8
00605 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00606 self.points.append(val1)
00607 return self
00608 except struct.error as e:
00609 raise genpy.DeserializationError(e)
00610
00611 _struct_I = genpy.struct_I
00612 _struct_2i = struct.Struct("<2i")
00613 _struct_3I = struct.Struct("<3I")
00614 _struct_4d = struct.Struct("<4d")
00615 _struct_3d = struct.Struct("<3d")