00001 """autogenerated by genmsg_py from PickupResult.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import std_msgs.msg
00007 import object_manipulation_msgs.msg
00008 import sensor_msgs.msg
00009
00010 class PickupResult(roslib.message.Message):
00011 _md5sum = "793066b09b7f497fba66b4c2d9e27356"
00012 _type = "object_manipulation_msgs/PickupResult"
00013 _has_header = False
00014 _full_text = """# The overall result of the pickup attempt
00015 ManipulationResult manipulation_result
00016
00017 # The performed grasp, if attempt was successful
00018 Grasp grasp
00019
00020 # the complete list of attempted grasp, in the order in which they have been attempted
00021 # the successful one should be the last one in this list
00022 Grasp[] attempted_grasps
00023
00024 # the outcomes of the attempted grasps, in the same order as attempted_grasps
00025 GraspResult[] attempted_grasp_results
00026 ================================================================================
00027 MSG: object_manipulation_msgs/ManipulationResult
00028 # Result codes for manipulation tasks
00029
00030 # task completed as expected
00031 # generally means you can proceed as planned
00032 int32 SUCCESS = 1
00033
00034 # task not possible (e.g. out of reach or obstacles in the way)
00035 # generally means that the world was not disturbed, so you can try another task
00036 int32 UNFEASIBLE = -1
00037
00038 # task was thought possible, but failed due to unexpected events during execution
00039 # it is likely that the world was disturbed, so you are encouraged to refresh
00040 # your sensed world model before proceeding to another task
00041 int32 FAILED = -2
00042
00043 # a lower level error prevented task completion (e.g. joint controller not responding)
00044 # generally requires human attention
00045 int32 ERROR = -3
00046
00047 # means that at some point during execution we ended up in a state that the collision-aware
00048 # arm navigation module will not move out of. The world was likely not disturbed, but you
00049 # probably need a new collision map to move the arm out of the stuck position
00050 int32 ARM_MOVEMENT_PREVENTED = -4
00051
00052 # specific to grasp actions
00053 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00054 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00055 int32 LIFT_FAILED = -5
00056
00057 # specific to place actions
00058 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00059 # it is likely that the collision environment will see collisions between the hand and the object
00060 int32 RETREAT_FAILED = -6
00061
00062 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00063 int32 CANCELLED = -7
00064
00065 # the actual value of this error code
00066 int32 value
00067
00068 ================================================================================
00069 MSG: object_manipulation_msgs/Grasp
00070
00071 # The internal posture of the hand for the pre-grasp
00072 # only positions are used
00073 sensor_msgs/JointState pre_grasp_posture
00074
00075 # The internal posture of the hand for the grasp
00076 # positions and efforts are used
00077 sensor_msgs/JointState grasp_posture
00078
00079 # The position of the end-effector for the grasp relative to the object
00080 geometry_msgs/Pose grasp_pose
00081
00082 # The estimated probability of success for this grasp
00083 float64 success_probability
00084
00085 # Debug flag to indicate that this grasp would be the best in its cluster
00086 bool cluster_rep
00087 ================================================================================
00088 MSG: sensor_msgs/JointState
00089 # This is a message that holds data to describe the state of a set of torque controlled joints.
00090 #
00091 # The state of each joint (revolute or prismatic) is defined by:
00092 # * the position of the joint (rad or m),
00093 # * the velocity of the joint (rad/s or m/s) and
00094 # * the effort that is applied in the joint (Nm or N).
00095 #
00096 # Each joint is uniquely identified by its name
00097 # The header specifies the time at which the joint states were recorded. All the joint states
00098 # in one message have to be recorded at the same time.
00099 #
00100 # This message consists of a multiple arrays, one for each part of the joint state.
00101 # The goal is to make each of the fields optional. When e.g. your joints have no
00102 # effort associated with them, you can leave the effort array empty.
00103 #
00104 # All arrays in this message should have the same size, or be empty.
00105 # This is the only way to uniquely associate the joint name with the correct
00106 # states.
00107
00108
00109 Header header
00110
00111 string[] name
00112 float64[] position
00113 float64[] velocity
00114 float64[] effort
00115
00116 ================================================================================
00117 MSG: std_msgs/Header
00118 # Standard metadata for higher-level stamped data types.
00119 # This is generally used to communicate timestamped data
00120 # in a particular coordinate frame.
00121 #
00122 # sequence ID: consecutively increasing ID
00123 uint32 seq
00124 #Two-integer timestamp that is expressed as:
00125 # * stamp.secs: seconds (stamp_secs) since epoch
00126 # * stamp.nsecs: nanoseconds since stamp_secs
00127 # time-handling sugar is provided by the client library
00128 time stamp
00129 #Frame this data is associated with
00130 # 0: no frame
00131 # 1: global frame
00132 string frame_id
00133
00134 ================================================================================
00135 MSG: geometry_msgs/Pose
00136 # A representation of pose in free space, composed of postion and orientation.
00137 Point position
00138 Quaternion orientation
00139
00140 ================================================================================
00141 MSG: geometry_msgs/Point
00142 # This contains the position of a point in free space
00143 float64 x
00144 float64 y
00145 float64 z
00146
00147 ================================================================================
00148 MSG: geometry_msgs/Quaternion
00149 # This represents an orientation in free space in quaternion form.
00150
00151 float64 x
00152 float64 y
00153 float64 z
00154 float64 w
00155
00156 ================================================================================
00157 MSG: object_manipulation_msgs/GraspResult
00158 int32 SUCCESS = 1
00159 int32 GRASP_OUT_OF_REACH = 2
00160 int32 GRASP_IN_COLLISION = 3
00161 int32 GRASP_UNFEASIBLE = 4
00162 int32 PREGRASP_OUT_OF_REACH = 5
00163 int32 PREGRASP_IN_COLLISION = 6
00164 int32 PREGRASP_UNFEASIBLE = 7
00165 int32 LIFT_OUT_OF_REACH = 8
00166 int32 LIFT_IN_COLLISION = 9
00167 int32 LIFT_UNFEASIBLE = 10
00168 int32 MOVE_ARM_FAILED = 11
00169 int32 GRASP_FAILED = 12
00170 int32 LIFT_FAILED = 13
00171 int32 RETREAT_FAILED = 14
00172 int32 result_code
00173
00174 # whether the state of the world was disturbed by this attempt. generally, this flag
00175 # shows if another task can be attempted, or a new sensed world model is recommeded
00176 # before proceeding
00177 bool continuation_possible
00178
00179 """
00180 __slots__ = ['manipulation_result','grasp','attempted_grasps','attempted_grasp_results']
00181 _slot_types = ['object_manipulation_msgs/ManipulationResult','object_manipulation_msgs/Grasp','object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspResult[]']
00182
00183 def __init__(self, *args, **kwds):
00184 """
00185 Constructor. Any message fields that are implicitly/explicitly
00186 set to None will be assigned a default value. The recommend
00187 use is keyword arguments as this is more robust to future message
00188 changes. You cannot mix in-order arguments and keyword arguments.
00189
00190 The available fields are:
00191 manipulation_result,grasp,attempted_grasps,attempted_grasp_results
00192
00193 @param args: complete set of field values, in .msg order
00194 @param kwds: use keyword arguments corresponding to message field names
00195 to set specific fields.
00196 """
00197 if args or kwds:
00198 super(PickupResult, self).__init__(*args, **kwds)
00199
00200 if self.manipulation_result is None:
00201 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00202 if self.grasp is None:
00203 self.grasp = object_manipulation_msgs.msg.Grasp()
00204 if self.attempted_grasps is None:
00205 self.attempted_grasps = []
00206 if self.attempted_grasp_results is None:
00207 self.attempted_grasp_results = []
00208 else:
00209 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00210 self.grasp = object_manipulation_msgs.msg.Grasp()
00211 self.attempted_grasps = []
00212 self.attempted_grasp_results = []
00213
00214 def _get_types(self):
00215 """
00216 internal API method
00217 """
00218 return self._slot_types
00219
00220 def serialize(self, buff):
00221 """
00222 serialize message into buffer
00223 @param buff: buffer
00224 @type buff: StringIO
00225 """
00226 try:
00227 _x = self
00228 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00229 _x = self.grasp.pre_grasp_posture.header.frame_id
00230 length = len(_x)
00231 buff.write(struct.pack('<I%ss'%length, length, _x))
00232 length = len(self.grasp.pre_grasp_posture.name)
00233 buff.write(_struct_I.pack(length))
00234 for val1 in self.grasp.pre_grasp_posture.name:
00235 length = len(val1)
00236 buff.write(struct.pack('<I%ss'%length, length, val1))
00237 length = len(self.grasp.pre_grasp_posture.position)
00238 buff.write(_struct_I.pack(length))
00239 pattern = '<%sd'%length
00240 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00241 length = len(self.grasp.pre_grasp_posture.velocity)
00242 buff.write(_struct_I.pack(length))
00243 pattern = '<%sd'%length
00244 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00245 length = len(self.grasp.pre_grasp_posture.effort)
00246 buff.write(_struct_I.pack(length))
00247 pattern = '<%sd'%length
00248 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00249 _x = self
00250 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00251 _x = self.grasp.grasp_posture.header.frame_id
00252 length = len(_x)
00253 buff.write(struct.pack('<I%ss'%length, length, _x))
00254 length = len(self.grasp.grasp_posture.name)
00255 buff.write(_struct_I.pack(length))
00256 for val1 in self.grasp.grasp_posture.name:
00257 length = len(val1)
00258 buff.write(struct.pack('<I%ss'%length, length, val1))
00259 length = len(self.grasp.grasp_posture.position)
00260 buff.write(_struct_I.pack(length))
00261 pattern = '<%sd'%length
00262 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00263 length = len(self.grasp.grasp_posture.velocity)
00264 buff.write(_struct_I.pack(length))
00265 pattern = '<%sd'%length
00266 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00267 length = len(self.grasp.grasp_posture.effort)
00268 buff.write(_struct_I.pack(length))
00269 pattern = '<%sd'%length
00270 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00271 _x = self
00272 buff.write(_struct_8dB.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep))
00273 length = len(self.attempted_grasps)
00274 buff.write(_struct_I.pack(length))
00275 for val1 in self.attempted_grasps:
00276 _v1 = val1.pre_grasp_posture
00277 _v2 = _v1.header
00278 buff.write(_struct_I.pack(_v2.seq))
00279 _v3 = _v2.stamp
00280 _x = _v3
00281 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00282 _x = _v2.frame_id
00283 length = len(_x)
00284 buff.write(struct.pack('<I%ss'%length, length, _x))
00285 length = len(_v1.name)
00286 buff.write(_struct_I.pack(length))
00287 for val3 in _v1.name:
00288 length = len(val3)
00289 buff.write(struct.pack('<I%ss'%length, length, val3))
00290 length = len(_v1.position)
00291 buff.write(_struct_I.pack(length))
00292 pattern = '<%sd'%length
00293 buff.write(struct.pack(pattern, *_v1.position))
00294 length = len(_v1.velocity)
00295 buff.write(_struct_I.pack(length))
00296 pattern = '<%sd'%length
00297 buff.write(struct.pack(pattern, *_v1.velocity))
00298 length = len(_v1.effort)
00299 buff.write(_struct_I.pack(length))
00300 pattern = '<%sd'%length
00301 buff.write(struct.pack(pattern, *_v1.effort))
00302 _v4 = val1.grasp_posture
00303 _v5 = _v4.header
00304 buff.write(_struct_I.pack(_v5.seq))
00305 _v6 = _v5.stamp
00306 _x = _v6
00307 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00308 _x = _v5.frame_id
00309 length = len(_x)
00310 buff.write(struct.pack('<I%ss'%length, length, _x))
00311 length = len(_v4.name)
00312 buff.write(_struct_I.pack(length))
00313 for val3 in _v4.name:
00314 length = len(val3)
00315 buff.write(struct.pack('<I%ss'%length, length, val3))
00316 length = len(_v4.position)
00317 buff.write(_struct_I.pack(length))
00318 pattern = '<%sd'%length
00319 buff.write(struct.pack(pattern, *_v4.position))
00320 length = len(_v4.velocity)
00321 buff.write(_struct_I.pack(length))
00322 pattern = '<%sd'%length
00323 buff.write(struct.pack(pattern, *_v4.velocity))
00324 length = len(_v4.effort)
00325 buff.write(_struct_I.pack(length))
00326 pattern = '<%sd'%length
00327 buff.write(struct.pack(pattern, *_v4.effort))
00328 _v7 = val1.grasp_pose
00329 _v8 = _v7.position
00330 _x = _v8
00331 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00332 _v9 = _v7.orientation
00333 _x = _v9
00334 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00335 _x = val1
00336 buff.write(_struct_dB.pack(_x.success_probability, _x.cluster_rep))
00337 length = len(self.attempted_grasp_results)
00338 buff.write(_struct_I.pack(length))
00339 for val1 in self.attempted_grasp_results:
00340 _x = val1
00341 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
00342 except struct.error, se: self._check_types(se)
00343 except TypeError, te: self._check_types(te)
00344
00345 def deserialize(self, str):
00346 """
00347 unpack serialized message in str into this message instance
00348 @param str: byte array of serialized message
00349 @type str: str
00350 """
00351 try:
00352 if self.manipulation_result is None:
00353 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00354 if self.grasp is None:
00355 self.grasp = object_manipulation_msgs.msg.Grasp()
00356 end = 0
00357 _x = self
00358 start = end
00359 end += 16
00360 (_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
00361 start = end
00362 end += 4
00363 (length,) = _struct_I.unpack(str[start:end])
00364 start = end
00365 end += length
00366 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00367 start = end
00368 end += 4
00369 (length,) = _struct_I.unpack(str[start:end])
00370 self.grasp.pre_grasp_posture.name = []
00371 for i in xrange(0, length):
00372 start = end
00373 end += 4
00374 (length,) = _struct_I.unpack(str[start:end])
00375 start = end
00376 end += length
00377 val1 = str[start:end]
00378 self.grasp.pre_grasp_posture.name.append(val1)
00379 start = end
00380 end += 4
00381 (length,) = _struct_I.unpack(str[start:end])
00382 pattern = '<%sd'%length
00383 start = end
00384 end += struct.calcsize(pattern)
00385 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
00386 start = end
00387 end += 4
00388 (length,) = _struct_I.unpack(str[start:end])
00389 pattern = '<%sd'%length
00390 start = end
00391 end += struct.calcsize(pattern)
00392 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00393 start = end
00394 end += 4
00395 (length,) = _struct_I.unpack(str[start:end])
00396 pattern = '<%sd'%length
00397 start = end
00398 end += struct.calcsize(pattern)
00399 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
00400 _x = self
00401 start = end
00402 end += 12
00403 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00404 start = end
00405 end += 4
00406 (length,) = _struct_I.unpack(str[start:end])
00407 start = end
00408 end += length
00409 self.grasp.grasp_posture.header.frame_id = str[start:end]
00410 start = end
00411 end += 4
00412 (length,) = _struct_I.unpack(str[start:end])
00413 self.grasp.grasp_posture.name = []
00414 for i in xrange(0, length):
00415 start = end
00416 end += 4
00417 (length,) = _struct_I.unpack(str[start:end])
00418 start = end
00419 end += length
00420 val1 = str[start:end]
00421 self.grasp.grasp_posture.name.append(val1)
00422 start = end
00423 end += 4
00424 (length,) = _struct_I.unpack(str[start:end])
00425 pattern = '<%sd'%length
00426 start = end
00427 end += struct.calcsize(pattern)
00428 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
00429 start = end
00430 end += 4
00431 (length,) = _struct_I.unpack(str[start:end])
00432 pattern = '<%sd'%length
00433 start = end
00434 end += struct.calcsize(pattern)
00435 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00436 start = end
00437 end += 4
00438 (length,) = _struct_I.unpack(str[start:end])
00439 pattern = '<%sd'%length
00440 start = end
00441 end += struct.calcsize(pattern)
00442 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
00443 _x = self
00444 start = end
00445 end += 65
00446 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep,) = _struct_8dB.unpack(str[start:end])
00447 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
00448 start = end
00449 end += 4
00450 (length,) = _struct_I.unpack(str[start:end])
00451 self.attempted_grasps = []
00452 for i in xrange(0, length):
00453 val1 = object_manipulation_msgs.msg.Grasp()
00454 _v10 = val1.pre_grasp_posture
00455 _v11 = _v10.header
00456 start = end
00457 end += 4
00458 (_v11.seq,) = _struct_I.unpack(str[start:end])
00459 _v12 = _v11.stamp
00460 _x = _v12
00461 start = end
00462 end += 8
00463 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00464 start = end
00465 end += 4
00466 (length,) = _struct_I.unpack(str[start:end])
00467 start = end
00468 end += length
00469 _v11.frame_id = str[start:end]
00470 start = end
00471 end += 4
00472 (length,) = _struct_I.unpack(str[start:end])
00473 _v10.name = []
00474 for i in xrange(0, length):
00475 start = end
00476 end += 4
00477 (length,) = _struct_I.unpack(str[start:end])
00478 start = end
00479 end += length
00480 val3 = str[start:end]
00481 _v10.name.append(val3)
00482 start = end
00483 end += 4
00484 (length,) = _struct_I.unpack(str[start:end])
00485 pattern = '<%sd'%length
00486 start = end
00487 end += struct.calcsize(pattern)
00488 _v10.position = struct.unpack(pattern, str[start:end])
00489 start = end
00490 end += 4
00491 (length,) = _struct_I.unpack(str[start:end])
00492 pattern = '<%sd'%length
00493 start = end
00494 end += struct.calcsize(pattern)
00495 _v10.velocity = struct.unpack(pattern, str[start:end])
00496 start = end
00497 end += 4
00498 (length,) = _struct_I.unpack(str[start:end])
00499 pattern = '<%sd'%length
00500 start = end
00501 end += struct.calcsize(pattern)
00502 _v10.effort = struct.unpack(pattern, str[start:end])
00503 _v13 = val1.grasp_posture
00504 _v14 = _v13.header
00505 start = end
00506 end += 4
00507 (_v14.seq,) = _struct_I.unpack(str[start:end])
00508 _v15 = _v14.stamp
00509 _x = _v15
00510 start = end
00511 end += 8
00512 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00513 start = end
00514 end += 4
00515 (length,) = _struct_I.unpack(str[start:end])
00516 start = end
00517 end += length
00518 _v14.frame_id = str[start:end]
00519 start = end
00520 end += 4
00521 (length,) = _struct_I.unpack(str[start:end])
00522 _v13.name = []
00523 for i in xrange(0, length):
00524 start = end
00525 end += 4
00526 (length,) = _struct_I.unpack(str[start:end])
00527 start = end
00528 end += length
00529 val3 = str[start:end]
00530 _v13.name.append(val3)
00531 start = end
00532 end += 4
00533 (length,) = _struct_I.unpack(str[start:end])
00534 pattern = '<%sd'%length
00535 start = end
00536 end += struct.calcsize(pattern)
00537 _v13.position = struct.unpack(pattern, str[start:end])
00538 start = end
00539 end += 4
00540 (length,) = _struct_I.unpack(str[start:end])
00541 pattern = '<%sd'%length
00542 start = end
00543 end += struct.calcsize(pattern)
00544 _v13.velocity = struct.unpack(pattern, str[start:end])
00545 start = end
00546 end += 4
00547 (length,) = _struct_I.unpack(str[start:end])
00548 pattern = '<%sd'%length
00549 start = end
00550 end += struct.calcsize(pattern)
00551 _v13.effort = struct.unpack(pattern, str[start:end])
00552 _v16 = val1.grasp_pose
00553 _v17 = _v16.position
00554 _x = _v17
00555 start = end
00556 end += 24
00557 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00558 _v18 = _v16.orientation
00559 _x = _v18
00560 start = end
00561 end += 32
00562 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00563 _x = val1
00564 start = end
00565 end += 9
00566 (_x.success_probability, _x.cluster_rep,) = _struct_dB.unpack(str[start:end])
00567 val1.cluster_rep = bool(val1.cluster_rep)
00568 self.attempted_grasps.append(val1)
00569 start = end
00570 end += 4
00571 (length,) = _struct_I.unpack(str[start:end])
00572 self.attempted_grasp_results = []
00573 for i in xrange(0, length):
00574 val1 = object_manipulation_msgs.msg.GraspResult()
00575 _x = val1
00576 start = end
00577 end += 5
00578 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
00579 val1.continuation_possible = bool(val1.continuation_possible)
00580 self.attempted_grasp_results.append(val1)
00581 return self
00582 except struct.error, e:
00583 raise roslib.message.DeserializationError(e)
00584
00585
00586 def serialize_numpy(self, buff, numpy):
00587 """
00588 serialize message with numpy array types into buffer
00589 @param buff: buffer
00590 @type buff: StringIO
00591 @param numpy: numpy python module
00592 @type numpy module
00593 """
00594 try:
00595 _x = self
00596 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00597 _x = self.grasp.pre_grasp_posture.header.frame_id
00598 length = len(_x)
00599 buff.write(struct.pack('<I%ss'%length, length, _x))
00600 length = len(self.grasp.pre_grasp_posture.name)
00601 buff.write(_struct_I.pack(length))
00602 for val1 in self.grasp.pre_grasp_posture.name:
00603 length = len(val1)
00604 buff.write(struct.pack('<I%ss'%length, length, val1))
00605 length = len(self.grasp.pre_grasp_posture.position)
00606 buff.write(_struct_I.pack(length))
00607 pattern = '<%sd'%length
00608 buff.write(self.grasp.pre_grasp_posture.position.tostring())
00609 length = len(self.grasp.pre_grasp_posture.velocity)
00610 buff.write(_struct_I.pack(length))
00611 pattern = '<%sd'%length
00612 buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
00613 length = len(self.grasp.pre_grasp_posture.effort)
00614 buff.write(_struct_I.pack(length))
00615 pattern = '<%sd'%length
00616 buff.write(self.grasp.pre_grasp_posture.effort.tostring())
00617 _x = self
00618 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00619 _x = self.grasp.grasp_posture.header.frame_id
00620 length = len(_x)
00621 buff.write(struct.pack('<I%ss'%length, length, _x))
00622 length = len(self.grasp.grasp_posture.name)
00623 buff.write(_struct_I.pack(length))
00624 for val1 in self.grasp.grasp_posture.name:
00625 length = len(val1)
00626 buff.write(struct.pack('<I%ss'%length, length, val1))
00627 length = len(self.grasp.grasp_posture.position)
00628 buff.write(_struct_I.pack(length))
00629 pattern = '<%sd'%length
00630 buff.write(self.grasp.grasp_posture.position.tostring())
00631 length = len(self.grasp.grasp_posture.velocity)
00632 buff.write(_struct_I.pack(length))
00633 pattern = '<%sd'%length
00634 buff.write(self.grasp.grasp_posture.velocity.tostring())
00635 length = len(self.grasp.grasp_posture.effort)
00636 buff.write(_struct_I.pack(length))
00637 pattern = '<%sd'%length
00638 buff.write(self.grasp.grasp_posture.effort.tostring())
00639 _x = self
00640 buff.write(_struct_8dB.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep))
00641 length = len(self.attempted_grasps)
00642 buff.write(_struct_I.pack(length))
00643 for val1 in self.attempted_grasps:
00644 _v19 = val1.pre_grasp_posture
00645 _v20 = _v19.header
00646 buff.write(_struct_I.pack(_v20.seq))
00647 _v21 = _v20.stamp
00648 _x = _v21
00649 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00650 _x = _v20.frame_id
00651 length = len(_x)
00652 buff.write(struct.pack('<I%ss'%length, length, _x))
00653 length = len(_v19.name)
00654 buff.write(_struct_I.pack(length))
00655 for val3 in _v19.name:
00656 length = len(val3)
00657 buff.write(struct.pack('<I%ss'%length, length, val3))
00658 length = len(_v19.position)
00659 buff.write(_struct_I.pack(length))
00660 pattern = '<%sd'%length
00661 buff.write(_v19.position.tostring())
00662 length = len(_v19.velocity)
00663 buff.write(_struct_I.pack(length))
00664 pattern = '<%sd'%length
00665 buff.write(_v19.velocity.tostring())
00666 length = len(_v19.effort)
00667 buff.write(_struct_I.pack(length))
00668 pattern = '<%sd'%length
00669 buff.write(_v19.effort.tostring())
00670 _v22 = val1.grasp_posture
00671 _v23 = _v22.header
00672 buff.write(_struct_I.pack(_v23.seq))
00673 _v24 = _v23.stamp
00674 _x = _v24
00675 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00676 _x = _v23.frame_id
00677 length = len(_x)
00678 buff.write(struct.pack('<I%ss'%length, length, _x))
00679 length = len(_v22.name)
00680 buff.write(_struct_I.pack(length))
00681 for val3 in _v22.name:
00682 length = len(val3)
00683 buff.write(struct.pack('<I%ss'%length, length, val3))
00684 length = len(_v22.position)
00685 buff.write(_struct_I.pack(length))
00686 pattern = '<%sd'%length
00687 buff.write(_v22.position.tostring())
00688 length = len(_v22.velocity)
00689 buff.write(_struct_I.pack(length))
00690 pattern = '<%sd'%length
00691 buff.write(_v22.velocity.tostring())
00692 length = len(_v22.effort)
00693 buff.write(_struct_I.pack(length))
00694 pattern = '<%sd'%length
00695 buff.write(_v22.effort.tostring())
00696 _v25 = val1.grasp_pose
00697 _v26 = _v25.position
00698 _x = _v26
00699 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00700 _v27 = _v25.orientation
00701 _x = _v27
00702 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00703 _x = val1
00704 buff.write(_struct_dB.pack(_x.success_probability, _x.cluster_rep))
00705 length = len(self.attempted_grasp_results)
00706 buff.write(_struct_I.pack(length))
00707 for val1 in self.attempted_grasp_results:
00708 _x = val1
00709 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
00710 except struct.error, se: self._check_types(se)
00711 except TypeError, te: self._check_types(te)
00712
00713 def deserialize_numpy(self, str, numpy):
00714 """
00715 unpack serialized message in str into this message instance using numpy for array types
00716 @param str: byte array of serialized message
00717 @type str: str
00718 @param numpy: numpy python module
00719 @type numpy: module
00720 """
00721 try:
00722 if self.manipulation_result is None:
00723 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00724 if self.grasp is None:
00725 self.grasp = object_manipulation_msgs.msg.Grasp()
00726 end = 0
00727 _x = self
00728 start = end
00729 end += 16
00730 (_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
00731 start = end
00732 end += 4
00733 (length,) = _struct_I.unpack(str[start:end])
00734 start = end
00735 end += length
00736 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00737 start = end
00738 end += 4
00739 (length,) = _struct_I.unpack(str[start:end])
00740 self.grasp.pre_grasp_posture.name = []
00741 for i in xrange(0, length):
00742 start = end
00743 end += 4
00744 (length,) = _struct_I.unpack(str[start:end])
00745 start = end
00746 end += length
00747 val1 = str[start:end]
00748 self.grasp.pre_grasp_posture.name.append(val1)
00749 start = end
00750 end += 4
00751 (length,) = _struct_I.unpack(str[start:end])
00752 pattern = '<%sd'%length
00753 start = end
00754 end += struct.calcsize(pattern)
00755 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00756 start = end
00757 end += 4
00758 (length,) = _struct_I.unpack(str[start:end])
00759 pattern = '<%sd'%length
00760 start = end
00761 end += struct.calcsize(pattern)
00762 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00763 start = end
00764 end += 4
00765 (length,) = _struct_I.unpack(str[start:end])
00766 pattern = '<%sd'%length
00767 start = end
00768 end += struct.calcsize(pattern)
00769 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00770 _x = self
00771 start = end
00772 end += 12
00773 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00774 start = end
00775 end += 4
00776 (length,) = _struct_I.unpack(str[start:end])
00777 start = end
00778 end += length
00779 self.grasp.grasp_posture.header.frame_id = str[start:end]
00780 start = end
00781 end += 4
00782 (length,) = _struct_I.unpack(str[start:end])
00783 self.grasp.grasp_posture.name = []
00784 for i in xrange(0, length):
00785 start = end
00786 end += 4
00787 (length,) = _struct_I.unpack(str[start:end])
00788 start = end
00789 end += length
00790 val1 = str[start:end]
00791 self.grasp.grasp_posture.name.append(val1)
00792 start = end
00793 end += 4
00794 (length,) = _struct_I.unpack(str[start:end])
00795 pattern = '<%sd'%length
00796 start = end
00797 end += struct.calcsize(pattern)
00798 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00799 start = end
00800 end += 4
00801 (length,) = _struct_I.unpack(str[start:end])
00802 pattern = '<%sd'%length
00803 start = end
00804 end += struct.calcsize(pattern)
00805 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00806 start = end
00807 end += 4
00808 (length,) = _struct_I.unpack(str[start:end])
00809 pattern = '<%sd'%length
00810 start = end
00811 end += struct.calcsize(pattern)
00812 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00813 _x = self
00814 start = end
00815 end += 65
00816 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep,) = _struct_8dB.unpack(str[start:end])
00817 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
00818 start = end
00819 end += 4
00820 (length,) = _struct_I.unpack(str[start:end])
00821 self.attempted_grasps = []
00822 for i in xrange(0, length):
00823 val1 = object_manipulation_msgs.msg.Grasp()
00824 _v28 = val1.pre_grasp_posture
00825 _v29 = _v28.header
00826 start = end
00827 end += 4
00828 (_v29.seq,) = _struct_I.unpack(str[start:end])
00829 _v30 = _v29.stamp
00830 _x = _v30
00831 start = end
00832 end += 8
00833 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00834 start = end
00835 end += 4
00836 (length,) = _struct_I.unpack(str[start:end])
00837 start = end
00838 end += length
00839 _v29.frame_id = str[start:end]
00840 start = end
00841 end += 4
00842 (length,) = _struct_I.unpack(str[start:end])
00843 _v28.name = []
00844 for i in xrange(0, length):
00845 start = end
00846 end += 4
00847 (length,) = _struct_I.unpack(str[start:end])
00848 start = end
00849 end += length
00850 val3 = str[start:end]
00851 _v28.name.append(val3)
00852 start = end
00853 end += 4
00854 (length,) = _struct_I.unpack(str[start:end])
00855 pattern = '<%sd'%length
00856 start = end
00857 end += struct.calcsize(pattern)
00858 _v28.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00859 start = end
00860 end += 4
00861 (length,) = _struct_I.unpack(str[start:end])
00862 pattern = '<%sd'%length
00863 start = end
00864 end += struct.calcsize(pattern)
00865 _v28.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00866 start = end
00867 end += 4
00868 (length,) = _struct_I.unpack(str[start:end])
00869 pattern = '<%sd'%length
00870 start = end
00871 end += struct.calcsize(pattern)
00872 _v28.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00873 _v31 = val1.grasp_posture
00874 _v32 = _v31.header
00875 start = end
00876 end += 4
00877 (_v32.seq,) = _struct_I.unpack(str[start:end])
00878 _v33 = _v32.stamp
00879 _x = _v33
00880 start = end
00881 end += 8
00882 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00883 start = end
00884 end += 4
00885 (length,) = _struct_I.unpack(str[start:end])
00886 start = end
00887 end += length
00888 _v32.frame_id = str[start:end]
00889 start = end
00890 end += 4
00891 (length,) = _struct_I.unpack(str[start:end])
00892 _v31.name = []
00893 for i in xrange(0, length):
00894 start = end
00895 end += 4
00896 (length,) = _struct_I.unpack(str[start:end])
00897 start = end
00898 end += length
00899 val3 = str[start:end]
00900 _v31.name.append(val3)
00901 start = end
00902 end += 4
00903 (length,) = _struct_I.unpack(str[start:end])
00904 pattern = '<%sd'%length
00905 start = end
00906 end += struct.calcsize(pattern)
00907 _v31.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00908 start = end
00909 end += 4
00910 (length,) = _struct_I.unpack(str[start:end])
00911 pattern = '<%sd'%length
00912 start = end
00913 end += struct.calcsize(pattern)
00914 _v31.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00915 start = end
00916 end += 4
00917 (length,) = _struct_I.unpack(str[start:end])
00918 pattern = '<%sd'%length
00919 start = end
00920 end += struct.calcsize(pattern)
00921 _v31.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00922 _v34 = val1.grasp_pose
00923 _v35 = _v34.position
00924 _x = _v35
00925 start = end
00926 end += 24
00927 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00928 _v36 = _v34.orientation
00929 _x = _v36
00930 start = end
00931 end += 32
00932 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00933 _x = val1
00934 start = end
00935 end += 9
00936 (_x.success_probability, _x.cluster_rep,) = _struct_dB.unpack(str[start:end])
00937 val1.cluster_rep = bool(val1.cluster_rep)
00938 self.attempted_grasps.append(val1)
00939 start = end
00940 end += 4
00941 (length,) = _struct_I.unpack(str[start:end])
00942 self.attempted_grasp_results = []
00943 for i in xrange(0, length):
00944 val1 = object_manipulation_msgs.msg.GraspResult()
00945 _x = val1
00946 start = end
00947 end += 5
00948 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
00949 val1.continuation_possible = bool(val1.continuation_possible)
00950 self.attempted_grasp_results.append(val1)
00951 return self
00952 except struct.error, e:
00953 raise roslib.message.DeserializationError(e)
00954
00955 _struct_I = roslib.message.struct_I
00956 _struct_8dB = struct.Struct("<8dB")
00957 _struct_dB = struct.Struct("<dB")
00958 _struct_3I = struct.Struct("<3I")
00959 _struct_i3I = struct.Struct("<i3I")
00960 _struct_4d = struct.Struct("<4d")
00961 _struct_iB = struct.Struct("<iB")
00962 _struct_2I = struct.Struct("<2I")
00963 _struct_3d = struct.Struct("<3d")