_PlaceGoal.py
Go to the documentation of this file.
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 #flag to mark the presence of a Header object
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       #message fields cannot be None, assign default values for those that are
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) #most likely buffer underfill
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) #most likely buffer underfill
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")


object_manipulation_msgs
Author(s): Matei Ciocarlie
autogenerated on Mon Oct 6 2014 02:58:11