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