00001 """autogenerated by genpy from object_manipulation_msgs/PlaceGoal.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 arm_navigation_msgs.msg
00008 import object_manipulation_msgs.msg
00009 import manipulation_msgs.msg
00010 import geometry_msgs.msg
00011 import sensor_msgs.msg
00012 import std_msgs.msg
00013
00014 class PlaceGoal(genpy.Message):
00015 _md5sum = "034b7ae47f0a62d98ce7315be451c829"
00016 _type = "object_manipulation_msgs/PlaceGoal"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019 # An action for placing an object
00020
00021 # which arm to be used for grasping
00022 string arm_name
00023
00024 # a list of possible locations for the gripper when placing
00025 # note that these refer to where the *gripper* should go when the object is placed
00026 # it is the job of the caller to make sure that these are set to that the *object*
00027 # ends up in a desired location
00028 geometry_msgs/PoseStamped[] place_locations
00029
00030 # the grasp that has been executed on this object
00031 # (contains the grasp_pose referred to above)
00032 manipulation_msgs/Grasp grasp
00033
00034 # how far the retreat should ideally be away from the place location
00035 float32 desired_retreat_distance
00036
00037 # the min distance between the retreat and the place location that must actually be feasible
00038 # for the place not to be rejected
00039 float32 min_retreat_distance
00040
00041 # how the place location should be approached
00042 # the frame_id that this lift is specified in MUST be either the robot_frame
00043 # or the gripper_frame specified in your hand description file
00044 GripperTranslation approach
00045
00046 # the name that the target object has in the collision map
00047 # can be left empty if no name is available
00048 string collision_object_name
00049
00050 # the name that the support surface (e.g. table) has in the collision map
00051 # can be left empty if no name is available
00052 string collision_support_surface_name
00053
00054 # whether collisions between the gripper and the support surface should be acceptable
00055 # during move from pre-place to place and during retreat. Collisions when moving to the
00056 # pre-place location are still not allowed even if this is set to true.
00057 bool allow_gripper_support_collision
00058
00059 # whether reactive placing based on tactile sensors should be used
00060 bool use_reactive_place
00061
00062 # how much the object should be padded by when deciding if the grasp
00063 # location is freasible or not
00064 float64 place_padding
00065
00066 # set this to true if you only want to query the manipulation pipeline as to what
00067 # place locations it thinks are feasible, without actually executing them. If this is set to
00068 # true, the atempted_location_results field of the result will be populated, but no arm
00069 # movement will be attempted
00070 bool only_perform_feasibility_test
00071
00072 # OPTIONAL (These will not have to be filled out most of the time)
00073 # constraints to be imposed on every point in the motion of the arm
00074 arm_navigation_msgs/Constraints path_constraints
00075
00076 # OPTIONAL (These will not have to be filled out most of the time)
00077 # additional collision operations to be used for every arm movement performed
00078 # during placing. Note that these will be added on top of (and thus overide) other
00079 # collision operations that the grasping pipeline deems necessary. Should be used
00080 # with care and only if special behaviors are desired.
00081 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations
00082
00083 # OPTIONAL (These will not have to be filled out most of the time)
00084 # additional link paddings to be used for every arm movement performed
00085 # during placing. Note that these will be added on top of (and thus overide) other
00086 # link paddings that the grasping pipeline deems necessary. Should be used
00087 # with care and only if special behaviors are desired.
00088 arm_navigation_msgs/LinkPadding[] additional_link_padding
00089
00090
00091 ================================================================================
00092 MSG: geometry_msgs/PoseStamped
00093 # A Pose with reference coordinate frame and timestamp
00094 Header header
00095 Pose pose
00096
00097 ================================================================================
00098 MSG: std_msgs/Header
00099 # Standard metadata for higher-level stamped data types.
00100 # This is generally used to communicate timestamped data
00101 # in a particular coordinate frame.
00102 #
00103 # sequence ID: consecutively increasing ID
00104 uint32 seq
00105 #Two-integer timestamp that is expressed as:
00106 # * stamp.secs: seconds (stamp_secs) since epoch
00107 # * stamp.nsecs: nanoseconds since stamp_secs
00108 # time-handling sugar is provided by the client library
00109 time stamp
00110 #Frame this data is associated with
00111 # 0: no frame
00112 # 1: global frame
00113 string frame_id
00114
00115 ================================================================================
00116 MSG: geometry_msgs/Pose
00117 # A representation of pose in free space, composed of postion and orientation.
00118 Point position
00119 Quaternion orientation
00120
00121 ================================================================================
00122 MSG: geometry_msgs/Point
00123 # This contains the position of a point in free space
00124 float64 x
00125 float64 y
00126 float64 z
00127
00128 ================================================================================
00129 MSG: geometry_msgs/Quaternion
00130 # This represents an orientation in free space in quaternion form.
00131
00132 float64 x
00133 float64 y
00134 float64 z
00135 float64 w
00136
00137 ================================================================================
00138 MSG: manipulation_msgs/Grasp
00139 # A name for this grasp
00140 string id
00141
00142 # The internal posture of the hand for the pre-grasp
00143 # only positions are used
00144 sensor_msgs/JointState pre_grasp_posture
00145
00146 # The internal posture of the hand for the grasp
00147 # positions and efforts are used
00148 sensor_msgs/JointState grasp_posture
00149
00150 # The position of the end-effector for the grasp relative to a reference frame
00151 # (that is always specified elsewhere, not in this message)
00152 geometry_msgs/PoseStamped grasp_pose
00153
00154 # The estimated probability of success for this grasp, or some other
00155 # measure of how "good" it is.
00156 float64 grasp_quality
00157
00158 # The approach motion
00159 GripperTranslation approach
00160
00161 # The retreat motion
00162 GripperTranslation retreat
00163
00164 # the maximum contact force to use while grasping (<=0 to disable)
00165 float32 max_contact_force
00166
00167 # an optional list of obstacles that we have semantic information about
00168 # and that can be touched/pushed/moved in the course of grasping
00169 string[] allowed_touch_objects
00170
00171 ================================================================================
00172 MSG: sensor_msgs/JointState
00173 # This is a message that holds data to describe the state of a set of torque controlled joints.
00174 #
00175 # The state of each joint (revolute or prismatic) is defined by:
00176 # * the position of the joint (rad or m),
00177 # * the velocity of the joint (rad/s or m/s) and
00178 # * the effort that is applied in the joint (Nm or N).
00179 #
00180 # Each joint is uniquely identified by its name
00181 # The header specifies the time at which the joint states were recorded. All the joint states
00182 # in one message have to be recorded at the same time.
00183 #
00184 # This message consists of a multiple arrays, one for each part of the joint state.
00185 # The goal is to make each of the fields optional. When e.g. your joints have no
00186 # effort associated with them, you can leave the effort array empty.
00187 #
00188 # All arrays in this message should have the same size, or be empty.
00189 # This is the only way to uniquely associate the joint name with the correct
00190 # states.
00191
00192
00193 Header header
00194
00195 string[] name
00196 float64[] position
00197 float64[] velocity
00198 float64[] effort
00199
00200 ================================================================================
00201 MSG: manipulation_msgs/GripperTranslation
00202 # defines a translation for the gripper, used in pickup or place tasks
00203 # for example for lifting an object off a table or approaching the table for placing
00204
00205 # the direction of the translation
00206 geometry_msgs/Vector3Stamped direction
00207
00208 # the desired translation distance
00209 float32 desired_distance
00210
00211 # the min distance that must be considered feasible before the
00212 # grasp is even attempted
00213 float32 min_distance
00214
00215 ================================================================================
00216 MSG: geometry_msgs/Vector3Stamped
00217 # This represents a Vector3 with reference coordinate frame and timestamp
00218 Header header
00219 Vector3 vector
00220
00221 ================================================================================
00222 MSG: geometry_msgs/Vector3
00223 # This represents a vector in free space.
00224
00225 float64 x
00226 float64 y
00227 float64 z
00228 ================================================================================
00229 MSG: object_manipulation_msgs/GripperTranslation
00230 # defines a translation for the gripper, used in pickup or place tasks
00231 # for example for lifting an object off a table or approaching the table for placing
00232
00233 # the direction of the translation
00234 geometry_msgs/Vector3Stamped direction
00235
00236 # the desired translation distance
00237 float32 desired_distance
00238
00239 # the min distance that must be considered feasible before the
00240 # grasp is even attempted
00241 float32 min_distance
00242 ================================================================================
00243 MSG: arm_navigation_msgs/Constraints
00244 # This message contains a list of motion planning constraints.
00245
00246 arm_navigation_msgs/JointConstraint[] joint_constraints
00247 arm_navigation_msgs/PositionConstraint[] position_constraints
00248 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00249 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00250
00251 ================================================================================
00252 MSG: arm_navigation_msgs/JointConstraint
00253 # Constrain the position of a joint to be within a certain bound
00254 string joint_name
00255
00256 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00257 float64 position
00258 float64 tolerance_above
00259 float64 tolerance_below
00260
00261 # A weighting factor for this constraint
00262 float64 weight
00263 ================================================================================
00264 MSG: arm_navigation_msgs/PositionConstraint
00265 # This message contains the definition of a position constraint.
00266 Header header
00267
00268 # The robot link this constraint refers to
00269 string link_name
00270
00271 # The offset (in the link frame) for the target point on the link we are planning for
00272 geometry_msgs/Point target_point_offset
00273
00274 # The nominal/target position for the point we are planning for
00275 geometry_msgs/Point position
00276
00277 # The shape of the bounded region that constrains the position of the end-effector
00278 # This region is always centered at the position defined above
00279 arm_navigation_msgs/Shape constraint_region_shape
00280
00281 # The orientation of the bounded region that constrains the position of the end-effector.
00282 # This allows the specification of non-axis aligned constraints
00283 geometry_msgs/Quaternion constraint_region_orientation
00284
00285 # Constraint weighting factor - a weight for this constraint
00286 float64 weight
00287
00288 ================================================================================
00289 MSG: arm_navigation_msgs/Shape
00290 byte SPHERE=0
00291 byte BOX=1
00292 byte CYLINDER=2
00293 byte MESH=3
00294
00295 byte type
00296
00297
00298 #### define sphere, box, cylinder ####
00299 # the origin of each shape is considered at the shape's center
00300
00301 # for sphere
00302 # radius := dimensions[0]
00303
00304 # for cylinder
00305 # radius := dimensions[0]
00306 # length := dimensions[1]
00307 # the length is along the Z axis
00308
00309 # for box
00310 # size_x := dimensions[0]
00311 # size_y := dimensions[1]
00312 # size_z := dimensions[2]
00313 float64[] dimensions
00314
00315
00316 #### define mesh ####
00317
00318 # list of triangles; triangle k is defined by tre vertices located
00319 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00320 int32[] triangles
00321 geometry_msgs/Point[] vertices
00322
00323 ================================================================================
00324 MSG: arm_navigation_msgs/OrientationConstraint
00325 # This message contains the definition of an orientation constraint.
00326 Header header
00327
00328 # The robot link this constraint refers to
00329 string link_name
00330
00331 # The type of the constraint
00332 int32 type
00333 int32 LINK_FRAME=0
00334 int32 HEADER_FRAME=1
00335
00336 # The desired orientation of the robot link specified as a quaternion
00337 geometry_msgs/Quaternion orientation
00338
00339 # optional RPY error tolerances specified if
00340 float64 absolute_roll_tolerance
00341 float64 absolute_pitch_tolerance
00342 float64 absolute_yaw_tolerance
00343
00344 # Constraint weighting factor - a weight for this constraint
00345 float64 weight
00346
00347 ================================================================================
00348 MSG: arm_navigation_msgs/VisibilityConstraint
00349 # This message contains the definition of a visibility constraint.
00350 Header header
00351
00352 # The point stamped target that needs to be kept within view of the sensor
00353 geometry_msgs/PointStamped target
00354
00355 # The local pose of the frame in which visibility is to be maintained
00356 # The frame id should represent the robot link to which the sensor is attached
00357 # The visual axis of the sensor is assumed to be along the X axis of this frame
00358 geometry_msgs/PoseStamped sensor_pose
00359
00360 # The deviation (in radians) that will be tolerated
00361 # Constraint error will be measured as the solid angle between the
00362 # X axis of the frame defined above and the vector between the origin
00363 # of the frame defined above and the target location
00364 float64 absolute_tolerance
00365
00366
00367 ================================================================================
00368 MSG: geometry_msgs/PointStamped
00369 # This represents a Point with reference coordinate frame and timestamp
00370 Header header
00371 Point point
00372
00373 ================================================================================
00374 MSG: arm_navigation_msgs/OrderedCollisionOperations
00375 # A set of collision operations that will be performed in the order they are specified
00376 CollisionOperation[] collision_operations
00377 ================================================================================
00378 MSG: arm_navigation_msgs/CollisionOperation
00379 # A definition of a collision operation
00380 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions
00381 # between the gripper and all objects in the collision space
00382
00383 string object1
00384 string object2
00385 string COLLISION_SET_ALL="all"
00386 string COLLISION_SET_OBJECTS="objects"
00387 string COLLISION_SET_ATTACHED_OBJECTS="attached"
00388
00389 # The penetration distance to which collisions are allowed. This is 0.0 by default.
00390 float64 penetration_distance
00391
00392 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above
00393 int32 operation
00394 int32 DISABLE=0
00395 int32 ENABLE=1
00396
00397 ================================================================================
00398 MSG: arm_navigation_msgs/LinkPadding
00399 #name for the link
00400 string link_name
00401
00402 # padding to apply to the link
00403 float64 padding
00404
00405 """
00406 __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','only_perform_feasibility_test','path_constraints','additional_collision_operations','additional_link_padding']
00407 _slot_types = ['string','geometry_msgs/PoseStamped[]','manipulation_msgs/Grasp','float32','float32','object_manipulation_msgs/GripperTranslation','string','string','bool','bool','float64','bool','arm_navigation_msgs/Constraints','arm_navigation_msgs/OrderedCollisionOperations','arm_navigation_msgs/LinkPadding[]']
00408
00409 def __init__(self, *args, **kwds):
00410 """
00411 Constructor. Any message fields that are implicitly/explicitly
00412 set to None will be assigned a default value. The recommend
00413 use is keyword arguments as this is more robust to future message
00414 changes. You cannot mix in-order arguments and keyword arguments.
00415
00416 The available fields are:
00417 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,only_perform_feasibility_test,path_constraints,additional_collision_operations,additional_link_padding
00418
00419 :param args: complete set of field values, in .msg order
00420 :param kwds: use keyword arguments corresponding to message field names
00421 to set specific fields.
00422 """
00423 if args or kwds:
00424 super(PlaceGoal, self).__init__(*args, **kwds)
00425
00426 if self.arm_name is None:
00427 self.arm_name = ''
00428 if self.place_locations is None:
00429 self.place_locations = []
00430 if self.grasp is None:
00431 self.grasp = manipulation_msgs.msg.Grasp()
00432 if self.desired_retreat_distance is None:
00433 self.desired_retreat_distance = 0.
00434 if self.min_retreat_distance is None:
00435 self.min_retreat_distance = 0.
00436 if self.approach is None:
00437 self.approach = object_manipulation_msgs.msg.GripperTranslation()
00438 if self.collision_object_name is None:
00439 self.collision_object_name = ''
00440 if self.collision_support_surface_name is None:
00441 self.collision_support_surface_name = ''
00442 if self.allow_gripper_support_collision is None:
00443 self.allow_gripper_support_collision = False
00444 if self.use_reactive_place is None:
00445 self.use_reactive_place = False
00446 if self.place_padding is None:
00447 self.place_padding = 0.
00448 if self.only_perform_feasibility_test is None:
00449 self.only_perform_feasibility_test = False
00450 if self.path_constraints is None:
00451 self.path_constraints = arm_navigation_msgs.msg.Constraints()
00452 if self.additional_collision_operations is None:
00453 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00454 if self.additional_link_padding is None:
00455 self.additional_link_padding = []
00456 else:
00457 self.arm_name = ''
00458 self.place_locations = []
00459 self.grasp = manipulation_msgs.msg.Grasp()
00460 self.desired_retreat_distance = 0.
00461 self.min_retreat_distance = 0.
00462 self.approach = object_manipulation_msgs.msg.GripperTranslation()
00463 self.collision_object_name = ''
00464 self.collision_support_surface_name = ''
00465 self.allow_gripper_support_collision = False
00466 self.use_reactive_place = False
00467 self.place_padding = 0.
00468 self.only_perform_feasibility_test = False
00469 self.path_constraints = arm_navigation_msgs.msg.Constraints()
00470 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00471 self.additional_link_padding = []
00472
00473 def _get_types(self):
00474 """
00475 internal API method
00476 """
00477 return self._slot_types
00478
00479 def serialize(self, buff):
00480 """
00481 serialize message into buffer
00482 :param buff: buffer, ``StringIO``
00483 """
00484 try:
00485 _x = self.arm_name
00486 length = len(_x)
00487 if python3 or type(_x) == unicode:
00488 _x = _x.encode('utf-8')
00489 length = len(_x)
00490 buff.write(struct.pack('<I%ss'%length, length, _x))
00491 length = len(self.place_locations)
00492 buff.write(_struct_I.pack(length))
00493 for val1 in self.place_locations:
00494 _v1 = val1.header
00495 buff.write(_struct_I.pack(_v1.seq))
00496 _v2 = _v1.stamp
00497 _x = _v2
00498 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00499 _x = _v1.frame_id
00500 length = len(_x)
00501 if python3 or type(_x) == unicode:
00502 _x = _x.encode('utf-8')
00503 length = len(_x)
00504 buff.write(struct.pack('<I%ss'%length, length, _x))
00505 _v3 = val1.pose
00506 _v4 = _v3.position
00507 _x = _v4
00508 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00509 _v5 = _v3.orientation
00510 _x = _v5
00511 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00512 _x = self.grasp.id
00513 length = len(_x)
00514 if python3 or type(_x) == unicode:
00515 _x = _x.encode('utf-8')
00516 length = len(_x)
00517 buff.write(struct.pack('<I%ss'%length, length, _x))
00518 _x = self
00519 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))
00520 _x = self.grasp.pre_grasp_posture.header.frame_id
00521 length = len(_x)
00522 if python3 or type(_x) == unicode:
00523 _x = _x.encode('utf-8')
00524 length = len(_x)
00525 buff.write(struct.pack('<I%ss'%length, length, _x))
00526 length = len(self.grasp.pre_grasp_posture.name)
00527 buff.write(_struct_I.pack(length))
00528 for val1 in self.grasp.pre_grasp_posture.name:
00529 length = len(val1)
00530 if python3 or type(val1) == unicode:
00531 val1 = val1.encode('utf-8')
00532 length = len(val1)
00533 buff.write(struct.pack('<I%ss'%length, length, val1))
00534 length = len(self.grasp.pre_grasp_posture.position)
00535 buff.write(_struct_I.pack(length))
00536 pattern = '<%sd'%length
00537 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00538 length = len(self.grasp.pre_grasp_posture.velocity)
00539 buff.write(_struct_I.pack(length))
00540 pattern = '<%sd'%length
00541 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00542 length = len(self.grasp.pre_grasp_posture.effort)
00543 buff.write(_struct_I.pack(length))
00544 pattern = '<%sd'%length
00545 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00546 _x = self
00547 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))
00548 _x = self.grasp.grasp_posture.header.frame_id
00549 length = len(_x)
00550 if python3 or type(_x) == unicode:
00551 _x = _x.encode('utf-8')
00552 length = len(_x)
00553 buff.write(struct.pack('<I%ss'%length, length, _x))
00554 length = len(self.grasp.grasp_posture.name)
00555 buff.write(_struct_I.pack(length))
00556 for val1 in self.grasp.grasp_posture.name:
00557 length = len(val1)
00558 if python3 or type(val1) == unicode:
00559 val1 = val1.encode('utf-8')
00560 length = len(val1)
00561 buff.write(struct.pack('<I%ss'%length, length, val1))
00562 length = len(self.grasp.grasp_posture.position)
00563 buff.write(_struct_I.pack(length))
00564 pattern = '<%sd'%length
00565 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00566 length = len(self.grasp.grasp_posture.velocity)
00567 buff.write(_struct_I.pack(length))
00568 pattern = '<%sd'%length
00569 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00570 length = len(self.grasp.grasp_posture.effort)
00571 buff.write(_struct_I.pack(length))
00572 pattern = '<%sd'%length
00573 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00574 _x = self
00575 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))
00576 _x = self.grasp.grasp_pose.header.frame_id
00577 length = len(_x)
00578 if python3 or type(_x) == unicode:
00579 _x = _x.encode('utf-8')
00580 length = len(_x)
00581 buff.write(struct.pack('<I%ss'%length, length, _x))
00582 _x = self
00583 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))
00584 _x = self.grasp.approach.direction.header.frame_id
00585 length = len(_x)
00586 if python3 or type(_x) == unicode:
00587 _x = _x.encode('utf-8')
00588 length = len(_x)
00589 buff.write(struct.pack('<I%ss'%length, length, _x))
00590 _x = self
00591 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))
00592 _x = self.grasp.retreat.direction.header.frame_id
00593 length = len(_x)
00594 if python3 or type(_x) == unicode:
00595 _x = _x.encode('utf-8')
00596 length = len(_x)
00597 buff.write(struct.pack('<I%ss'%length, length, _x))
00598 _x = self
00599 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))
00600 length = len(self.grasp.allowed_touch_objects)
00601 buff.write(_struct_I.pack(length))
00602 for val1 in self.grasp.allowed_touch_objects:
00603 length = len(val1)
00604 if python3 or type(val1) == unicode:
00605 val1 = val1.encode('utf-8')
00606 length = len(val1)
00607 buff.write(struct.pack('<I%ss'%length, length, val1))
00608 _x = self
00609 buff.write(_struct_2f3I.pack(_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))
00610 _x = self.approach.direction.header.frame_id
00611 length = len(_x)
00612 if python3 or type(_x) == unicode:
00613 _x = _x.encode('utf-8')
00614 length = len(_x)
00615 buff.write(struct.pack('<I%ss'%length, length, _x))
00616 _x = self
00617 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))
00618 _x = self.collision_object_name
00619 length = len(_x)
00620 if python3 or type(_x) == unicode:
00621 _x = _x.encode('utf-8')
00622 length = len(_x)
00623 buff.write(struct.pack('<I%ss'%length, length, _x))
00624 _x = self.collision_support_surface_name
00625 length = len(_x)
00626 if python3 or type(_x) == unicode:
00627 _x = _x.encode('utf-8')
00628 length = len(_x)
00629 buff.write(struct.pack('<I%ss'%length, length, _x))
00630 _x = self
00631 buff.write(_struct_2BdB.pack(_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test))
00632 length = len(self.path_constraints.joint_constraints)
00633 buff.write(_struct_I.pack(length))
00634 for val1 in self.path_constraints.joint_constraints:
00635 _x = val1.joint_name
00636 length = len(_x)
00637 if python3 or type(_x) == unicode:
00638 _x = _x.encode('utf-8')
00639 length = len(_x)
00640 buff.write(struct.pack('<I%ss'%length, length, _x))
00641 _x = val1
00642 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00643 length = len(self.path_constraints.position_constraints)
00644 buff.write(_struct_I.pack(length))
00645 for val1 in self.path_constraints.position_constraints:
00646 _v6 = val1.header
00647 buff.write(_struct_I.pack(_v6.seq))
00648 _v7 = _v6.stamp
00649 _x = _v7
00650 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00651 _x = _v6.frame_id
00652 length = len(_x)
00653 if python3 or type(_x) == unicode:
00654 _x = _x.encode('utf-8')
00655 length = len(_x)
00656 buff.write(struct.pack('<I%ss'%length, length, _x))
00657 _x = val1.link_name
00658 length = len(_x)
00659 if python3 or type(_x) == unicode:
00660 _x = _x.encode('utf-8')
00661 length = len(_x)
00662 buff.write(struct.pack('<I%ss'%length, length, _x))
00663 _v8 = val1.target_point_offset
00664 _x = _v8
00665 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00666 _v9 = val1.position
00667 _x = _v9
00668 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00669 _v10 = val1.constraint_region_shape
00670 buff.write(_struct_b.pack(_v10.type))
00671 length = len(_v10.dimensions)
00672 buff.write(_struct_I.pack(length))
00673 pattern = '<%sd'%length
00674 buff.write(struct.pack(pattern, *_v10.dimensions))
00675 length = len(_v10.triangles)
00676 buff.write(_struct_I.pack(length))
00677 pattern = '<%si'%length
00678 buff.write(struct.pack(pattern, *_v10.triangles))
00679 length = len(_v10.vertices)
00680 buff.write(_struct_I.pack(length))
00681 for val3 in _v10.vertices:
00682 _x = val3
00683 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00684 _v11 = val1.constraint_region_orientation
00685 _x = _v11
00686 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00687 buff.write(_struct_d.pack(val1.weight))
00688 length = len(self.path_constraints.orientation_constraints)
00689 buff.write(_struct_I.pack(length))
00690 for val1 in self.path_constraints.orientation_constraints:
00691 _v12 = val1.header
00692 buff.write(_struct_I.pack(_v12.seq))
00693 _v13 = _v12.stamp
00694 _x = _v13
00695 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00696 _x = _v12.frame_id
00697 length = len(_x)
00698 if python3 or type(_x) == unicode:
00699 _x = _x.encode('utf-8')
00700 length = len(_x)
00701 buff.write(struct.pack('<I%ss'%length, length, _x))
00702 _x = val1.link_name
00703 length = len(_x)
00704 if python3 or type(_x) == unicode:
00705 _x = _x.encode('utf-8')
00706 length = len(_x)
00707 buff.write(struct.pack('<I%ss'%length, length, _x))
00708 buff.write(_struct_i.pack(val1.type))
00709 _v14 = val1.orientation
00710 _x = _v14
00711 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00712 _x = val1
00713 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00714 length = len(self.path_constraints.visibility_constraints)
00715 buff.write(_struct_I.pack(length))
00716 for val1 in self.path_constraints.visibility_constraints:
00717 _v15 = val1.header
00718 buff.write(_struct_I.pack(_v15.seq))
00719 _v16 = _v15.stamp
00720 _x = _v16
00721 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00722 _x = _v15.frame_id
00723 length = len(_x)
00724 if python3 or type(_x) == unicode:
00725 _x = _x.encode('utf-8')
00726 length = len(_x)
00727 buff.write(struct.pack('<I%ss'%length, length, _x))
00728 _v17 = val1.target
00729 _v18 = _v17.header
00730 buff.write(_struct_I.pack(_v18.seq))
00731 _v19 = _v18.stamp
00732 _x = _v19
00733 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00734 _x = _v18.frame_id
00735 length = len(_x)
00736 if python3 or type(_x) == unicode:
00737 _x = _x.encode('utf-8')
00738 length = len(_x)
00739 buff.write(struct.pack('<I%ss'%length, length, _x))
00740 _v20 = _v17.point
00741 _x = _v20
00742 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00743 _v21 = val1.sensor_pose
00744 _v22 = _v21.header
00745 buff.write(_struct_I.pack(_v22.seq))
00746 _v23 = _v22.stamp
00747 _x = _v23
00748 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00749 _x = _v22.frame_id
00750 length = len(_x)
00751 if python3 or type(_x) == unicode:
00752 _x = _x.encode('utf-8')
00753 length = len(_x)
00754 buff.write(struct.pack('<I%ss'%length, length, _x))
00755 _v24 = _v21.pose
00756 _v25 = _v24.position
00757 _x = _v25
00758 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00759 _v26 = _v24.orientation
00760 _x = _v26
00761 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00762 buff.write(_struct_d.pack(val1.absolute_tolerance))
00763 length = len(self.additional_collision_operations.collision_operations)
00764 buff.write(_struct_I.pack(length))
00765 for val1 in self.additional_collision_operations.collision_operations:
00766 _x = val1.object1
00767 length = len(_x)
00768 if python3 or type(_x) == unicode:
00769 _x = _x.encode('utf-8')
00770 length = len(_x)
00771 buff.write(struct.pack('<I%ss'%length, length, _x))
00772 _x = val1.object2
00773 length = len(_x)
00774 if python3 or type(_x) == unicode:
00775 _x = _x.encode('utf-8')
00776 length = len(_x)
00777 buff.write(struct.pack('<I%ss'%length, length, _x))
00778 _x = val1
00779 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
00780 length = len(self.additional_link_padding)
00781 buff.write(_struct_I.pack(length))
00782 for val1 in self.additional_link_padding:
00783 _x = val1.link_name
00784 length = len(_x)
00785 if python3 or type(_x) == unicode:
00786 _x = _x.encode('utf-8')
00787 length = len(_x)
00788 buff.write(struct.pack('<I%ss'%length, length, _x))
00789 buff.write(_struct_d.pack(val1.padding))
00790 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00791 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00792
00793 def deserialize(self, str):
00794 """
00795 unpack serialized message in str into this message instance
00796 :param str: byte array of serialized message, ``str``
00797 """
00798 try:
00799 if self.place_locations is None:
00800 self.place_locations = None
00801 if self.grasp is None:
00802 self.grasp = manipulation_msgs.msg.Grasp()
00803 if self.approach is None:
00804 self.approach = object_manipulation_msgs.msg.GripperTranslation()
00805 if self.path_constraints is None:
00806 self.path_constraints = arm_navigation_msgs.msg.Constraints()
00807 if self.additional_collision_operations is None:
00808 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00809 if self.additional_link_padding is None:
00810 self.additional_link_padding = None
00811 end = 0
00812 start = end
00813 end += 4
00814 (length,) = _struct_I.unpack(str[start:end])
00815 start = end
00816 end += length
00817 if python3:
00818 self.arm_name = str[start:end].decode('utf-8')
00819 else:
00820 self.arm_name = str[start:end]
00821 start = end
00822 end += 4
00823 (length,) = _struct_I.unpack(str[start:end])
00824 self.place_locations = []
00825 for i in range(0, length):
00826 val1 = geometry_msgs.msg.PoseStamped()
00827 _v27 = val1.header
00828 start = end
00829 end += 4
00830 (_v27.seq,) = _struct_I.unpack(str[start:end])
00831 _v28 = _v27.stamp
00832 _x = _v28
00833 start = end
00834 end += 8
00835 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00836 start = end
00837 end += 4
00838 (length,) = _struct_I.unpack(str[start:end])
00839 start = end
00840 end += length
00841 if python3:
00842 _v27.frame_id = str[start:end].decode('utf-8')
00843 else:
00844 _v27.frame_id = str[start:end]
00845 _v29 = val1.pose
00846 _v30 = _v29.position
00847 _x = _v30
00848 start = end
00849 end += 24
00850 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00851 _v31 = _v29.orientation
00852 _x = _v31
00853 start = end
00854 end += 32
00855 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00856 self.place_locations.append(val1)
00857 start = end
00858 end += 4
00859 (length,) = _struct_I.unpack(str[start:end])
00860 start = end
00861 end += length
00862 if python3:
00863 self.grasp.id = str[start:end].decode('utf-8')
00864 else:
00865 self.grasp.id = str[start:end]
00866 _x = self
00867 start = end
00868 end += 12
00869 (_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])
00870 start = end
00871 end += 4
00872 (length,) = _struct_I.unpack(str[start:end])
00873 start = end
00874 end += length
00875 if python3:
00876 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00877 else:
00878 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00879 start = end
00880 end += 4
00881 (length,) = _struct_I.unpack(str[start:end])
00882 self.grasp.pre_grasp_posture.name = []
00883 for i in range(0, length):
00884 start = end
00885 end += 4
00886 (length,) = _struct_I.unpack(str[start:end])
00887 start = end
00888 end += length
00889 if python3:
00890 val1 = str[start:end].decode('utf-8')
00891 else:
00892 val1 = str[start:end]
00893 self.grasp.pre_grasp_posture.name.append(val1)
00894 start = end
00895 end += 4
00896 (length,) = _struct_I.unpack(str[start:end])
00897 pattern = '<%sd'%length
00898 start = end
00899 end += struct.calcsize(pattern)
00900 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
00901 start = end
00902 end += 4
00903 (length,) = _struct_I.unpack(str[start:end])
00904 pattern = '<%sd'%length
00905 start = end
00906 end += struct.calcsize(pattern)
00907 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00908 start = end
00909 end += 4
00910 (length,) = _struct_I.unpack(str[start:end])
00911 pattern = '<%sd'%length
00912 start = end
00913 end += struct.calcsize(pattern)
00914 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
00915 _x = self
00916 start = end
00917 end += 12
00918 (_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])
00919 start = end
00920 end += 4
00921 (length,) = _struct_I.unpack(str[start:end])
00922 start = end
00923 end += length
00924 if python3:
00925 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00926 else:
00927 self.grasp.grasp_posture.header.frame_id = str[start:end]
00928 start = end
00929 end += 4
00930 (length,) = _struct_I.unpack(str[start:end])
00931 self.grasp.grasp_posture.name = []
00932 for i in range(0, length):
00933 start = end
00934 end += 4
00935 (length,) = _struct_I.unpack(str[start:end])
00936 start = end
00937 end += length
00938 if python3:
00939 val1 = str[start:end].decode('utf-8')
00940 else:
00941 val1 = str[start:end]
00942 self.grasp.grasp_posture.name.append(val1)
00943 start = end
00944 end += 4
00945 (length,) = _struct_I.unpack(str[start:end])
00946 pattern = '<%sd'%length
00947 start = end
00948 end += struct.calcsize(pattern)
00949 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
00950 start = end
00951 end += 4
00952 (length,) = _struct_I.unpack(str[start:end])
00953 pattern = '<%sd'%length
00954 start = end
00955 end += struct.calcsize(pattern)
00956 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00957 start = end
00958 end += 4
00959 (length,) = _struct_I.unpack(str[start:end])
00960 pattern = '<%sd'%length
00961 start = end
00962 end += struct.calcsize(pattern)
00963 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
00964 _x = self
00965 start = end
00966 end += 12
00967 (_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])
00968 start = end
00969 end += 4
00970 (length,) = _struct_I.unpack(str[start:end])
00971 start = end
00972 end += length
00973 if python3:
00974 self.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
00975 else:
00976 self.grasp.grasp_pose.header.frame_id = str[start:end]
00977 _x = self
00978 start = end
00979 end += 76
00980 (_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])
00981 start = end
00982 end += 4
00983 (length,) = _struct_I.unpack(str[start:end])
00984 start = end
00985 end += length
00986 if python3:
00987 self.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
00988 else:
00989 self.grasp.approach.direction.header.frame_id = str[start:end]
00990 _x = self
00991 start = end
00992 end += 44
00993 (_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])
00994 start = end
00995 end += 4
00996 (length,) = _struct_I.unpack(str[start:end])
00997 start = end
00998 end += length
00999 if python3:
01000 self.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
01001 else:
01002 self.grasp.retreat.direction.header.frame_id = str[start:end]
01003 _x = self
01004 start = end
01005 end += 36
01006 (_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])
01007 start = end
01008 end += 4
01009 (length,) = _struct_I.unpack(str[start:end])
01010 self.grasp.allowed_touch_objects = []
01011 for i in range(0, length):
01012 start = end
01013 end += 4
01014 (length,) = _struct_I.unpack(str[start:end])
01015 start = end
01016 end += length
01017 if python3:
01018 val1 = str[start:end].decode('utf-8')
01019 else:
01020 val1 = str[start:end]
01021 self.grasp.allowed_touch_objects.append(val1)
01022 _x = self
01023 start = end
01024 end += 20
01025 (_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_2f3I.unpack(str[start:end])
01026 start = end
01027 end += 4
01028 (length,) = _struct_I.unpack(str[start:end])
01029 start = end
01030 end += length
01031 if python3:
01032 self.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01033 else:
01034 self.approach.direction.header.frame_id = str[start:end]
01035 _x = self
01036 start = end
01037 end += 32
01038 (_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])
01039 start = end
01040 end += 4
01041 (length,) = _struct_I.unpack(str[start:end])
01042 start = end
01043 end += length
01044 if python3:
01045 self.collision_object_name = str[start:end].decode('utf-8')
01046 else:
01047 self.collision_object_name = str[start:end]
01048 start = end
01049 end += 4
01050 (length,) = _struct_I.unpack(str[start:end])
01051 start = end
01052 end += length
01053 if python3:
01054 self.collision_support_surface_name = str[start:end].decode('utf-8')
01055 else:
01056 self.collision_support_surface_name = str[start:end]
01057 _x = self
01058 start = end
01059 end += 11
01060 (_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end])
01061 self.allow_gripper_support_collision = bool(self.allow_gripper_support_collision)
01062 self.use_reactive_place = bool(self.use_reactive_place)
01063 self.only_perform_feasibility_test = bool(self.only_perform_feasibility_test)
01064 start = end
01065 end += 4
01066 (length,) = _struct_I.unpack(str[start:end])
01067 self.path_constraints.joint_constraints = []
01068 for i in range(0, length):
01069 val1 = arm_navigation_msgs.msg.JointConstraint()
01070 start = end
01071 end += 4
01072 (length,) = _struct_I.unpack(str[start:end])
01073 start = end
01074 end += length
01075 if python3:
01076 val1.joint_name = str[start:end].decode('utf-8')
01077 else:
01078 val1.joint_name = str[start:end]
01079 _x = val1
01080 start = end
01081 end += 32
01082 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01083 self.path_constraints.joint_constraints.append(val1)
01084 start = end
01085 end += 4
01086 (length,) = _struct_I.unpack(str[start:end])
01087 self.path_constraints.position_constraints = []
01088 for i in range(0, length):
01089 val1 = arm_navigation_msgs.msg.PositionConstraint()
01090 _v32 = val1.header
01091 start = end
01092 end += 4
01093 (_v32.seq,) = _struct_I.unpack(str[start:end])
01094 _v33 = _v32.stamp
01095 _x = _v33
01096 start = end
01097 end += 8
01098 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01099 start = end
01100 end += 4
01101 (length,) = _struct_I.unpack(str[start:end])
01102 start = end
01103 end += length
01104 if python3:
01105 _v32.frame_id = str[start:end].decode('utf-8')
01106 else:
01107 _v32.frame_id = str[start:end]
01108 start = end
01109 end += 4
01110 (length,) = _struct_I.unpack(str[start:end])
01111 start = end
01112 end += length
01113 if python3:
01114 val1.link_name = str[start:end].decode('utf-8')
01115 else:
01116 val1.link_name = str[start:end]
01117 _v34 = val1.target_point_offset
01118 _x = _v34
01119 start = end
01120 end += 24
01121 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01122 _v35 = val1.position
01123 _x = _v35
01124 start = end
01125 end += 24
01126 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01127 _v36 = val1.constraint_region_shape
01128 start = end
01129 end += 1
01130 (_v36.type,) = _struct_b.unpack(str[start:end])
01131 start = end
01132 end += 4
01133 (length,) = _struct_I.unpack(str[start:end])
01134 pattern = '<%sd'%length
01135 start = end
01136 end += struct.calcsize(pattern)
01137 _v36.dimensions = struct.unpack(pattern, str[start:end])
01138 start = end
01139 end += 4
01140 (length,) = _struct_I.unpack(str[start:end])
01141 pattern = '<%si'%length
01142 start = end
01143 end += struct.calcsize(pattern)
01144 _v36.triangles = struct.unpack(pattern, str[start:end])
01145 start = end
01146 end += 4
01147 (length,) = _struct_I.unpack(str[start:end])
01148 _v36.vertices = []
01149 for i in range(0, length):
01150 val3 = geometry_msgs.msg.Point()
01151 _x = val3
01152 start = end
01153 end += 24
01154 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01155 _v36.vertices.append(val3)
01156 _v37 = val1.constraint_region_orientation
01157 _x = _v37
01158 start = end
01159 end += 32
01160 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01161 start = end
01162 end += 8
01163 (val1.weight,) = _struct_d.unpack(str[start:end])
01164 self.path_constraints.position_constraints.append(val1)
01165 start = end
01166 end += 4
01167 (length,) = _struct_I.unpack(str[start:end])
01168 self.path_constraints.orientation_constraints = []
01169 for i in range(0, length):
01170 val1 = arm_navigation_msgs.msg.OrientationConstraint()
01171 _v38 = val1.header
01172 start = end
01173 end += 4
01174 (_v38.seq,) = _struct_I.unpack(str[start:end])
01175 _v39 = _v38.stamp
01176 _x = _v39
01177 start = end
01178 end += 8
01179 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01180 start = end
01181 end += 4
01182 (length,) = _struct_I.unpack(str[start:end])
01183 start = end
01184 end += length
01185 if python3:
01186 _v38.frame_id = str[start:end].decode('utf-8')
01187 else:
01188 _v38.frame_id = str[start:end]
01189 start = end
01190 end += 4
01191 (length,) = _struct_I.unpack(str[start:end])
01192 start = end
01193 end += length
01194 if python3:
01195 val1.link_name = str[start:end].decode('utf-8')
01196 else:
01197 val1.link_name = str[start:end]
01198 start = end
01199 end += 4
01200 (val1.type,) = _struct_i.unpack(str[start:end])
01201 _v40 = val1.orientation
01202 _x = _v40
01203 start = end
01204 end += 32
01205 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01206 _x = val1
01207 start = end
01208 end += 32
01209 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
01210 self.path_constraints.orientation_constraints.append(val1)
01211 start = end
01212 end += 4
01213 (length,) = _struct_I.unpack(str[start:end])
01214 self.path_constraints.visibility_constraints = []
01215 for i in range(0, length):
01216 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
01217 _v41 = val1.header
01218 start = end
01219 end += 4
01220 (_v41.seq,) = _struct_I.unpack(str[start:end])
01221 _v42 = _v41.stamp
01222 _x = _v42
01223 start = end
01224 end += 8
01225 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01226 start = end
01227 end += 4
01228 (length,) = _struct_I.unpack(str[start:end])
01229 start = end
01230 end += length
01231 if python3:
01232 _v41.frame_id = str[start:end].decode('utf-8')
01233 else:
01234 _v41.frame_id = str[start:end]
01235 _v43 = val1.target
01236 _v44 = _v43.header
01237 start = end
01238 end += 4
01239 (_v44.seq,) = _struct_I.unpack(str[start:end])
01240 _v45 = _v44.stamp
01241 _x = _v45
01242 start = end
01243 end += 8
01244 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01245 start = end
01246 end += 4
01247 (length,) = _struct_I.unpack(str[start:end])
01248 start = end
01249 end += length
01250 if python3:
01251 _v44.frame_id = str[start:end].decode('utf-8')
01252 else:
01253 _v44.frame_id = str[start:end]
01254 _v46 = _v43.point
01255 _x = _v46
01256 start = end
01257 end += 24
01258 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01259 _v47 = val1.sensor_pose
01260 _v48 = _v47.header
01261 start = end
01262 end += 4
01263 (_v48.seq,) = _struct_I.unpack(str[start:end])
01264 _v49 = _v48.stamp
01265 _x = _v49
01266 start = end
01267 end += 8
01268 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01269 start = end
01270 end += 4
01271 (length,) = _struct_I.unpack(str[start:end])
01272 start = end
01273 end += length
01274 if python3:
01275 _v48.frame_id = str[start:end].decode('utf-8')
01276 else:
01277 _v48.frame_id = str[start:end]
01278 _v50 = _v47.pose
01279 _v51 = _v50.position
01280 _x = _v51
01281 start = end
01282 end += 24
01283 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01284 _v52 = _v50.orientation
01285 _x = _v52
01286 start = end
01287 end += 32
01288 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01289 start = end
01290 end += 8
01291 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
01292 self.path_constraints.visibility_constraints.append(val1)
01293 start = end
01294 end += 4
01295 (length,) = _struct_I.unpack(str[start:end])
01296 self.additional_collision_operations.collision_operations = []
01297 for i in range(0, length):
01298 val1 = arm_navigation_msgs.msg.CollisionOperation()
01299 start = end
01300 end += 4
01301 (length,) = _struct_I.unpack(str[start:end])
01302 start = end
01303 end += length
01304 if python3:
01305 val1.object1 = str[start:end].decode('utf-8')
01306 else:
01307 val1.object1 = str[start:end]
01308 start = end
01309 end += 4
01310 (length,) = _struct_I.unpack(str[start:end])
01311 start = end
01312 end += length
01313 if python3:
01314 val1.object2 = str[start:end].decode('utf-8')
01315 else:
01316 val1.object2 = str[start:end]
01317 _x = val1
01318 start = end
01319 end += 12
01320 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
01321 self.additional_collision_operations.collision_operations.append(val1)
01322 start = end
01323 end += 4
01324 (length,) = _struct_I.unpack(str[start:end])
01325 self.additional_link_padding = []
01326 for i in range(0, length):
01327 val1 = arm_navigation_msgs.msg.LinkPadding()
01328 start = end
01329 end += 4
01330 (length,) = _struct_I.unpack(str[start:end])
01331 start = end
01332 end += length
01333 if python3:
01334 val1.link_name = str[start:end].decode('utf-8')
01335 else:
01336 val1.link_name = str[start:end]
01337 start = end
01338 end += 8
01339 (val1.padding,) = _struct_d.unpack(str[start:end])
01340 self.additional_link_padding.append(val1)
01341 return self
01342 except struct.error as e:
01343 raise genpy.DeserializationError(e)
01344
01345
01346 def serialize_numpy(self, buff, numpy):
01347 """
01348 serialize message with numpy array types into buffer
01349 :param buff: buffer, ``StringIO``
01350 :param numpy: numpy python module
01351 """
01352 try:
01353 _x = self.arm_name
01354 length = len(_x)
01355 if python3 or type(_x) == unicode:
01356 _x = _x.encode('utf-8')
01357 length = len(_x)
01358 buff.write(struct.pack('<I%ss'%length, length, _x))
01359 length = len(self.place_locations)
01360 buff.write(_struct_I.pack(length))
01361 for val1 in self.place_locations:
01362 _v53 = val1.header
01363 buff.write(_struct_I.pack(_v53.seq))
01364 _v54 = _v53.stamp
01365 _x = _v54
01366 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01367 _x = _v53.frame_id
01368 length = len(_x)
01369 if python3 or type(_x) == unicode:
01370 _x = _x.encode('utf-8')
01371 length = len(_x)
01372 buff.write(struct.pack('<I%ss'%length, length, _x))
01373 _v55 = val1.pose
01374 _v56 = _v55.position
01375 _x = _v56
01376 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01377 _v57 = _v55.orientation
01378 _x = _v57
01379 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01380 _x = self.grasp.id
01381 length = len(_x)
01382 if python3 or type(_x) == unicode:
01383 _x = _x.encode('utf-8')
01384 length = len(_x)
01385 buff.write(struct.pack('<I%ss'%length, length, _x))
01386 _x = self
01387 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))
01388 _x = self.grasp.pre_grasp_posture.header.frame_id
01389 length = len(_x)
01390 if python3 or type(_x) == unicode:
01391 _x = _x.encode('utf-8')
01392 length = len(_x)
01393 buff.write(struct.pack('<I%ss'%length, length, _x))
01394 length = len(self.grasp.pre_grasp_posture.name)
01395 buff.write(_struct_I.pack(length))
01396 for val1 in self.grasp.pre_grasp_posture.name:
01397 length = len(val1)
01398 if python3 or type(val1) == unicode:
01399 val1 = val1.encode('utf-8')
01400 length = len(val1)
01401 buff.write(struct.pack('<I%ss'%length, length, val1))
01402 length = len(self.grasp.pre_grasp_posture.position)
01403 buff.write(_struct_I.pack(length))
01404 pattern = '<%sd'%length
01405 buff.write(self.grasp.pre_grasp_posture.position.tostring())
01406 length = len(self.grasp.pre_grasp_posture.velocity)
01407 buff.write(_struct_I.pack(length))
01408 pattern = '<%sd'%length
01409 buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
01410 length = len(self.grasp.pre_grasp_posture.effort)
01411 buff.write(_struct_I.pack(length))
01412 pattern = '<%sd'%length
01413 buff.write(self.grasp.pre_grasp_posture.effort.tostring())
01414 _x = self
01415 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))
01416 _x = self.grasp.grasp_posture.header.frame_id
01417 length = len(_x)
01418 if python3 or type(_x) == unicode:
01419 _x = _x.encode('utf-8')
01420 length = len(_x)
01421 buff.write(struct.pack('<I%ss'%length, length, _x))
01422 length = len(self.grasp.grasp_posture.name)
01423 buff.write(_struct_I.pack(length))
01424 for val1 in self.grasp.grasp_posture.name:
01425 length = len(val1)
01426 if python3 or type(val1) == unicode:
01427 val1 = val1.encode('utf-8')
01428 length = len(val1)
01429 buff.write(struct.pack('<I%ss'%length, length, val1))
01430 length = len(self.grasp.grasp_posture.position)
01431 buff.write(_struct_I.pack(length))
01432 pattern = '<%sd'%length
01433 buff.write(self.grasp.grasp_posture.position.tostring())
01434 length = len(self.grasp.grasp_posture.velocity)
01435 buff.write(_struct_I.pack(length))
01436 pattern = '<%sd'%length
01437 buff.write(self.grasp.grasp_posture.velocity.tostring())
01438 length = len(self.grasp.grasp_posture.effort)
01439 buff.write(_struct_I.pack(length))
01440 pattern = '<%sd'%length
01441 buff.write(self.grasp.grasp_posture.effort.tostring())
01442 _x = self
01443 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))
01444 _x = self.grasp.grasp_pose.header.frame_id
01445 length = len(_x)
01446 if python3 or type(_x) == unicode:
01447 _x = _x.encode('utf-8')
01448 length = len(_x)
01449 buff.write(struct.pack('<I%ss'%length, length, _x))
01450 _x = self
01451 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))
01452 _x = self.grasp.approach.direction.header.frame_id
01453 length = len(_x)
01454 if python3 or type(_x) == unicode:
01455 _x = _x.encode('utf-8')
01456 length = len(_x)
01457 buff.write(struct.pack('<I%ss'%length, length, _x))
01458 _x = self
01459 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))
01460 _x = self.grasp.retreat.direction.header.frame_id
01461 length = len(_x)
01462 if python3 or type(_x) == unicode:
01463 _x = _x.encode('utf-8')
01464 length = len(_x)
01465 buff.write(struct.pack('<I%ss'%length, length, _x))
01466 _x = self
01467 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))
01468 length = len(self.grasp.allowed_touch_objects)
01469 buff.write(_struct_I.pack(length))
01470 for val1 in self.grasp.allowed_touch_objects:
01471 length = len(val1)
01472 if python3 or type(val1) == unicode:
01473 val1 = val1.encode('utf-8')
01474 length = len(val1)
01475 buff.write(struct.pack('<I%ss'%length, length, val1))
01476 _x = self
01477 buff.write(_struct_2f3I.pack(_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))
01478 _x = self.approach.direction.header.frame_id
01479 length = len(_x)
01480 if python3 or type(_x) == unicode:
01481 _x = _x.encode('utf-8')
01482 length = len(_x)
01483 buff.write(struct.pack('<I%ss'%length, length, _x))
01484 _x = self
01485 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))
01486 _x = self.collision_object_name
01487 length = len(_x)
01488 if python3 or type(_x) == unicode:
01489 _x = _x.encode('utf-8')
01490 length = len(_x)
01491 buff.write(struct.pack('<I%ss'%length, length, _x))
01492 _x = self.collision_support_surface_name
01493 length = len(_x)
01494 if python3 or type(_x) == unicode:
01495 _x = _x.encode('utf-8')
01496 length = len(_x)
01497 buff.write(struct.pack('<I%ss'%length, length, _x))
01498 _x = self
01499 buff.write(_struct_2BdB.pack(_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test))
01500 length = len(self.path_constraints.joint_constraints)
01501 buff.write(_struct_I.pack(length))
01502 for val1 in self.path_constraints.joint_constraints:
01503 _x = val1.joint_name
01504 length = len(_x)
01505 if python3 or type(_x) == unicode:
01506 _x = _x.encode('utf-8')
01507 length = len(_x)
01508 buff.write(struct.pack('<I%ss'%length, length, _x))
01509 _x = val1
01510 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01511 length = len(self.path_constraints.position_constraints)
01512 buff.write(_struct_I.pack(length))
01513 for val1 in self.path_constraints.position_constraints:
01514 _v58 = val1.header
01515 buff.write(_struct_I.pack(_v58.seq))
01516 _v59 = _v58.stamp
01517 _x = _v59
01518 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01519 _x = _v58.frame_id
01520 length = len(_x)
01521 if python3 or type(_x) == unicode:
01522 _x = _x.encode('utf-8')
01523 length = len(_x)
01524 buff.write(struct.pack('<I%ss'%length, length, _x))
01525 _x = val1.link_name
01526 length = len(_x)
01527 if python3 or type(_x) == unicode:
01528 _x = _x.encode('utf-8')
01529 length = len(_x)
01530 buff.write(struct.pack('<I%ss'%length, length, _x))
01531 _v60 = val1.target_point_offset
01532 _x = _v60
01533 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01534 _v61 = val1.position
01535 _x = _v61
01536 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01537 _v62 = val1.constraint_region_shape
01538 buff.write(_struct_b.pack(_v62.type))
01539 length = len(_v62.dimensions)
01540 buff.write(_struct_I.pack(length))
01541 pattern = '<%sd'%length
01542 buff.write(_v62.dimensions.tostring())
01543 length = len(_v62.triangles)
01544 buff.write(_struct_I.pack(length))
01545 pattern = '<%si'%length
01546 buff.write(_v62.triangles.tostring())
01547 length = len(_v62.vertices)
01548 buff.write(_struct_I.pack(length))
01549 for val3 in _v62.vertices:
01550 _x = val3
01551 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01552 _v63 = val1.constraint_region_orientation
01553 _x = _v63
01554 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01555 buff.write(_struct_d.pack(val1.weight))
01556 length = len(self.path_constraints.orientation_constraints)
01557 buff.write(_struct_I.pack(length))
01558 for val1 in self.path_constraints.orientation_constraints:
01559 _v64 = val1.header
01560 buff.write(_struct_I.pack(_v64.seq))
01561 _v65 = _v64.stamp
01562 _x = _v65
01563 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01564 _x = _v64.frame_id
01565 length = len(_x)
01566 if python3 or type(_x) == unicode:
01567 _x = _x.encode('utf-8')
01568 length = len(_x)
01569 buff.write(struct.pack('<I%ss'%length, length, _x))
01570 _x = val1.link_name
01571 length = len(_x)
01572 if python3 or type(_x) == unicode:
01573 _x = _x.encode('utf-8')
01574 length = len(_x)
01575 buff.write(struct.pack('<I%ss'%length, length, _x))
01576 buff.write(_struct_i.pack(val1.type))
01577 _v66 = val1.orientation
01578 _x = _v66
01579 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01580 _x = val1
01581 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01582 length = len(self.path_constraints.visibility_constraints)
01583 buff.write(_struct_I.pack(length))
01584 for val1 in self.path_constraints.visibility_constraints:
01585 _v67 = val1.header
01586 buff.write(_struct_I.pack(_v67.seq))
01587 _v68 = _v67.stamp
01588 _x = _v68
01589 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01590 _x = _v67.frame_id
01591 length = len(_x)
01592 if python3 or type(_x) == unicode:
01593 _x = _x.encode('utf-8')
01594 length = len(_x)
01595 buff.write(struct.pack('<I%ss'%length, length, _x))
01596 _v69 = val1.target
01597 _v70 = _v69.header
01598 buff.write(_struct_I.pack(_v70.seq))
01599 _v71 = _v70.stamp
01600 _x = _v71
01601 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01602 _x = _v70.frame_id
01603 length = len(_x)
01604 if python3 or type(_x) == unicode:
01605 _x = _x.encode('utf-8')
01606 length = len(_x)
01607 buff.write(struct.pack('<I%ss'%length, length, _x))
01608 _v72 = _v69.point
01609 _x = _v72
01610 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01611 _v73 = val1.sensor_pose
01612 _v74 = _v73.header
01613 buff.write(_struct_I.pack(_v74.seq))
01614 _v75 = _v74.stamp
01615 _x = _v75
01616 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01617 _x = _v74.frame_id
01618 length = len(_x)
01619 if python3 or type(_x) == unicode:
01620 _x = _x.encode('utf-8')
01621 length = len(_x)
01622 buff.write(struct.pack('<I%ss'%length, length, _x))
01623 _v76 = _v73.pose
01624 _v77 = _v76.position
01625 _x = _v77
01626 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01627 _v78 = _v76.orientation
01628 _x = _v78
01629 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01630 buff.write(_struct_d.pack(val1.absolute_tolerance))
01631 length = len(self.additional_collision_operations.collision_operations)
01632 buff.write(_struct_I.pack(length))
01633 for val1 in self.additional_collision_operations.collision_operations:
01634 _x = val1.object1
01635 length = len(_x)
01636 if python3 or type(_x) == unicode:
01637 _x = _x.encode('utf-8')
01638 length = len(_x)
01639 buff.write(struct.pack('<I%ss'%length, length, _x))
01640 _x = val1.object2
01641 length = len(_x)
01642 if python3 or type(_x) == unicode:
01643 _x = _x.encode('utf-8')
01644 length = len(_x)
01645 buff.write(struct.pack('<I%ss'%length, length, _x))
01646 _x = val1
01647 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
01648 length = len(self.additional_link_padding)
01649 buff.write(_struct_I.pack(length))
01650 for val1 in self.additional_link_padding:
01651 _x = val1.link_name
01652 length = len(_x)
01653 if python3 or type(_x) == unicode:
01654 _x = _x.encode('utf-8')
01655 length = len(_x)
01656 buff.write(struct.pack('<I%ss'%length, length, _x))
01657 buff.write(_struct_d.pack(val1.padding))
01658 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
01659 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
01660
01661 def deserialize_numpy(self, str, numpy):
01662 """
01663 unpack serialized message in str into this message instance using numpy for array types
01664 :param str: byte array of serialized message, ``str``
01665 :param numpy: numpy python module
01666 """
01667 try:
01668 if self.place_locations is None:
01669 self.place_locations = None
01670 if self.grasp is None:
01671 self.grasp = manipulation_msgs.msg.Grasp()
01672 if self.approach is None:
01673 self.approach = object_manipulation_msgs.msg.GripperTranslation()
01674 if self.path_constraints is None:
01675 self.path_constraints = arm_navigation_msgs.msg.Constraints()
01676 if self.additional_collision_operations is None:
01677 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
01678 if self.additional_link_padding is None:
01679 self.additional_link_padding = None
01680 end = 0
01681 start = end
01682 end += 4
01683 (length,) = _struct_I.unpack(str[start:end])
01684 start = end
01685 end += length
01686 if python3:
01687 self.arm_name = str[start:end].decode('utf-8')
01688 else:
01689 self.arm_name = str[start:end]
01690 start = end
01691 end += 4
01692 (length,) = _struct_I.unpack(str[start:end])
01693 self.place_locations = []
01694 for i in range(0, length):
01695 val1 = geometry_msgs.msg.PoseStamped()
01696 _v79 = val1.header
01697 start = end
01698 end += 4
01699 (_v79.seq,) = _struct_I.unpack(str[start:end])
01700 _v80 = _v79.stamp
01701 _x = _v80
01702 start = end
01703 end += 8
01704 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01705 start = end
01706 end += 4
01707 (length,) = _struct_I.unpack(str[start:end])
01708 start = end
01709 end += length
01710 if python3:
01711 _v79.frame_id = str[start:end].decode('utf-8')
01712 else:
01713 _v79.frame_id = str[start:end]
01714 _v81 = val1.pose
01715 _v82 = _v81.position
01716 _x = _v82
01717 start = end
01718 end += 24
01719 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01720 _v83 = _v81.orientation
01721 _x = _v83
01722 start = end
01723 end += 32
01724 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01725 self.place_locations.append(val1)
01726 start = end
01727 end += 4
01728 (length,) = _struct_I.unpack(str[start:end])
01729 start = end
01730 end += length
01731 if python3:
01732 self.grasp.id = str[start:end].decode('utf-8')
01733 else:
01734 self.grasp.id = str[start:end]
01735 _x = self
01736 start = end
01737 end += 12
01738 (_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])
01739 start = end
01740 end += 4
01741 (length,) = _struct_I.unpack(str[start:end])
01742 start = end
01743 end += length
01744 if python3:
01745 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01746 else:
01747 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01748 start = end
01749 end += 4
01750 (length,) = _struct_I.unpack(str[start:end])
01751 self.grasp.pre_grasp_posture.name = []
01752 for i in range(0, length):
01753 start = end
01754 end += 4
01755 (length,) = _struct_I.unpack(str[start:end])
01756 start = end
01757 end += length
01758 if python3:
01759 val1 = str[start:end].decode('utf-8')
01760 else:
01761 val1 = str[start:end]
01762 self.grasp.pre_grasp_posture.name.append(val1)
01763 start = end
01764 end += 4
01765 (length,) = _struct_I.unpack(str[start:end])
01766 pattern = '<%sd'%length
01767 start = end
01768 end += struct.calcsize(pattern)
01769 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01770 start = end
01771 end += 4
01772 (length,) = _struct_I.unpack(str[start:end])
01773 pattern = '<%sd'%length
01774 start = end
01775 end += struct.calcsize(pattern)
01776 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01777 start = end
01778 end += 4
01779 (length,) = _struct_I.unpack(str[start:end])
01780 pattern = '<%sd'%length
01781 start = end
01782 end += struct.calcsize(pattern)
01783 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01784 _x = self
01785 start = end
01786 end += 12
01787 (_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])
01788 start = end
01789 end += 4
01790 (length,) = _struct_I.unpack(str[start:end])
01791 start = end
01792 end += length
01793 if python3:
01794 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01795 else:
01796 self.grasp.grasp_posture.header.frame_id = str[start:end]
01797 start = end
01798 end += 4
01799 (length,) = _struct_I.unpack(str[start:end])
01800 self.grasp.grasp_posture.name = []
01801 for i in range(0, length):
01802 start = end
01803 end += 4
01804 (length,) = _struct_I.unpack(str[start:end])
01805 start = end
01806 end += length
01807 if python3:
01808 val1 = str[start:end].decode('utf-8')
01809 else:
01810 val1 = str[start:end]
01811 self.grasp.grasp_posture.name.append(val1)
01812 start = end
01813 end += 4
01814 (length,) = _struct_I.unpack(str[start:end])
01815 pattern = '<%sd'%length
01816 start = end
01817 end += struct.calcsize(pattern)
01818 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01819 start = end
01820 end += 4
01821 (length,) = _struct_I.unpack(str[start:end])
01822 pattern = '<%sd'%length
01823 start = end
01824 end += struct.calcsize(pattern)
01825 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01826 start = end
01827 end += 4
01828 (length,) = _struct_I.unpack(str[start:end])
01829 pattern = '<%sd'%length
01830 start = end
01831 end += struct.calcsize(pattern)
01832 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01833 _x = self
01834 start = end
01835 end += 12
01836 (_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])
01837 start = end
01838 end += 4
01839 (length,) = _struct_I.unpack(str[start:end])
01840 start = end
01841 end += length
01842 if python3:
01843 self.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
01844 else:
01845 self.grasp.grasp_pose.header.frame_id = str[start:end]
01846 _x = self
01847 start = end
01848 end += 76
01849 (_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])
01850 start = end
01851 end += 4
01852 (length,) = _struct_I.unpack(str[start:end])
01853 start = end
01854 end += length
01855 if python3:
01856 self.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01857 else:
01858 self.grasp.approach.direction.header.frame_id = str[start:end]
01859 _x = self
01860 start = end
01861 end += 44
01862 (_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])
01863 start = end
01864 end += 4
01865 (length,) = _struct_I.unpack(str[start:end])
01866 start = end
01867 end += length
01868 if python3:
01869 self.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
01870 else:
01871 self.grasp.retreat.direction.header.frame_id = str[start:end]
01872 _x = self
01873 start = end
01874 end += 36
01875 (_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])
01876 start = end
01877 end += 4
01878 (length,) = _struct_I.unpack(str[start:end])
01879 self.grasp.allowed_touch_objects = []
01880 for i in range(0, length):
01881 start = end
01882 end += 4
01883 (length,) = _struct_I.unpack(str[start:end])
01884 start = end
01885 end += length
01886 if python3:
01887 val1 = str[start:end].decode('utf-8')
01888 else:
01889 val1 = str[start:end]
01890 self.grasp.allowed_touch_objects.append(val1)
01891 _x = self
01892 start = end
01893 end += 20
01894 (_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_2f3I.unpack(str[start:end])
01895 start = end
01896 end += 4
01897 (length,) = _struct_I.unpack(str[start:end])
01898 start = end
01899 end += length
01900 if python3:
01901 self.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01902 else:
01903 self.approach.direction.header.frame_id = str[start:end]
01904 _x = self
01905 start = end
01906 end += 32
01907 (_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])
01908 start = end
01909 end += 4
01910 (length,) = _struct_I.unpack(str[start:end])
01911 start = end
01912 end += length
01913 if python3:
01914 self.collision_object_name = str[start:end].decode('utf-8')
01915 else:
01916 self.collision_object_name = str[start:end]
01917 start = end
01918 end += 4
01919 (length,) = _struct_I.unpack(str[start:end])
01920 start = end
01921 end += length
01922 if python3:
01923 self.collision_support_surface_name = str[start:end].decode('utf-8')
01924 else:
01925 self.collision_support_surface_name = str[start:end]
01926 _x = self
01927 start = end
01928 end += 11
01929 (_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end])
01930 self.allow_gripper_support_collision = bool(self.allow_gripper_support_collision)
01931 self.use_reactive_place = bool(self.use_reactive_place)
01932 self.only_perform_feasibility_test = bool(self.only_perform_feasibility_test)
01933 start = end
01934 end += 4
01935 (length,) = _struct_I.unpack(str[start:end])
01936 self.path_constraints.joint_constraints = []
01937 for i in range(0, length):
01938 val1 = arm_navigation_msgs.msg.JointConstraint()
01939 start = end
01940 end += 4
01941 (length,) = _struct_I.unpack(str[start:end])
01942 start = end
01943 end += length
01944 if python3:
01945 val1.joint_name = str[start:end].decode('utf-8')
01946 else:
01947 val1.joint_name = str[start:end]
01948 _x = val1
01949 start = end
01950 end += 32
01951 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01952 self.path_constraints.joint_constraints.append(val1)
01953 start = end
01954 end += 4
01955 (length,) = _struct_I.unpack(str[start:end])
01956 self.path_constraints.position_constraints = []
01957 for i in range(0, length):
01958 val1 = arm_navigation_msgs.msg.PositionConstraint()
01959 _v84 = val1.header
01960 start = end
01961 end += 4
01962 (_v84.seq,) = _struct_I.unpack(str[start:end])
01963 _v85 = _v84.stamp
01964 _x = _v85
01965 start = end
01966 end += 8
01967 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01968 start = end
01969 end += 4
01970 (length,) = _struct_I.unpack(str[start:end])
01971 start = end
01972 end += length
01973 if python3:
01974 _v84.frame_id = str[start:end].decode('utf-8')
01975 else:
01976 _v84.frame_id = str[start:end]
01977 start = end
01978 end += 4
01979 (length,) = _struct_I.unpack(str[start:end])
01980 start = end
01981 end += length
01982 if python3:
01983 val1.link_name = str[start:end].decode('utf-8')
01984 else:
01985 val1.link_name = str[start:end]
01986 _v86 = val1.target_point_offset
01987 _x = _v86
01988 start = end
01989 end += 24
01990 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01991 _v87 = val1.position
01992 _x = _v87
01993 start = end
01994 end += 24
01995 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01996 _v88 = val1.constraint_region_shape
01997 start = end
01998 end += 1
01999 (_v88.type,) = _struct_b.unpack(str[start:end])
02000 start = end
02001 end += 4
02002 (length,) = _struct_I.unpack(str[start:end])
02003 pattern = '<%sd'%length
02004 start = end
02005 end += struct.calcsize(pattern)
02006 _v88.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02007 start = end
02008 end += 4
02009 (length,) = _struct_I.unpack(str[start:end])
02010 pattern = '<%si'%length
02011 start = end
02012 end += struct.calcsize(pattern)
02013 _v88.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02014 start = end
02015 end += 4
02016 (length,) = _struct_I.unpack(str[start:end])
02017 _v88.vertices = []
02018 for i in range(0, length):
02019 val3 = geometry_msgs.msg.Point()
02020 _x = val3
02021 start = end
02022 end += 24
02023 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02024 _v88.vertices.append(val3)
02025 _v89 = val1.constraint_region_orientation
02026 _x = _v89
02027 start = end
02028 end += 32
02029 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02030 start = end
02031 end += 8
02032 (val1.weight,) = _struct_d.unpack(str[start:end])
02033 self.path_constraints.position_constraints.append(val1)
02034 start = end
02035 end += 4
02036 (length,) = _struct_I.unpack(str[start:end])
02037 self.path_constraints.orientation_constraints = []
02038 for i in range(0, length):
02039 val1 = arm_navigation_msgs.msg.OrientationConstraint()
02040 _v90 = val1.header
02041 start = end
02042 end += 4
02043 (_v90.seq,) = _struct_I.unpack(str[start:end])
02044 _v91 = _v90.stamp
02045 _x = _v91
02046 start = end
02047 end += 8
02048 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02049 start = end
02050 end += 4
02051 (length,) = _struct_I.unpack(str[start:end])
02052 start = end
02053 end += length
02054 if python3:
02055 _v90.frame_id = str[start:end].decode('utf-8')
02056 else:
02057 _v90.frame_id = str[start:end]
02058 start = end
02059 end += 4
02060 (length,) = _struct_I.unpack(str[start:end])
02061 start = end
02062 end += length
02063 if python3:
02064 val1.link_name = str[start:end].decode('utf-8')
02065 else:
02066 val1.link_name = str[start:end]
02067 start = end
02068 end += 4
02069 (val1.type,) = _struct_i.unpack(str[start:end])
02070 _v92 = val1.orientation
02071 _x = _v92
02072 start = end
02073 end += 32
02074 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02075 _x = val1
02076 start = end
02077 end += 32
02078 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02079 self.path_constraints.orientation_constraints.append(val1)
02080 start = end
02081 end += 4
02082 (length,) = _struct_I.unpack(str[start:end])
02083 self.path_constraints.visibility_constraints = []
02084 for i in range(0, length):
02085 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02086 _v93 = val1.header
02087 start = end
02088 end += 4
02089 (_v93.seq,) = _struct_I.unpack(str[start:end])
02090 _v94 = _v93.stamp
02091 _x = _v94
02092 start = end
02093 end += 8
02094 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02095 start = end
02096 end += 4
02097 (length,) = _struct_I.unpack(str[start:end])
02098 start = end
02099 end += length
02100 if python3:
02101 _v93.frame_id = str[start:end].decode('utf-8')
02102 else:
02103 _v93.frame_id = str[start:end]
02104 _v95 = val1.target
02105 _v96 = _v95.header
02106 start = end
02107 end += 4
02108 (_v96.seq,) = _struct_I.unpack(str[start:end])
02109 _v97 = _v96.stamp
02110 _x = _v97
02111 start = end
02112 end += 8
02113 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02114 start = end
02115 end += 4
02116 (length,) = _struct_I.unpack(str[start:end])
02117 start = end
02118 end += length
02119 if python3:
02120 _v96.frame_id = str[start:end].decode('utf-8')
02121 else:
02122 _v96.frame_id = str[start:end]
02123 _v98 = _v95.point
02124 _x = _v98
02125 start = end
02126 end += 24
02127 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02128 _v99 = val1.sensor_pose
02129 _v100 = _v99.header
02130 start = end
02131 end += 4
02132 (_v100.seq,) = _struct_I.unpack(str[start:end])
02133 _v101 = _v100.stamp
02134 _x = _v101
02135 start = end
02136 end += 8
02137 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02138 start = end
02139 end += 4
02140 (length,) = _struct_I.unpack(str[start:end])
02141 start = end
02142 end += length
02143 if python3:
02144 _v100.frame_id = str[start:end].decode('utf-8')
02145 else:
02146 _v100.frame_id = str[start:end]
02147 _v102 = _v99.pose
02148 _v103 = _v102.position
02149 _x = _v103
02150 start = end
02151 end += 24
02152 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02153 _v104 = _v102.orientation
02154 _x = _v104
02155 start = end
02156 end += 32
02157 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02158 start = end
02159 end += 8
02160 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02161 self.path_constraints.visibility_constraints.append(val1)
02162 start = end
02163 end += 4
02164 (length,) = _struct_I.unpack(str[start:end])
02165 self.additional_collision_operations.collision_operations = []
02166 for i in range(0, length):
02167 val1 = arm_navigation_msgs.msg.CollisionOperation()
02168 start = end
02169 end += 4
02170 (length,) = _struct_I.unpack(str[start:end])
02171 start = end
02172 end += length
02173 if python3:
02174 val1.object1 = str[start:end].decode('utf-8')
02175 else:
02176 val1.object1 = str[start:end]
02177 start = end
02178 end += 4
02179 (length,) = _struct_I.unpack(str[start:end])
02180 start = end
02181 end += length
02182 if python3:
02183 val1.object2 = str[start:end].decode('utf-8')
02184 else:
02185 val1.object2 = str[start:end]
02186 _x = val1
02187 start = end
02188 end += 12
02189 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
02190 self.additional_collision_operations.collision_operations.append(val1)
02191 start = end
02192 end += 4
02193 (length,) = _struct_I.unpack(str[start:end])
02194 self.additional_link_padding = []
02195 for i in range(0, length):
02196 val1 = arm_navigation_msgs.msg.LinkPadding()
02197 start = end
02198 end += 4
02199 (length,) = _struct_I.unpack(str[start:end])
02200 start = end
02201 end += length
02202 if python3:
02203 val1.link_name = str[start:end].decode('utf-8')
02204 else:
02205 val1.link_name = str[start:end]
02206 start = end
02207 end += 8
02208 (val1.padding,) = _struct_d.unpack(str[start:end])
02209 self.additional_link_padding.append(val1)
02210 return self
02211 except struct.error as e:
02212 raise genpy.DeserializationError(e)
02213
02214 _struct_I = genpy.struct_I
02215 _struct_b = struct.Struct("<b")
02216 _struct_2f3I = struct.Struct("<2f3I")
02217 _struct_d = struct.Struct("<d")
02218 _struct_di = struct.Struct("<di")
02219 _struct_i = struct.Struct("<i")
02220 _struct_3d2f = struct.Struct("<3d2f")
02221 _struct_3d3f = struct.Struct("<3d3f")
02222 _struct_8d3I = struct.Struct("<8d3I")
02223 _struct_3I = struct.Struct("<3I")
02224 _struct_2BdB = struct.Struct("<2BdB")
02225 _struct_3d2f3I = struct.Struct("<3d2f3I")
02226 _struct_4d = struct.Struct("<4d")
02227 _struct_2I = struct.Struct("<2I")
02228 _struct_3d = struct.Struct("<3d")