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