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