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