00001 """autogenerated by genpy from object_manipulation_msgs/PickupResult.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import geometry_msgs.msg
00008 import manipulation_msgs.msg
00009 import object_manipulation_msgs.msg
00010 import std_msgs.msg
00011 import sensor_msgs.msg
00012
00013 class PickupResult(genpy.Message):
00014 _md5sum = "39cd5db390aea1106e31a3fa0b90019f"
00015 _type = "object_manipulation_msgs/PickupResult"
00016 _has_header = False
00017 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00018
00019 # The overall result of the pickup attempt
00020 ManipulationResult manipulation_result
00021
00022 # The performed grasp, if attempt was successful
00023 manipulation_msgs/Grasp grasp
00024
00025 # the complete list of attempted grasp, in the order in which they have been attempted
00026 # the successful one should be the last one in this list
00027 manipulation_msgs/Grasp[] attempted_grasps
00028
00029 # the outcomes of the attempted grasps, in the same order as attempted_grasps
00030 GraspResult[] attempted_grasp_results
00031
00032
00033 ================================================================================
00034 MSG: object_manipulation_msgs/ManipulationResult
00035 # Result codes for manipulation tasks
00036
00037 # task completed as expected
00038 # generally means you can proceed as planned
00039 int32 SUCCESS = 1
00040
00041 # task not possible (e.g. out of reach or obstacles in the way)
00042 # generally means that the world was not disturbed, so you can try another task
00043 int32 UNFEASIBLE = -1
00044
00045 # task was thought possible, but failed due to unexpected events during execution
00046 # it is likely that the world was disturbed, so you are encouraged to refresh
00047 # your sensed world model before proceeding to another task
00048 int32 FAILED = -2
00049
00050 # a lower level error prevented task completion (e.g. joint controller not responding)
00051 # generally requires human attention
00052 int32 ERROR = -3
00053
00054 # means that at some point during execution we ended up in a state that the collision-aware
00055 # arm navigation module will not move out of. The world was likely not disturbed, but you
00056 # probably need a new collision map to move the arm out of the stuck position
00057 int32 ARM_MOVEMENT_PREVENTED = -4
00058
00059 # specific to grasp actions
00060 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00061 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00062 int32 LIFT_FAILED = -5
00063
00064 # specific to place actions
00065 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00066 # it is likely that the collision environment will see collisions between the hand and the object
00067 int32 RETREAT_FAILED = -6
00068
00069 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00070 int32 CANCELLED = -7
00071
00072 # the actual value of this error code
00073 int32 value
00074
00075 ================================================================================
00076 MSG: manipulation_msgs/Grasp
00077 # A name for this grasp
00078 string id
00079
00080 # The internal posture of the hand for the pre-grasp
00081 # only positions are used
00082 sensor_msgs/JointState pre_grasp_posture
00083
00084 # The internal posture of the hand for the grasp
00085 # positions and efforts are used
00086 sensor_msgs/JointState grasp_posture
00087
00088 # The position of the end-effector for the grasp relative to a reference frame
00089 # (that is always specified elsewhere, not in this message)
00090 geometry_msgs/PoseStamped grasp_pose
00091
00092 # The estimated probability of success for this grasp, or some other
00093 # measure of how "good" it is.
00094 float64 grasp_quality
00095
00096 # The approach motion
00097 GripperTranslation approach
00098
00099 # The retreat motion
00100 GripperTranslation retreat
00101
00102 # the maximum contact force to use while grasping (<=0 to disable)
00103 float32 max_contact_force
00104
00105 # an optional list of obstacles that we have semantic information about
00106 # and that can be touched/pushed/moved in the course of grasping
00107 string[] allowed_touch_objects
00108
00109 ================================================================================
00110 MSG: sensor_msgs/JointState
00111 # This is a message that holds data to describe the state of a set of torque controlled joints.
00112 #
00113 # The state of each joint (revolute or prismatic) is defined by:
00114 # * the position of the joint (rad or m),
00115 # * the velocity of the joint (rad/s or m/s) and
00116 # * the effort that is applied in the joint (Nm or N).
00117 #
00118 # Each joint is uniquely identified by its name
00119 # The header specifies the time at which the joint states were recorded. All the joint states
00120 # in one message have to be recorded at the same time.
00121 #
00122 # This message consists of a multiple arrays, one for each part of the joint state.
00123 # The goal is to make each of the fields optional. When e.g. your joints have no
00124 # effort associated with them, you can leave the effort array empty.
00125 #
00126 # All arrays in this message should have the same size, or be empty.
00127 # This is the only way to uniquely associate the joint name with the correct
00128 # states.
00129
00130
00131 Header header
00132
00133 string[] name
00134 float64[] position
00135 float64[] velocity
00136 float64[] effort
00137
00138 ================================================================================
00139 MSG: std_msgs/Header
00140 # Standard metadata for higher-level stamped data types.
00141 # This is generally used to communicate timestamped data
00142 # in a particular coordinate frame.
00143 #
00144 # sequence ID: consecutively increasing ID
00145 uint32 seq
00146 #Two-integer timestamp that is expressed as:
00147 # * stamp.secs: seconds (stamp_secs) since epoch
00148 # * stamp.nsecs: nanoseconds since stamp_secs
00149 # time-handling sugar is provided by the client library
00150 time stamp
00151 #Frame this data is associated with
00152 # 0: no frame
00153 # 1: global frame
00154 string frame_id
00155
00156 ================================================================================
00157 MSG: geometry_msgs/PoseStamped
00158 # A Pose with reference coordinate frame and timestamp
00159 Header header
00160 Pose pose
00161
00162 ================================================================================
00163 MSG: geometry_msgs/Pose
00164 # A representation of pose in free space, composed of postion and orientation.
00165 Point position
00166 Quaternion orientation
00167
00168 ================================================================================
00169 MSG: geometry_msgs/Point
00170 # This contains the position of a point in free space
00171 float64 x
00172 float64 y
00173 float64 z
00174
00175 ================================================================================
00176 MSG: geometry_msgs/Quaternion
00177 # This represents an orientation in free space in quaternion form.
00178
00179 float64 x
00180 float64 y
00181 float64 z
00182 float64 w
00183
00184 ================================================================================
00185 MSG: manipulation_msgs/GripperTranslation
00186 # defines a translation for the gripper, used in pickup or place tasks
00187 # for example for lifting an object off a table or approaching the table for placing
00188
00189 # the direction of the translation
00190 geometry_msgs/Vector3Stamped direction
00191
00192 # the desired translation distance
00193 float32 desired_distance
00194
00195 # the min distance that must be considered feasible before the
00196 # grasp is even attempted
00197 float32 min_distance
00198
00199 ================================================================================
00200 MSG: geometry_msgs/Vector3Stamped
00201 # This represents a Vector3 with reference coordinate frame and timestamp
00202 Header header
00203 Vector3 vector
00204
00205 ================================================================================
00206 MSG: geometry_msgs/Vector3
00207 # This represents a vector in free space.
00208
00209 float64 x
00210 float64 y
00211 float64 z
00212 ================================================================================
00213 MSG: object_manipulation_msgs/GraspResult
00214 int32 SUCCESS = 1
00215 int32 GRASP_OUT_OF_REACH = 2
00216 int32 GRASP_IN_COLLISION = 3
00217 int32 GRASP_UNFEASIBLE = 4
00218 int32 PREGRASP_OUT_OF_REACH = 5
00219 int32 PREGRASP_IN_COLLISION = 6
00220 int32 PREGRASP_UNFEASIBLE = 7
00221 int32 LIFT_OUT_OF_REACH = 8
00222 int32 LIFT_IN_COLLISION = 9
00223 int32 LIFT_UNFEASIBLE = 10
00224 int32 MOVE_ARM_FAILED = 11
00225 int32 GRASP_FAILED = 12
00226 int32 LIFT_FAILED = 13
00227 int32 RETREAT_FAILED = 14
00228 int32 result_code
00229
00230 # whether the state of the world was disturbed by this attempt. generally, this flag
00231 # shows if another task can be attempted, or a new sensed world model is recommeded
00232 # before proceeding
00233 bool continuation_possible
00234
00235 """
00236 __slots__ = ['manipulation_result','grasp','attempted_grasps','attempted_grasp_results']
00237 _slot_types = ['object_manipulation_msgs/ManipulationResult','manipulation_msgs/Grasp','manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspResult[]']
00238
00239 def __init__(self, *args, **kwds):
00240 """
00241 Constructor. Any message fields that are implicitly/explicitly
00242 set to None will be assigned a default value. The recommend
00243 use is keyword arguments as this is more robust to future message
00244 changes. You cannot mix in-order arguments and keyword arguments.
00245
00246 The available fields are:
00247 manipulation_result,grasp,attempted_grasps,attempted_grasp_results
00248
00249 :param args: complete set of field values, in .msg order
00250 :param kwds: use keyword arguments corresponding to message field names
00251 to set specific fields.
00252 """
00253 if args or kwds:
00254 super(PickupResult, self).__init__(*args, **kwds)
00255
00256 if self.manipulation_result is None:
00257 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00258 if self.grasp is None:
00259 self.grasp = manipulation_msgs.msg.Grasp()
00260 if self.attempted_grasps is None:
00261 self.attempted_grasps = []
00262 if self.attempted_grasp_results is None:
00263 self.attempted_grasp_results = []
00264 else:
00265 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00266 self.grasp = manipulation_msgs.msg.Grasp()
00267 self.attempted_grasps = []
00268 self.attempted_grasp_results = []
00269
00270 def _get_types(self):
00271 """
00272 internal API method
00273 """
00274 return self._slot_types
00275
00276 def serialize(self, buff):
00277 """
00278 serialize message into buffer
00279 :param buff: buffer, ``StringIO``
00280 """
00281 try:
00282 buff.write(_struct_i.pack(self.manipulation_result.value))
00283 _x = self.grasp.id
00284 length = len(_x)
00285 if python3 or type(_x) == unicode:
00286 _x = _x.encode('utf-8')
00287 length = len(_x)
00288 buff.write(struct.pack('<I%ss'%length, length, _x))
00289 _x = self
00290 buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00291 _x = self.grasp.pre_grasp_posture.header.frame_id
00292 length = len(_x)
00293 if python3 or type(_x) == unicode:
00294 _x = _x.encode('utf-8')
00295 length = len(_x)
00296 buff.write(struct.pack('<I%ss'%length, length, _x))
00297 length = len(self.grasp.pre_grasp_posture.name)
00298 buff.write(_struct_I.pack(length))
00299 for val1 in self.grasp.pre_grasp_posture.name:
00300 length = len(val1)
00301 if python3 or type(val1) == unicode:
00302 val1 = val1.encode('utf-8')
00303 length = len(val1)
00304 buff.write(struct.pack('<I%ss'%length, length, val1))
00305 length = len(self.grasp.pre_grasp_posture.position)
00306 buff.write(_struct_I.pack(length))
00307 pattern = '<%sd'%length
00308 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00309 length = len(self.grasp.pre_grasp_posture.velocity)
00310 buff.write(_struct_I.pack(length))
00311 pattern = '<%sd'%length
00312 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00313 length = len(self.grasp.pre_grasp_posture.effort)
00314 buff.write(_struct_I.pack(length))
00315 pattern = '<%sd'%length
00316 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00317 _x = self
00318 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))
00319 _x = self.grasp.grasp_posture.header.frame_id
00320 length = len(_x)
00321 if python3 or type(_x) == unicode:
00322 _x = _x.encode('utf-8')
00323 length = len(_x)
00324 buff.write(struct.pack('<I%ss'%length, length, _x))
00325 length = len(self.grasp.grasp_posture.name)
00326 buff.write(_struct_I.pack(length))
00327 for val1 in self.grasp.grasp_posture.name:
00328 length = len(val1)
00329 if python3 or type(val1) == unicode:
00330 val1 = val1.encode('utf-8')
00331 length = len(val1)
00332 buff.write(struct.pack('<I%ss'%length, length, val1))
00333 length = len(self.grasp.grasp_posture.position)
00334 buff.write(_struct_I.pack(length))
00335 pattern = '<%sd'%length
00336 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00337 length = len(self.grasp.grasp_posture.velocity)
00338 buff.write(_struct_I.pack(length))
00339 pattern = '<%sd'%length
00340 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00341 length = len(self.grasp.grasp_posture.effort)
00342 buff.write(_struct_I.pack(length))
00343 pattern = '<%sd'%length
00344 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00345 _x = self
00346 buff.write(_struct_3I.pack(_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs))
00347 _x = self.grasp.grasp_pose.header.frame_id
00348 length = len(_x)
00349 if python3 or type(_x) == unicode:
00350 _x = _x.encode('utf-8')
00351 length = len(_x)
00352 buff.write(struct.pack('<I%ss'%length, length, _x))
00353 _x = self
00354 buff.write(_struct_8d3I.pack(_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs))
00355 _x = self.grasp.approach.direction.header.frame_id
00356 length = len(_x)
00357 if python3 or type(_x) == unicode:
00358 _x = _x.encode('utf-8')
00359 length = len(_x)
00360 buff.write(struct.pack('<I%ss'%length, length, _x))
00361 _x = self
00362 buff.write(_struct_3d2f3I.pack(_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs))
00363 _x = self.grasp.retreat.direction.header.frame_id
00364 length = len(_x)
00365 if python3 or type(_x) == unicode:
00366 _x = _x.encode('utf-8')
00367 length = len(_x)
00368 buff.write(struct.pack('<I%ss'%length, length, _x))
00369 _x = self
00370 buff.write(_struct_3d3f.pack(_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force))
00371 length = len(self.grasp.allowed_touch_objects)
00372 buff.write(_struct_I.pack(length))
00373 for val1 in self.grasp.allowed_touch_objects:
00374 length = len(val1)
00375 if python3 or type(val1) == unicode:
00376 val1 = val1.encode('utf-8')
00377 length = len(val1)
00378 buff.write(struct.pack('<I%ss'%length, length, val1))
00379 length = len(self.attempted_grasps)
00380 buff.write(_struct_I.pack(length))
00381 for val1 in self.attempted_grasps:
00382 _x = val1.id
00383 length = len(_x)
00384 if python3 or type(_x) == unicode:
00385 _x = _x.encode('utf-8')
00386 length = len(_x)
00387 buff.write(struct.pack('<I%ss'%length, length, _x))
00388 _v1 = val1.pre_grasp_posture
00389 _v2 = _v1.header
00390 buff.write(_struct_I.pack(_v2.seq))
00391 _v3 = _v2.stamp
00392 _x = _v3
00393 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00394 _x = _v2.frame_id
00395 length = len(_x)
00396 if python3 or type(_x) == unicode:
00397 _x = _x.encode('utf-8')
00398 length = len(_x)
00399 buff.write(struct.pack('<I%ss'%length, length, _x))
00400 length = len(_v1.name)
00401 buff.write(_struct_I.pack(length))
00402 for val3 in _v1.name:
00403 length = len(val3)
00404 if python3 or type(val3) == unicode:
00405 val3 = val3.encode('utf-8')
00406 length = len(val3)
00407 buff.write(struct.pack('<I%ss'%length, length, val3))
00408 length = len(_v1.position)
00409 buff.write(_struct_I.pack(length))
00410 pattern = '<%sd'%length
00411 buff.write(struct.pack(pattern, *_v1.position))
00412 length = len(_v1.velocity)
00413 buff.write(_struct_I.pack(length))
00414 pattern = '<%sd'%length
00415 buff.write(struct.pack(pattern, *_v1.velocity))
00416 length = len(_v1.effort)
00417 buff.write(_struct_I.pack(length))
00418 pattern = '<%sd'%length
00419 buff.write(struct.pack(pattern, *_v1.effort))
00420 _v4 = val1.grasp_posture
00421 _v5 = _v4.header
00422 buff.write(_struct_I.pack(_v5.seq))
00423 _v6 = _v5.stamp
00424 _x = _v6
00425 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00426 _x = _v5.frame_id
00427 length = len(_x)
00428 if python3 or type(_x) == unicode:
00429 _x = _x.encode('utf-8')
00430 length = len(_x)
00431 buff.write(struct.pack('<I%ss'%length, length, _x))
00432 length = len(_v4.name)
00433 buff.write(_struct_I.pack(length))
00434 for val3 in _v4.name:
00435 length = len(val3)
00436 if python3 or type(val3) == unicode:
00437 val3 = val3.encode('utf-8')
00438 length = len(val3)
00439 buff.write(struct.pack('<I%ss'%length, length, val3))
00440 length = len(_v4.position)
00441 buff.write(_struct_I.pack(length))
00442 pattern = '<%sd'%length
00443 buff.write(struct.pack(pattern, *_v4.position))
00444 length = len(_v4.velocity)
00445 buff.write(_struct_I.pack(length))
00446 pattern = '<%sd'%length
00447 buff.write(struct.pack(pattern, *_v4.velocity))
00448 length = len(_v4.effort)
00449 buff.write(_struct_I.pack(length))
00450 pattern = '<%sd'%length
00451 buff.write(struct.pack(pattern, *_v4.effort))
00452 _v7 = val1.grasp_pose
00453 _v8 = _v7.header
00454 buff.write(_struct_I.pack(_v8.seq))
00455 _v9 = _v8.stamp
00456 _x = _v9
00457 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00458 _x = _v8.frame_id
00459 length = len(_x)
00460 if python3 or type(_x) == unicode:
00461 _x = _x.encode('utf-8')
00462 length = len(_x)
00463 buff.write(struct.pack('<I%ss'%length, length, _x))
00464 _v10 = _v7.pose
00465 _v11 = _v10.position
00466 _x = _v11
00467 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00468 _v12 = _v10.orientation
00469 _x = _v12
00470 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00471 buff.write(_struct_d.pack(val1.grasp_quality))
00472 _v13 = val1.approach
00473 _v14 = _v13.direction
00474 _v15 = _v14.header
00475 buff.write(_struct_I.pack(_v15.seq))
00476 _v16 = _v15.stamp
00477 _x = _v16
00478 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00479 _x = _v15.frame_id
00480 length = len(_x)
00481 if python3 or type(_x) == unicode:
00482 _x = _x.encode('utf-8')
00483 length = len(_x)
00484 buff.write(struct.pack('<I%ss'%length, length, _x))
00485 _v17 = _v14.vector
00486 _x = _v17
00487 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00488 _x = _v13
00489 buff.write(_struct_2f.pack(_x.desired_distance, _x.min_distance))
00490 _v18 = val1.retreat
00491 _v19 = _v18.direction
00492 _v20 = _v19.header
00493 buff.write(_struct_I.pack(_v20.seq))
00494 _v21 = _v20.stamp
00495 _x = _v21
00496 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00497 _x = _v20.frame_id
00498 length = len(_x)
00499 if python3 or type(_x) == unicode:
00500 _x = _x.encode('utf-8')
00501 length = len(_x)
00502 buff.write(struct.pack('<I%ss'%length, length, _x))
00503 _v22 = _v19.vector
00504 _x = _v22
00505 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00506 _x = _v18
00507 buff.write(_struct_2f.pack(_x.desired_distance, _x.min_distance))
00508 buff.write(_struct_f.pack(val1.max_contact_force))
00509 length = len(val1.allowed_touch_objects)
00510 buff.write(_struct_I.pack(length))
00511 for val2 in val1.allowed_touch_objects:
00512 length = len(val2)
00513 if python3 or type(val2) == unicode:
00514 val2 = val2.encode('utf-8')
00515 length = len(val2)
00516 buff.write(struct.pack('<I%ss'%length, length, val2))
00517 length = len(self.attempted_grasp_results)
00518 buff.write(_struct_I.pack(length))
00519 for val1 in self.attempted_grasp_results:
00520 _x = val1
00521 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
00522 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00523 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00524
00525 def deserialize(self, str):
00526 """
00527 unpack serialized message in str into this message instance
00528 :param str: byte array of serialized message, ``str``
00529 """
00530 try:
00531 if self.manipulation_result is None:
00532 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00533 if self.grasp is None:
00534 self.grasp = manipulation_msgs.msg.Grasp()
00535 if self.attempted_grasps is None:
00536 self.attempted_grasps = None
00537 if self.attempted_grasp_results is None:
00538 self.attempted_grasp_results = None
00539 end = 0
00540 start = end
00541 end += 4
00542 (self.manipulation_result.value,) = _struct_i.unpack(str[start:end])
00543 start = end
00544 end += 4
00545 (length,) = _struct_I.unpack(str[start:end])
00546 start = end
00547 end += length
00548 if python3:
00549 self.grasp.id = str[start:end].decode('utf-8')
00550 else:
00551 self.grasp.id = str[start:end]
00552 _x = self
00553 start = end
00554 end += 12
00555 (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00556 start = end
00557 end += 4
00558 (length,) = _struct_I.unpack(str[start:end])
00559 start = end
00560 end += length
00561 if python3:
00562 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00563 else:
00564 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00565 start = end
00566 end += 4
00567 (length,) = _struct_I.unpack(str[start:end])
00568 self.grasp.pre_grasp_posture.name = []
00569 for i in range(0, length):
00570 start = end
00571 end += 4
00572 (length,) = _struct_I.unpack(str[start:end])
00573 start = end
00574 end += length
00575 if python3:
00576 val1 = str[start:end].decode('utf-8')
00577 else:
00578 val1 = str[start:end]
00579 self.grasp.pre_grasp_posture.name.append(val1)
00580 start = end
00581 end += 4
00582 (length,) = _struct_I.unpack(str[start:end])
00583 pattern = '<%sd'%length
00584 start = end
00585 end += struct.calcsize(pattern)
00586 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
00587 start = end
00588 end += 4
00589 (length,) = _struct_I.unpack(str[start:end])
00590 pattern = '<%sd'%length
00591 start = end
00592 end += struct.calcsize(pattern)
00593 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00594 start = end
00595 end += 4
00596 (length,) = _struct_I.unpack(str[start:end])
00597 pattern = '<%sd'%length
00598 start = end
00599 end += struct.calcsize(pattern)
00600 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
00601 _x = self
00602 start = end
00603 end += 12
00604 (_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])
00605 start = end
00606 end += 4
00607 (length,) = _struct_I.unpack(str[start:end])
00608 start = end
00609 end += length
00610 if python3:
00611 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00612 else:
00613 self.grasp.grasp_posture.header.frame_id = str[start:end]
00614 start = end
00615 end += 4
00616 (length,) = _struct_I.unpack(str[start:end])
00617 self.grasp.grasp_posture.name = []
00618 for i in range(0, length):
00619 start = end
00620 end += 4
00621 (length,) = _struct_I.unpack(str[start:end])
00622 start = end
00623 end += length
00624 if python3:
00625 val1 = str[start:end].decode('utf-8')
00626 else:
00627 val1 = str[start:end]
00628 self.grasp.grasp_posture.name.append(val1)
00629 start = end
00630 end += 4
00631 (length,) = _struct_I.unpack(str[start:end])
00632 pattern = '<%sd'%length
00633 start = end
00634 end += struct.calcsize(pattern)
00635 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
00636 start = end
00637 end += 4
00638 (length,) = _struct_I.unpack(str[start:end])
00639 pattern = '<%sd'%length
00640 start = end
00641 end += struct.calcsize(pattern)
00642 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00643 start = end
00644 end += 4
00645 (length,) = _struct_I.unpack(str[start:end])
00646 pattern = '<%sd'%length
00647 start = end
00648 end += struct.calcsize(pattern)
00649 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
00650 _x = self
00651 start = end
00652 end += 12
00653 (_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00654 start = end
00655 end += 4
00656 (length,) = _struct_I.unpack(str[start:end])
00657 start = end
00658 end += length
00659 if python3:
00660 self.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
00661 else:
00662 self.grasp.grasp_pose.header.frame_id = str[start:end]
00663 _x = self
00664 start = end
00665 end += 76
00666 (_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
00667 start = end
00668 end += 4
00669 (length,) = _struct_I.unpack(str[start:end])
00670 start = end
00671 end += length
00672 if python3:
00673 self.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
00674 else:
00675 self.grasp.approach.direction.header.frame_id = str[start:end]
00676 _x = self
00677 start = end
00678 end += 44
00679 (_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
00680 start = end
00681 end += 4
00682 (length,) = _struct_I.unpack(str[start:end])
00683 start = end
00684 end += length
00685 if python3:
00686 self.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
00687 else:
00688 self.grasp.retreat.direction.header.frame_id = str[start:end]
00689 _x = self
00690 start = end
00691 end += 36
00692 (_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
00693 start = end
00694 end += 4
00695 (length,) = _struct_I.unpack(str[start:end])
00696 self.grasp.allowed_touch_objects = []
00697 for i in range(0, length):
00698 start = end
00699 end += 4
00700 (length,) = _struct_I.unpack(str[start:end])
00701 start = end
00702 end += length
00703 if python3:
00704 val1 = str[start:end].decode('utf-8')
00705 else:
00706 val1 = str[start:end]
00707 self.grasp.allowed_touch_objects.append(val1)
00708 start = end
00709 end += 4
00710 (length,) = _struct_I.unpack(str[start:end])
00711 self.attempted_grasps = []
00712 for i in range(0, length):
00713 val1 = manipulation_msgs.msg.Grasp()
00714 start = end
00715 end += 4
00716 (length,) = _struct_I.unpack(str[start:end])
00717 start = end
00718 end += length
00719 if python3:
00720 val1.id = str[start:end].decode('utf-8')
00721 else:
00722 val1.id = str[start:end]
00723 _v23 = val1.pre_grasp_posture
00724 _v24 = _v23.header
00725 start = end
00726 end += 4
00727 (_v24.seq,) = _struct_I.unpack(str[start:end])
00728 _v25 = _v24.stamp
00729 _x = _v25
00730 start = end
00731 end += 8
00732 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00733 start = end
00734 end += 4
00735 (length,) = _struct_I.unpack(str[start:end])
00736 start = end
00737 end += length
00738 if python3:
00739 _v24.frame_id = str[start:end].decode('utf-8')
00740 else:
00741 _v24.frame_id = str[start:end]
00742 start = end
00743 end += 4
00744 (length,) = _struct_I.unpack(str[start:end])
00745 _v23.name = []
00746 for i in range(0, length):
00747 start = end
00748 end += 4
00749 (length,) = _struct_I.unpack(str[start:end])
00750 start = end
00751 end += length
00752 if python3:
00753 val3 = str[start:end].decode('utf-8')
00754 else:
00755 val3 = str[start:end]
00756 _v23.name.append(val3)
00757 start = end
00758 end += 4
00759 (length,) = _struct_I.unpack(str[start:end])
00760 pattern = '<%sd'%length
00761 start = end
00762 end += struct.calcsize(pattern)
00763 _v23.position = struct.unpack(pattern, str[start:end])
00764 start = end
00765 end += 4
00766 (length,) = _struct_I.unpack(str[start:end])
00767 pattern = '<%sd'%length
00768 start = end
00769 end += struct.calcsize(pattern)
00770 _v23.velocity = struct.unpack(pattern, str[start:end])
00771 start = end
00772 end += 4
00773 (length,) = _struct_I.unpack(str[start:end])
00774 pattern = '<%sd'%length
00775 start = end
00776 end += struct.calcsize(pattern)
00777 _v23.effort = struct.unpack(pattern, str[start:end])
00778 _v26 = val1.grasp_posture
00779 _v27 = _v26.header
00780 start = end
00781 end += 4
00782 (_v27.seq,) = _struct_I.unpack(str[start:end])
00783 _v28 = _v27.stamp
00784 _x = _v28
00785 start = end
00786 end += 8
00787 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00788 start = end
00789 end += 4
00790 (length,) = _struct_I.unpack(str[start:end])
00791 start = end
00792 end += length
00793 if python3:
00794 _v27.frame_id = str[start:end].decode('utf-8')
00795 else:
00796 _v27.frame_id = str[start:end]
00797 start = end
00798 end += 4
00799 (length,) = _struct_I.unpack(str[start:end])
00800 _v26.name = []
00801 for i in range(0, length):
00802 start = end
00803 end += 4
00804 (length,) = _struct_I.unpack(str[start:end])
00805 start = end
00806 end += length
00807 if python3:
00808 val3 = str[start:end].decode('utf-8')
00809 else:
00810 val3 = str[start:end]
00811 _v26.name.append(val3)
00812 start = end
00813 end += 4
00814 (length,) = _struct_I.unpack(str[start:end])
00815 pattern = '<%sd'%length
00816 start = end
00817 end += struct.calcsize(pattern)
00818 _v26.position = struct.unpack(pattern, str[start:end])
00819 start = end
00820 end += 4
00821 (length,) = _struct_I.unpack(str[start:end])
00822 pattern = '<%sd'%length
00823 start = end
00824 end += struct.calcsize(pattern)
00825 _v26.velocity = struct.unpack(pattern, str[start:end])
00826 start = end
00827 end += 4
00828 (length,) = _struct_I.unpack(str[start:end])
00829 pattern = '<%sd'%length
00830 start = end
00831 end += struct.calcsize(pattern)
00832 _v26.effort = struct.unpack(pattern, str[start:end])
00833 _v29 = val1.grasp_pose
00834 _v30 = _v29.header
00835 start = end
00836 end += 4
00837 (_v30.seq,) = _struct_I.unpack(str[start:end])
00838 _v31 = _v30.stamp
00839 _x = _v31
00840 start = end
00841 end += 8
00842 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00843 start = end
00844 end += 4
00845 (length,) = _struct_I.unpack(str[start:end])
00846 start = end
00847 end += length
00848 if python3:
00849 _v30.frame_id = str[start:end].decode('utf-8')
00850 else:
00851 _v30.frame_id = str[start:end]
00852 _v32 = _v29.pose
00853 _v33 = _v32.position
00854 _x = _v33
00855 start = end
00856 end += 24
00857 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00858 _v34 = _v32.orientation
00859 _x = _v34
00860 start = end
00861 end += 32
00862 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00863 start = end
00864 end += 8
00865 (val1.grasp_quality,) = _struct_d.unpack(str[start:end])
00866 _v35 = val1.approach
00867 _v36 = _v35.direction
00868 _v37 = _v36.header
00869 start = end
00870 end += 4
00871 (_v37.seq,) = _struct_I.unpack(str[start:end])
00872 _v38 = _v37.stamp
00873 _x = _v38
00874 start = end
00875 end += 8
00876 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00877 start = end
00878 end += 4
00879 (length,) = _struct_I.unpack(str[start:end])
00880 start = end
00881 end += length
00882 if python3:
00883 _v37.frame_id = str[start:end].decode('utf-8')
00884 else:
00885 _v37.frame_id = str[start:end]
00886 _v39 = _v36.vector
00887 _x = _v39
00888 start = end
00889 end += 24
00890 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00891 _x = _v35
00892 start = end
00893 end += 8
00894 (_x.desired_distance, _x.min_distance,) = _struct_2f.unpack(str[start:end])
00895 _v40 = val1.retreat
00896 _v41 = _v40.direction
00897 _v42 = _v41.header
00898 start = end
00899 end += 4
00900 (_v42.seq,) = _struct_I.unpack(str[start:end])
00901 _v43 = _v42.stamp
00902 _x = _v43
00903 start = end
00904 end += 8
00905 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00906 start = end
00907 end += 4
00908 (length,) = _struct_I.unpack(str[start:end])
00909 start = end
00910 end += length
00911 if python3:
00912 _v42.frame_id = str[start:end].decode('utf-8')
00913 else:
00914 _v42.frame_id = str[start:end]
00915 _v44 = _v41.vector
00916 _x = _v44
00917 start = end
00918 end += 24
00919 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00920 _x = _v40
00921 start = end
00922 end += 8
00923 (_x.desired_distance, _x.min_distance,) = _struct_2f.unpack(str[start:end])
00924 start = end
00925 end += 4
00926 (val1.max_contact_force,) = _struct_f.unpack(str[start:end])
00927 start = end
00928 end += 4
00929 (length,) = _struct_I.unpack(str[start:end])
00930 val1.allowed_touch_objects = []
00931 for i in range(0, length):
00932 start = end
00933 end += 4
00934 (length,) = _struct_I.unpack(str[start:end])
00935 start = end
00936 end += length
00937 if python3:
00938 val2 = str[start:end].decode('utf-8')
00939 else:
00940 val2 = str[start:end]
00941 val1.allowed_touch_objects.append(val2)
00942 self.attempted_grasps.append(val1)
00943 start = end
00944 end += 4
00945 (length,) = _struct_I.unpack(str[start:end])
00946 self.attempted_grasp_results = []
00947 for i in range(0, length):
00948 val1 = object_manipulation_msgs.msg.GraspResult()
00949 _x = val1
00950 start = end
00951 end += 5
00952 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
00953 val1.continuation_possible = bool(val1.continuation_possible)
00954 self.attempted_grasp_results.append(val1)
00955 return self
00956 except struct.error as e:
00957 raise genpy.DeserializationError(e)
00958
00959
00960 def serialize_numpy(self, buff, numpy):
00961 """
00962 serialize message with numpy array types into buffer
00963 :param buff: buffer, ``StringIO``
00964 :param numpy: numpy python module
00965 """
00966 try:
00967 buff.write(_struct_i.pack(self.manipulation_result.value))
00968 _x = self.grasp.id
00969 length = len(_x)
00970 if python3 or type(_x) == unicode:
00971 _x = _x.encode('utf-8')
00972 length = len(_x)
00973 buff.write(struct.pack('<I%ss'%length, length, _x))
00974 _x = self
00975 buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00976 _x = self.grasp.pre_grasp_posture.header.frame_id
00977 length = len(_x)
00978 if python3 or type(_x) == unicode:
00979 _x = _x.encode('utf-8')
00980 length = len(_x)
00981 buff.write(struct.pack('<I%ss'%length, length, _x))
00982 length = len(self.grasp.pre_grasp_posture.name)
00983 buff.write(_struct_I.pack(length))
00984 for val1 in self.grasp.pre_grasp_posture.name:
00985 length = len(val1)
00986 if python3 or type(val1) == unicode:
00987 val1 = val1.encode('utf-8')
00988 length = len(val1)
00989 buff.write(struct.pack('<I%ss'%length, length, val1))
00990 length = len(self.grasp.pre_grasp_posture.position)
00991 buff.write(_struct_I.pack(length))
00992 pattern = '<%sd'%length
00993 buff.write(self.grasp.pre_grasp_posture.position.tostring())
00994 length = len(self.grasp.pre_grasp_posture.velocity)
00995 buff.write(_struct_I.pack(length))
00996 pattern = '<%sd'%length
00997 buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
00998 length = len(self.grasp.pre_grasp_posture.effort)
00999 buff.write(_struct_I.pack(length))
01000 pattern = '<%sd'%length
01001 buff.write(self.grasp.pre_grasp_posture.effort.tostring())
01002 _x = self
01003 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))
01004 _x = self.grasp.grasp_posture.header.frame_id
01005 length = len(_x)
01006 if python3 or type(_x) == unicode:
01007 _x = _x.encode('utf-8')
01008 length = len(_x)
01009 buff.write(struct.pack('<I%ss'%length, length, _x))
01010 length = len(self.grasp.grasp_posture.name)
01011 buff.write(_struct_I.pack(length))
01012 for val1 in self.grasp.grasp_posture.name:
01013 length = len(val1)
01014 if python3 or type(val1) == unicode:
01015 val1 = val1.encode('utf-8')
01016 length = len(val1)
01017 buff.write(struct.pack('<I%ss'%length, length, val1))
01018 length = len(self.grasp.grasp_posture.position)
01019 buff.write(_struct_I.pack(length))
01020 pattern = '<%sd'%length
01021 buff.write(self.grasp.grasp_posture.position.tostring())
01022 length = len(self.grasp.grasp_posture.velocity)
01023 buff.write(_struct_I.pack(length))
01024 pattern = '<%sd'%length
01025 buff.write(self.grasp.grasp_posture.velocity.tostring())
01026 length = len(self.grasp.grasp_posture.effort)
01027 buff.write(_struct_I.pack(length))
01028 pattern = '<%sd'%length
01029 buff.write(self.grasp.grasp_posture.effort.tostring())
01030 _x = self
01031 buff.write(_struct_3I.pack(_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs))
01032 _x = self.grasp.grasp_pose.header.frame_id
01033 length = len(_x)
01034 if python3 or type(_x) == unicode:
01035 _x = _x.encode('utf-8')
01036 length = len(_x)
01037 buff.write(struct.pack('<I%ss'%length, length, _x))
01038 _x = self
01039 buff.write(_struct_8d3I.pack(_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs))
01040 _x = self.grasp.approach.direction.header.frame_id
01041 length = len(_x)
01042 if python3 or type(_x) == unicode:
01043 _x = _x.encode('utf-8')
01044 length = len(_x)
01045 buff.write(struct.pack('<I%ss'%length, length, _x))
01046 _x = self
01047 buff.write(_struct_3d2f3I.pack(_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs))
01048 _x = self.grasp.retreat.direction.header.frame_id
01049 length = len(_x)
01050 if python3 or type(_x) == unicode:
01051 _x = _x.encode('utf-8')
01052 length = len(_x)
01053 buff.write(struct.pack('<I%ss'%length, length, _x))
01054 _x = self
01055 buff.write(_struct_3d3f.pack(_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force))
01056 length = len(self.grasp.allowed_touch_objects)
01057 buff.write(_struct_I.pack(length))
01058 for val1 in self.grasp.allowed_touch_objects:
01059 length = len(val1)
01060 if python3 or type(val1) == unicode:
01061 val1 = val1.encode('utf-8')
01062 length = len(val1)
01063 buff.write(struct.pack('<I%ss'%length, length, val1))
01064 length = len(self.attempted_grasps)
01065 buff.write(_struct_I.pack(length))
01066 for val1 in self.attempted_grasps:
01067 _x = val1.id
01068 length = len(_x)
01069 if python3 or type(_x) == unicode:
01070 _x = _x.encode('utf-8')
01071 length = len(_x)
01072 buff.write(struct.pack('<I%ss'%length, length, _x))
01073 _v45 = val1.pre_grasp_posture
01074 _v46 = _v45.header
01075 buff.write(_struct_I.pack(_v46.seq))
01076 _v47 = _v46.stamp
01077 _x = _v47
01078 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01079 _x = _v46.frame_id
01080 length = len(_x)
01081 if python3 or type(_x) == unicode:
01082 _x = _x.encode('utf-8')
01083 length = len(_x)
01084 buff.write(struct.pack('<I%ss'%length, length, _x))
01085 length = len(_v45.name)
01086 buff.write(_struct_I.pack(length))
01087 for val3 in _v45.name:
01088 length = len(val3)
01089 if python3 or type(val3) == unicode:
01090 val3 = val3.encode('utf-8')
01091 length = len(val3)
01092 buff.write(struct.pack('<I%ss'%length, length, val3))
01093 length = len(_v45.position)
01094 buff.write(_struct_I.pack(length))
01095 pattern = '<%sd'%length
01096 buff.write(_v45.position.tostring())
01097 length = len(_v45.velocity)
01098 buff.write(_struct_I.pack(length))
01099 pattern = '<%sd'%length
01100 buff.write(_v45.velocity.tostring())
01101 length = len(_v45.effort)
01102 buff.write(_struct_I.pack(length))
01103 pattern = '<%sd'%length
01104 buff.write(_v45.effort.tostring())
01105 _v48 = val1.grasp_posture
01106 _v49 = _v48.header
01107 buff.write(_struct_I.pack(_v49.seq))
01108 _v50 = _v49.stamp
01109 _x = _v50
01110 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01111 _x = _v49.frame_id
01112 length = len(_x)
01113 if python3 or type(_x) == unicode:
01114 _x = _x.encode('utf-8')
01115 length = len(_x)
01116 buff.write(struct.pack('<I%ss'%length, length, _x))
01117 length = len(_v48.name)
01118 buff.write(_struct_I.pack(length))
01119 for val3 in _v48.name:
01120 length = len(val3)
01121 if python3 or type(val3) == unicode:
01122 val3 = val3.encode('utf-8')
01123 length = len(val3)
01124 buff.write(struct.pack('<I%ss'%length, length, val3))
01125 length = len(_v48.position)
01126 buff.write(_struct_I.pack(length))
01127 pattern = '<%sd'%length
01128 buff.write(_v48.position.tostring())
01129 length = len(_v48.velocity)
01130 buff.write(_struct_I.pack(length))
01131 pattern = '<%sd'%length
01132 buff.write(_v48.velocity.tostring())
01133 length = len(_v48.effort)
01134 buff.write(_struct_I.pack(length))
01135 pattern = '<%sd'%length
01136 buff.write(_v48.effort.tostring())
01137 _v51 = val1.grasp_pose
01138 _v52 = _v51.header
01139 buff.write(_struct_I.pack(_v52.seq))
01140 _v53 = _v52.stamp
01141 _x = _v53
01142 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01143 _x = _v52.frame_id
01144 length = len(_x)
01145 if python3 or type(_x) == unicode:
01146 _x = _x.encode('utf-8')
01147 length = len(_x)
01148 buff.write(struct.pack('<I%ss'%length, length, _x))
01149 _v54 = _v51.pose
01150 _v55 = _v54.position
01151 _x = _v55
01152 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01153 _v56 = _v54.orientation
01154 _x = _v56
01155 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01156 buff.write(_struct_d.pack(val1.grasp_quality))
01157 _v57 = val1.approach
01158 _v58 = _v57.direction
01159 _v59 = _v58.header
01160 buff.write(_struct_I.pack(_v59.seq))
01161 _v60 = _v59.stamp
01162 _x = _v60
01163 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01164 _x = _v59.frame_id
01165 length = len(_x)
01166 if python3 or type(_x) == unicode:
01167 _x = _x.encode('utf-8')
01168 length = len(_x)
01169 buff.write(struct.pack('<I%ss'%length, length, _x))
01170 _v61 = _v58.vector
01171 _x = _v61
01172 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01173 _x = _v57
01174 buff.write(_struct_2f.pack(_x.desired_distance, _x.min_distance))
01175 _v62 = val1.retreat
01176 _v63 = _v62.direction
01177 _v64 = _v63.header
01178 buff.write(_struct_I.pack(_v64.seq))
01179 _v65 = _v64.stamp
01180 _x = _v65
01181 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01182 _x = _v64.frame_id
01183 length = len(_x)
01184 if python3 or type(_x) == unicode:
01185 _x = _x.encode('utf-8')
01186 length = len(_x)
01187 buff.write(struct.pack('<I%ss'%length, length, _x))
01188 _v66 = _v63.vector
01189 _x = _v66
01190 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01191 _x = _v62
01192 buff.write(_struct_2f.pack(_x.desired_distance, _x.min_distance))
01193 buff.write(_struct_f.pack(val1.max_contact_force))
01194 length = len(val1.allowed_touch_objects)
01195 buff.write(_struct_I.pack(length))
01196 for val2 in val1.allowed_touch_objects:
01197 length = len(val2)
01198 if python3 or type(val2) == unicode:
01199 val2 = val2.encode('utf-8')
01200 length = len(val2)
01201 buff.write(struct.pack('<I%ss'%length, length, val2))
01202 length = len(self.attempted_grasp_results)
01203 buff.write(_struct_I.pack(length))
01204 for val1 in self.attempted_grasp_results:
01205 _x = val1
01206 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
01207 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
01208 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
01209
01210 def deserialize_numpy(self, str, numpy):
01211 """
01212 unpack serialized message in str into this message instance using numpy for array types
01213 :param str: byte array of serialized message, ``str``
01214 :param numpy: numpy python module
01215 """
01216 try:
01217 if self.manipulation_result is None:
01218 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
01219 if self.grasp is None:
01220 self.grasp = manipulation_msgs.msg.Grasp()
01221 if self.attempted_grasps is None:
01222 self.attempted_grasps = None
01223 if self.attempted_grasp_results is None:
01224 self.attempted_grasp_results = None
01225 end = 0
01226 start = end
01227 end += 4
01228 (self.manipulation_result.value,) = _struct_i.unpack(str[start:end])
01229 start = end
01230 end += 4
01231 (length,) = _struct_I.unpack(str[start:end])
01232 start = end
01233 end += length
01234 if python3:
01235 self.grasp.id = str[start:end].decode('utf-8')
01236 else:
01237 self.grasp.id = str[start:end]
01238 _x = self
01239 start = end
01240 end += 12
01241 (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01242 start = end
01243 end += 4
01244 (length,) = _struct_I.unpack(str[start:end])
01245 start = end
01246 end += length
01247 if python3:
01248 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01249 else:
01250 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01251 start = end
01252 end += 4
01253 (length,) = _struct_I.unpack(str[start:end])
01254 self.grasp.pre_grasp_posture.name = []
01255 for i in range(0, length):
01256 start = end
01257 end += 4
01258 (length,) = _struct_I.unpack(str[start:end])
01259 start = end
01260 end += length
01261 if python3:
01262 val1 = str[start:end].decode('utf-8')
01263 else:
01264 val1 = str[start:end]
01265 self.grasp.pre_grasp_posture.name.append(val1)
01266 start = end
01267 end += 4
01268 (length,) = _struct_I.unpack(str[start:end])
01269 pattern = '<%sd'%length
01270 start = end
01271 end += struct.calcsize(pattern)
01272 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01273 start = end
01274 end += 4
01275 (length,) = _struct_I.unpack(str[start:end])
01276 pattern = '<%sd'%length
01277 start = end
01278 end += struct.calcsize(pattern)
01279 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01280 start = end
01281 end += 4
01282 (length,) = _struct_I.unpack(str[start:end])
01283 pattern = '<%sd'%length
01284 start = end
01285 end += struct.calcsize(pattern)
01286 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01287 _x = self
01288 start = end
01289 end += 12
01290 (_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])
01291 start = end
01292 end += 4
01293 (length,) = _struct_I.unpack(str[start:end])
01294 start = end
01295 end += length
01296 if python3:
01297 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01298 else:
01299 self.grasp.grasp_posture.header.frame_id = str[start:end]
01300 start = end
01301 end += 4
01302 (length,) = _struct_I.unpack(str[start:end])
01303 self.grasp.grasp_posture.name = []
01304 for i in range(0, length):
01305 start = end
01306 end += 4
01307 (length,) = _struct_I.unpack(str[start:end])
01308 start = end
01309 end += length
01310 if python3:
01311 val1 = str[start:end].decode('utf-8')
01312 else:
01313 val1 = str[start:end]
01314 self.grasp.grasp_posture.name.append(val1)
01315 start = end
01316 end += 4
01317 (length,) = _struct_I.unpack(str[start:end])
01318 pattern = '<%sd'%length
01319 start = end
01320 end += struct.calcsize(pattern)
01321 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01322 start = end
01323 end += 4
01324 (length,) = _struct_I.unpack(str[start:end])
01325 pattern = '<%sd'%length
01326 start = end
01327 end += struct.calcsize(pattern)
01328 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01329 start = end
01330 end += 4
01331 (length,) = _struct_I.unpack(str[start:end])
01332 pattern = '<%sd'%length
01333 start = end
01334 end += struct.calcsize(pattern)
01335 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01336 _x = self
01337 start = end
01338 end += 12
01339 (_x.grasp.grasp_pose.header.seq, _x.grasp.grasp_pose.header.stamp.secs, _x.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01340 start = end
01341 end += 4
01342 (length,) = _struct_I.unpack(str[start:end])
01343 start = end
01344 end += length
01345 if python3:
01346 self.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
01347 else:
01348 self.grasp.grasp_pose.header.frame_id = str[start:end]
01349 _x = self
01350 start = end
01351 end += 76
01352 (_x.grasp.grasp_pose.pose.position.x, _x.grasp.grasp_pose.pose.position.y, _x.grasp.grasp_pose.pose.position.z, _x.grasp.grasp_pose.pose.orientation.x, _x.grasp.grasp_pose.pose.orientation.y, _x.grasp.grasp_pose.pose.orientation.z, _x.grasp.grasp_pose.pose.orientation.w, _x.grasp.grasp_quality, _x.grasp.approach.direction.header.seq, _x.grasp.approach.direction.header.stamp.secs, _x.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
01353 start = end
01354 end += 4
01355 (length,) = _struct_I.unpack(str[start:end])
01356 start = end
01357 end += length
01358 if python3:
01359 self.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01360 else:
01361 self.grasp.approach.direction.header.frame_id = str[start:end]
01362 _x = self
01363 start = end
01364 end += 44
01365 (_x.grasp.approach.direction.vector.x, _x.grasp.approach.direction.vector.y, _x.grasp.approach.direction.vector.z, _x.grasp.approach.desired_distance, _x.grasp.approach.min_distance, _x.grasp.retreat.direction.header.seq, _x.grasp.retreat.direction.header.stamp.secs, _x.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
01366 start = end
01367 end += 4
01368 (length,) = _struct_I.unpack(str[start:end])
01369 start = end
01370 end += length
01371 if python3:
01372 self.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
01373 else:
01374 self.grasp.retreat.direction.header.frame_id = str[start:end]
01375 _x = self
01376 start = end
01377 end += 36
01378 (_x.grasp.retreat.direction.vector.x, _x.grasp.retreat.direction.vector.y, _x.grasp.retreat.direction.vector.z, _x.grasp.retreat.desired_distance, _x.grasp.retreat.min_distance, _x.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
01379 start = end
01380 end += 4
01381 (length,) = _struct_I.unpack(str[start:end])
01382 self.grasp.allowed_touch_objects = []
01383 for i in range(0, length):
01384 start = end
01385 end += 4
01386 (length,) = _struct_I.unpack(str[start:end])
01387 start = end
01388 end += length
01389 if python3:
01390 val1 = str[start:end].decode('utf-8')
01391 else:
01392 val1 = str[start:end]
01393 self.grasp.allowed_touch_objects.append(val1)
01394 start = end
01395 end += 4
01396 (length,) = _struct_I.unpack(str[start:end])
01397 self.attempted_grasps = []
01398 for i in range(0, length):
01399 val1 = manipulation_msgs.msg.Grasp()
01400 start = end
01401 end += 4
01402 (length,) = _struct_I.unpack(str[start:end])
01403 start = end
01404 end += length
01405 if python3:
01406 val1.id = str[start:end].decode('utf-8')
01407 else:
01408 val1.id = str[start:end]
01409 _v67 = val1.pre_grasp_posture
01410 _v68 = _v67.header
01411 start = end
01412 end += 4
01413 (_v68.seq,) = _struct_I.unpack(str[start:end])
01414 _v69 = _v68.stamp
01415 _x = _v69
01416 start = end
01417 end += 8
01418 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01419 start = end
01420 end += 4
01421 (length,) = _struct_I.unpack(str[start:end])
01422 start = end
01423 end += length
01424 if python3:
01425 _v68.frame_id = str[start:end].decode('utf-8')
01426 else:
01427 _v68.frame_id = str[start:end]
01428 start = end
01429 end += 4
01430 (length,) = _struct_I.unpack(str[start:end])
01431 _v67.name = []
01432 for i in range(0, length):
01433 start = end
01434 end += 4
01435 (length,) = _struct_I.unpack(str[start:end])
01436 start = end
01437 end += length
01438 if python3:
01439 val3 = str[start:end].decode('utf-8')
01440 else:
01441 val3 = str[start:end]
01442 _v67.name.append(val3)
01443 start = end
01444 end += 4
01445 (length,) = _struct_I.unpack(str[start:end])
01446 pattern = '<%sd'%length
01447 start = end
01448 end += struct.calcsize(pattern)
01449 _v67.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01450 start = end
01451 end += 4
01452 (length,) = _struct_I.unpack(str[start:end])
01453 pattern = '<%sd'%length
01454 start = end
01455 end += struct.calcsize(pattern)
01456 _v67.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01457 start = end
01458 end += 4
01459 (length,) = _struct_I.unpack(str[start:end])
01460 pattern = '<%sd'%length
01461 start = end
01462 end += struct.calcsize(pattern)
01463 _v67.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01464 _v70 = val1.grasp_posture
01465 _v71 = _v70.header
01466 start = end
01467 end += 4
01468 (_v71.seq,) = _struct_I.unpack(str[start:end])
01469 _v72 = _v71.stamp
01470 _x = _v72
01471 start = end
01472 end += 8
01473 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01474 start = end
01475 end += 4
01476 (length,) = _struct_I.unpack(str[start:end])
01477 start = end
01478 end += length
01479 if python3:
01480 _v71.frame_id = str[start:end].decode('utf-8')
01481 else:
01482 _v71.frame_id = str[start:end]
01483 start = end
01484 end += 4
01485 (length,) = _struct_I.unpack(str[start:end])
01486 _v70.name = []
01487 for i in range(0, length):
01488 start = end
01489 end += 4
01490 (length,) = _struct_I.unpack(str[start:end])
01491 start = end
01492 end += length
01493 if python3:
01494 val3 = str[start:end].decode('utf-8')
01495 else:
01496 val3 = str[start:end]
01497 _v70.name.append(val3)
01498 start = end
01499 end += 4
01500 (length,) = _struct_I.unpack(str[start:end])
01501 pattern = '<%sd'%length
01502 start = end
01503 end += struct.calcsize(pattern)
01504 _v70.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01505 start = end
01506 end += 4
01507 (length,) = _struct_I.unpack(str[start:end])
01508 pattern = '<%sd'%length
01509 start = end
01510 end += struct.calcsize(pattern)
01511 _v70.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01512 start = end
01513 end += 4
01514 (length,) = _struct_I.unpack(str[start:end])
01515 pattern = '<%sd'%length
01516 start = end
01517 end += struct.calcsize(pattern)
01518 _v70.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01519 _v73 = val1.grasp_pose
01520 _v74 = _v73.header
01521 start = end
01522 end += 4
01523 (_v74.seq,) = _struct_I.unpack(str[start:end])
01524 _v75 = _v74.stamp
01525 _x = _v75
01526 start = end
01527 end += 8
01528 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01529 start = end
01530 end += 4
01531 (length,) = _struct_I.unpack(str[start:end])
01532 start = end
01533 end += length
01534 if python3:
01535 _v74.frame_id = str[start:end].decode('utf-8')
01536 else:
01537 _v74.frame_id = str[start:end]
01538 _v76 = _v73.pose
01539 _v77 = _v76.position
01540 _x = _v77
01541 start = end
01542 end += 24
01543 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01544 _v78 = _v76.orientation
01545 _x = _v78
01546 start = end
01547 end += 32
01548 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01549 start = end
01550 end += 8
01551 (val1.grasp_quality,) = _struct_d.unpack(str[start:end])
01552 _v79 = val1.approach
01553 _v80 = _v79.direction
01554 _v81 = _v80.header
01555 start = end
01556 end += 4
01557 (_v81.seq,) = _struct_I.unpack(str[start:end])
01558 _v82 = _v81.stamp
01559 _x = _v82
01560 start = end
01561 end += 8
01562 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01563 start = end
01564 end += 4
01565 (length,) = _struct_I.unpack(str[start:end])
01566 start = end
01567 end += length
01568 if python3:
01569 _v81.frame_id = str[start:end].decode('utf-8')
01570 else:
01571 _v81.frame_id = str[start:end]
01572 _v83 = _v80.vector
01573 _x = _v83
01574 start = end
01575 end += 24
01576 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01577 _x = _v79
01578 start = end
01579 end += 8
01580 (_x.desired_distance, _x.min_distance,) = _struct_2f.unpack(str[start:end])
01581 _v84 = val1.retreat
01582 _v85 = _v84.direction
01583 _v86 = _v85.header
01584 start = end
01585 end += 4
01586 (_v86.seq,) = _struct_I.unpack(str[start:end])
01587 _v87 = _v86.stamp
01588 _x = _v87
01589 start = end
01590 end += 8
01591 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01592 start = end
01593 end += 4
01594 (length,) = _struct_I.unpack(str[start:end])
01595 start = end
01596 end += length
01597 if python3:
01598 _v86.frame_id = str[start:end].decode('utf-8')
01599 else:
01600 _v86.frame_id = str[start:end]
01601 _v88 = _v85.vector
01602 _x = _v88
01603 start = end
01604 end += 24
01605 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01606 _x = _v84
01607 start = end
01608 end += 8
01609 (_x.desired_distance, _x.min_distance,) = _struct_2f.unpack(str[start:end])
01610 start = end
01611 end += 4
01612 (val1.max_contact_force,) = _struct_f.unpack(str[start:end])
01613 start = end
01614 end += 4
01615 (length,) = _struct_I.unpack(str[start:end])
01616 val1.allowed_touch_objects = []
01617 for i in range(0, length):
01618 start = end
01619 end += 4
01620 (length,) = _struct_I.unpack(str[start:end])
01621 start = end
01622 end += length
01623 if python3:
01624 val2 = str[start:end].decode('utf-8')
01625 else:
01626 val2 = str[start:end]
01627 val1.allowed_touch_objects.append(val2)
01628 self.attempted_grasps.append(val1)
01629 start = end
01630 end += 4
01631 (length,) = _struct_I.unpack(str[start:end])
01632 self.attempted_grasp_results = []
01633 for i in range(0, length):
01634 val1 = object_manipulation_msgs.msg.GraspResult()
01635 _x = val1
01636 start = end
01637 end += 5
01638 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
01639 val1.continuation_possible = bool(val1.continuation_possible)
01640 self.attempted_grasp_results.append(val1)
01641 return self
01642 except struct.error as e:
01643 raise genpy.DeserializationError(e)
01644
01645 _struct_I = genpy.struct_I
01646 _struct_d = struct.Struct("<d")
01647 _struct_f = struct.Struct("<f")
01648 _struct_i = struct.Struct("<i")
01649 _struct_2f = struct.Struct("<2f")
01650 _struct_3d3f = struct.Struct("<3d3f")
01651 _struct_8d3I = struct.Struct("<8d3I")
01652 _struct_3I = struct.Struct("<3I")
01653 _struct_3d2f3I = struct.Struct("<3d2f3I")
01654 _struct_4d = struct.Struct("<4d")
01655 _struct_iB = struct.Struct("<iB")
01656 _struct_2I = struct.Struct("<2I")
01657 _struct_3d = struct.Struct("<3d")