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