00001 """autogenerated by genpy from object_manipulation_msgs/PlaceAction.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import arm_navigation_msgs.msg
00008 import object_manipulation_msgs.msg
00009 import actionlib_msgs.msg
00010 import manipulation_msgs.msg
00011 import geometry_msgs.msg
00012 import sensor_msgs.msg
00013 import genpy
00014 import std_msgs.msg
00015
00016 class PlaceAction(genpy.Message):
00017 _md5sum = "20827ed348658285b3d2f325f128e609"
00018 _type = "object_manipulation_msgs/PlaceAction"
00019 _has_header = False
00020 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00021
00022 PlaceActionGoal action_goal
00023 PlaceActionResult action_result
00024 PlaceActionFeedback action_feedback
00025
00026 ================================================================================
00027 MSG: object_manipulation_msgs/PlaceActionGoal
00028 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00029
00030 Header header
00031 actionlib_msgs/GoalID goal_id
00032 PlaceGoal goal
00033
00034 ================================================================================
00035 MSG: std_msgs/Header
00036 # Standard metadata for higher-level stamped data types.
00037 # This is generally used to communicate timestamped data
00038 # in a particular coordinate frame.
00039 #
00040 # sequence ID: consecutively increasing ID
00041 uint32 seq
00042 #Two-integer timestamp that is expressed as:
00043 # * stamp.secs: seconds (stamp_secs) since epoch
00044 # * stamp.nsecs: nanoseconds since stamp_secs
00045 # time-handling sugar is provided by the client library
00046 time stamp
00047 #Frame this data is associated with
00048 # 0: no frame
00049 # 1: global frame
00050 string frame_id
00051
00052 ================================================================================
00053 MSG: actionlib_msgs/GoalID
00054 # The stamp should store the time at which this goal was requested.
00055 # It is used by an action server when it tries to preempt all
00056 # goals that were requested before a certain time
00057 time stamp
00058
00059 # The id provides a way to associate feedback and
00060 # result message with specific goal requests. The id
00061 # specified must be unique.
00062 string id
00063
00064
00065 ================================================================================
00066 MSG: object_manipulation_msgs/PlaceGoal
00067 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00068 # An action for placing an object
00069
00070 # which arm to be used for grasping
00071 string arm_name
00072
00073 # a list of possible locations for the gripper when placing
00074 # note that these refer to where the *gripper* should go when the object is placed
00075 # it is the job of the caller to make sure that these are set to that the *object*
00076 # ends up in a desired location
00077 geometry_msgs/PoseStamped[] place_locations
00078
00079 # the grasp that has been executed on this object
00080 # (contains the grasp_pose referred to above)
00081 manipulation_msgs/Grasp grasp
00082
00083 # how far the retreat should ideally be away from the place location
00084 float32 desired_retreat_distance
00085
00086 # the min distance between the retreat and the place location that must actually be feasible
00087 # for the place not to be rejected
00088 float32 min_retreat_distance
00089
00090 # how the place location should be approached
00091 # the frame_id that this lift is specified in MUST be either the robot_frame
00092 # or the gripper_frame specified in your hand description file
00093 GripperTranslation approach
00094
00095 # the name that the target object has in the collision map
00096 # can be left empty if no name is available
00097 string collision_object_name
00098
00099 # the name that the support surface (e.g. table) has in the collision map
00100 # can be left empty if no name is available
00101 string collision_support_surface_name
00102
00103 # whether collisions between the gripper and the support surface should be acceptable
00104 # during move from pre-place to place and during retreat. Collisions when moving to the
00105 # pre-place location are still not allowed even if this is set to true.
00106 bool allow_gripper_support_collision
00107
00108 # whether reactive placing based on tactile sensors should be used
00109 bool use_reactive_place
00110
00111 # how much the object should be padded by when deciding if the grasp
00112 # location is freasible or not
00113 float64 place_padding
00114
00115 # set this to true if you only want to query the manipulation pipeline as to what
00116 # place locations it thinks are feasible, without actually executing them. If this is set to
00117 # true, the atempted_location_results field of the result will be populated, but no arm
00118 # movement will be attempted
00119 bool only_perform_feasibility_test
00120
00121 # OPTIONAL (These will not have to be filled out most of the time)
00122 # constraints to be imposed on every point in the motion of the arm
00123 arm_navigation_msgs/Constraints path_constraints
00124
00125 # OPTIONAL (These will not have to be filled out most of the time)
00126 # additional collision operations to be used for every arm movement performed
00127 # during placing. Note that these will be added on top of (and thus overide) other
00128 # collision operations that the grasping pipeline deems necessary. Should be used
00129 # with care and only if special behaviors are desired.
00130 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations
00131
00132 # OPTIONAL (These will not have to be filled out most of the time)
00133 # additional link paddings to be used for every arm movement performed
00134 # during placing. Note that these will be added on top of (and thus overide) other
00135 # link paddings that the grasping pipeline deems necessary. Should be used
00136 # with care and only if special behaviors are desired.
00137 arm_navigation_msgs/LinkPadding[] additional_link_padding
00138
00139
00140 ================================================================================
00141 MSG: geometry_msgs/PoseStamped
00142 # A Pose with reference coordinate frame and timestamp
00143 Header header
00144 Pose pose
00145
00146 ================================================================================
00147 MSG: geometry_msgs/Pose
00148 # A representation of pose in free space, composed of postion and orientation.
00149 Point position
00150 Quaternion orientation
00151
00152 ================================================================================
00153 MSG: geometry_msgs/Point
00154 # This contains the position of a point in free space
00155 float64 x
00156 float64 y
00157 float64 z
00158
00159 ================================================================================
00160 MSG: geometry_msgs/Quaternion
00161 # This represents an orientation in free space in quaternion form.
00162
00163 float64 x
00164 float64 y
00165 float64 z
00166 float64 w
00167
00168 ================================================================================
00169 MSG: manipulation_msgs/Grasp
00170 # A name for this grasp
00171 string id
00172
00173 # The internal posture of the hand for the pre-grasp
00174 # only positions are used
00175 sensor_msgs/JointState pre_grasp_posture
00176
00177 # The internal posture of the hand for the grasp
00178 # positions and efforts are used
00179 sensor_msgs/JointState grasp_posture
00180
00181 # The position of the end-effector for the grasp relative to a reference frame
00182 # (that is always specified elsewhere, not in this message)
00183 geometry_msgs/PoseStamped grasp_pose
00184
00185 # The estimated probability of success for this grasp, or some other
00186 # measure of how "good" it is.
00187 float64 grasp_quality
00188
00189 # The approach motion
00190 GripperTranslation approach
00191
00192 # The retreat motion
00193 GripperTranslation retreat
00194
00195 # the maximum contact force to use while grasping (<=0 to disable)
00196 float32 max_contact_force
00197
00198 # an optional list of obstacles that we have semantic information about
00199 # and that can be touched/pushed/moved in the course of grasping
00200 string[] allowed_touch_objects
00201
00202 ================================================================================
00203 MSG: sensor_msgs/JointState
00204 # This is a message that holds data to describe the state of a set of torque controlled joints.
00205 #
00206 # The state of each joint (revolute or prismatic) is defined by:
00207 # * the position of the joint (rad or m),
00208 # * the velocity of the joint (rad/s or m/s) and
00209 # * the effort that is applied in the joint (Nm or N).
00210 #
00211 # Each joint is uniquely identified by its name
00212 # The header specifies the time at which the joint states were recorded. All the joint states
00213 # in one message have to be recorded at the same time.
00214 #
00215 # This message consists of a multiple arrays, one for each part of the joint state.
00216 # The goal is to make each of the fields optional. When e.g. your joints have no
00217 # effort associated with them, you can leave the effort array empty.
00218 #
00219 # All arrays in this message should have the same size, or be empty.
00220 # This is the only way to uniquely associate the joint name with the correct
00221 # states.
00222
00223
00224 Header header
00225
00226 string[] name
00227 float64[] position
00228 float64[] velocity
00229 float64[] effort
00230
00231 ================================================================================
00232 MSG: manipulation_msgs/GripperTranslation
00233 # defines a translation for the gripper, used in pickup or place tasks
00234 # for example for lifting an object off a table or approaching the table for placing
00235
00236 # the direction of the translation
00237 geometry_msgs/Vector3Stamped direction
00238
00239 # the desired translation distance
00240 float32 desired_distance
00241
00242 # the min distance that must be considered feasible before the
00243 # grasp is even attempted
00244 float32 min_distance
00245
00246 ================================================================================
00247 MSG: geometry_msgs/Vector3Stamped
00248 # This represents a Vector3 with reference coordinate frame and timestamp
00249 Header header
00250 Vector3 vector
00251
00252 ================================================================================
00253 MSG: geometry_msgs/Vector3
00254 # This represents a vector in free space.
00255
00256 float64 x
00257 float64 y
00258 float64 z
00259 ================================================================================
00260 MSG: object_manipulation_msgs/GripperTranslation
00261 # defines a translation for the gripper, used in pickup or place tasks
00262 # for example for lifting an object off a table or approaching the table for placing
00263
00264 # the direction of the translation
00265 geometry_msgs/Vector3Stamped direction
00266
00267 # the desired translation distance
00268 float32 desired_distance
00269
00270 # the min distance that must be considered feasible before the
00271 # grasp is even attempted
00272 float32 min_distance
00273 ================================================================================
00274 MSG: arm_navigation_msgs/Constraints
00275 # This message contains a list of motion planning constraints.
00276
00277 arm_navigation_msgs/JointConstraint[] joint_constraints
00278 arm_navigation_msgs/PositionConstraint[] position_constraints
00279 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00280 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00281
00282 ================================================================================
00283 MSG: arm_navigation_msgs/JointConstraint
00284 # Constrain the position of a joint to be within a certain bound
00285 string joint_name
00286
00287 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00288 float64 position
00289 float64 tolerance_above
00290 float64 tolerance_below
00291
00292 # A weighting factor for this constraint
00293 float64 weight
00294 ================================================================================
00295 MSG: arm_navigation_msgs/PositionConstraint
00296 # This message contains the definition of a position constraint.
00297 Header header
00298
00299 # The robot link this constraint refers to
00300 string link_name
00301
00302 # The offset (in the link frame) for the target point on the link we are planning for
00303 geometry_msgs/Point target_point_offset
00304
00305 # The nominal/target position for the point we are planning for
00306 geometry_msgs/Point position
00307
00308 # The shape of the bounded region that constrains the position of the end-effector
00309 # This region is always centered at the position defined above
00310 arm_navigation_msgs/Shape constraint_region_shape
00311
00312 # The orientation of the bounded region that constrains the position of the end-effector.
00313 # This allows the specification of non-axis aligned constraints
00314 geometry_msgs/Quaternion constraint_region_orientation
00315
00316 # Constraint weighting factor - a weight for this constraint
00317 float64 weight
00318
00319 ================================================================================
00320 MSG: arm_navigation_msgs/Shape
00321 byte SPHERE=0
00322 byte BOX=1
00323 byte CYLINDER=2
00324 byte MESH=3
00325
00326 byte type
00327
00328
00329 #### define sphere, box, cylinder ####
00330 # the origin of each shape is considered at the shape's center
00331
00332 # for sphere
00333 # radius := dimensions[0]
00334
00335 # for cylinder
00336 # radius := dimensions[0]
00337 # length := dimensions[1]
00338 # the length is along the Z axis
00339
00340 # for box
00341 # size_x := dimensions[0]
00342 # size_y := dimensions[1]
00343 # size_z := dimensions[2]
00344 float64[] dimensions
00345
00346
00347 #### define mesh ####
00348
00349 # list of triangles; triangle k is defined by tre vertices located
00350 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00351 int32[] triangles
00352 geometry_msgs/Point[] vertices
00353
00354 ================================================================================
00355 MSG: arm_navigation_msgs/OrientationConstraint
00356 # This message contains the definition of an orientation constraint.
00357 Header header
00358
00359 # The robot link this constraint refers to
00360 string link_name
00361
00362 # The type of the constraint
00363 int32 type
00364 int32 LINK_FRAME=0
00365 int32 HEADER_FRAME=1
00366
00367 # The desired orientation of the robot link specified as a quaternion
00368 geometry_msgs/Quaternion orientation
00369
00370 # optional RPY error tolerances specified if
00371 float64 absolute_roll_tolerance
00372 float64 absolute_pitch_tolerance
00373 float64 absolute_yaw_tolerance
00374
00375 # Constraint weighting factor - a weight for this constraint
00376 float64 weight
00377
00378 ================================================================================
00379 MSG: arm_navigation_msgs/VisibilityConstraint
00380 # This message contains the definition of a visibility constraint.
00381 Header header
00382
00383 # The point stamped target that needs to be kept within view of the sensor
00384 geometry_msgs/PointStamped target
00385
00386 # The local pose of the frame in which visibility is to be maintained
00387 # The frame id should represent the robot link to which the sensor is attached
00388 # The visual axis of the sensor is assumed to be along the X axis of this frame
00389 geometry_msgs/PoseStamped sensor_pose
00390
00391 # The deviation (in radians) that will be tolerated
00392 # Constraint error will be measured as the solid angle between the
00393 # X axis of the frame defined above and the vector between the origin
00394 # of the frame defined above and the target location
00395 float64 absolute_tolerance
00396
00397
00398 ================================================================================
00399 MSG: geometry_msgs/PointStamped
00400 # This represents a Point with reference coordinate frame and timestamp
00401 Header header
00402 Point point
00403
00404 ================================================================================
00405 MSG: arm_navigation_msgs/OrderedCollisionOperations
00406 # A set of collision operations that will be performed in the order they are specified
00407 CollisionOperation[] collision_operations
00408 ================================================================================
00409 MSG: arm_navigation_msgs/CollisionOperation
00410 # A definition of a collision operation
00411 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions
00412 # between the gripper and all objects in the collision space
00413
00414 string object1
00415 string object2
00416 string COLLISION_SET_ALL="all"
00417 string COLLISION_SET_OBJECTS="objects"
00418 string COLLISION_SET_ATTACHED_OBJECTS="attached"
00419
00420 # The penetration distance to which collisions are allowed. This is 0.0 by default.
00421 float64 penetration_distance
00422
00423 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above
00424 int32 operation
00425 int32 DISABLE=0
00426 int32 ENABLE=1
00427
00428 ================================================================================
00429 MSG: arm_navigation_msgs/LinkPadding
00430 #name for the link
00431 string link_name
00432
00433 # padding to apply to the link
00434 float64 padding
00435
00436 ================================================================================
00437 MSG: object_manipulation_msgs/PlaceActionResult
00438 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00439
00440 Header header
00441 actionlib_msgs/GoalStatus status
00442 PlaceResult result
00443
00444 ================================================================================
00445 MSG: actionlib_msgs/GoalStatus
00446 GoalID goal_id
00447 uint8 status
00448 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00449 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00450 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00451 # and has since completed its execution (Terminal State)
00452 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00453 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00454 # to some failure (Terminal State)
00455 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00456 # because the goal was unattainable or invalid (Terminal State)
00457 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00458 # and has not yet completed execution
00459 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00460 # but the action server has not yet confirmed that the goal is canceled
00461 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00462 # and was successfully cancelled (Terminal State)
00463 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00464 # sent over the wire by an action server
00465
00466 #Allow for the user to associate a string with GoalStatus for debugging
00467 string text
00468
00469
00470 ================================================================================
00471 MSG: object_manipulation_msgs/PlaceResult
00472 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00473
00474 # The result of the pickup attempt
00475 ManipulationResult manipulation_result
00476
00477 # The successful place location, if any
00478 geometry_msgs/PoseStamped place_location
00479
00480 # the list of attempted locations, in the order in which they were attempted
00481 # the successful one should be the last one in this list
00482 geometry_msgs/PoseStamped[] attempted_locations
00483
00484 # the outcomes of the attempted locations, in the same order as attempted_locations
00485 PlaceLocationResult[] attempted_location_results
00486
00487
00488 ================================================================================
00489 MSG: object_manipulation_msgs/ManipulationResult
00490 # Result codes for manipulation tasks
00491
00492 # task completed as expected
00493 # generally means you can proceed as planned
00494 int32 SUCCESS = 1
00495
00496 # task not possible (e.g. out of reach or obstacles in the way)
00497 # generally means that the world was not disturbed, so you can try another task
00498 int32 UNFEASIBLE = -1
00499
00500 # task was thought possible, but failed due to unexpected events during execution
00501 # it is likely that the world was disturbed, so you are encouraged to refresh
00502 # your sensed world model before proceeding to another task
00503 int32 FAILED = -2
00504
00505 # a lower level error prevented task completion (e.g. joint controller not responding)
00506 # generally requires human attention
00507 int32 ERROR = -3
00508
00509 # means that at some point during execution we ended up in a state that the collision-aware
00510 # arm navigation module will not move out of. The world was likely not disturbed, but you
00511 # probably need a new collision map to move the arm out of the stuck position
00512 int32 ARM_MOVEMENT_PREVENTED = -4
00513
00514 # specific to grasp actions
00515 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00516 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00517 int32 LIFT_FAILED = -5
00518
00519 # specific to place actions
00520 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00521 # it is likely that the collision environment will see collisions between the hand and the object
00522 int32 RETREAT_FAILED = -6
00523
00524 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00525 int32 CANCELLED = -7
00526
00527 # the actual value of this error code
00528 int32 value
00529
00530 ================================================================================
00531 MSG: object_manipulation_msgs/PlaceLocationResult
00532 int32 SUCCESS = 1
00533 int32 PLACE_OUT_OF_REACH = 2
00534 int32 PLACE_IN_COLLISION = 3
00535 int32 PLACE_UNFEASIBLE = 4
00536 int32 PREPLACE_OUT_OF_REACH = 5
00537 int32 PREPLACE_IN_COLLISION = 6
00538 int32 PREPLACE_UNFEASIBLE = 7
00539 int32 RETREAT_OUT_OF_REACH = 8
00540 int32 RETREAT_IN_COLLISION = 9
00541 int32 RETREAT_UNFEASIBLE = 10
00542 int32 MOVE_ARM_FAILED = 11
00543 int32 PLACE_FAILED = 12
00544 int32 RETREAT_FAILED = 13
00545 int32 result_code
00546
00547 # whether the state of the world was disturbed by this attempt. generally, this flag
00548 # shows if another task can be attempted, or a new sensed world model is recommeded
00549 # before proceeding
00550 bool continuation_possible
00551
00552 ================================================================================
00553 MSG: object_manipulation_msgs/PlaceActionFeedback
00554 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00555
00556 Header header
00557 actionlib_msgs/GoalStatus status
00558 PlaceFeedback feedback
00559
00560 ================================================================================
00561 MSG: object_manipulation_msgs/PlaceFeedback
00562 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00563
00564 # The number of the place location currently being attempted
00565 int32 current_location
00566
00567 # The total number of locations that will be attempted
00568 int32 total_locations
00569
00570
00571 """
00572 __slots__ = ['action_goal','action_result','action_feedback']
00573 _slot_types = ['object_manipulation_msgs/PlaceActionGoal','object_manipulation_msgs/PlaceActionResult','object_manipulation_msgs/PlaceActionFeedback']
00574
00575 def __init__(self, *args, **kwds):
00576 """
00577 Constructor. Any message fields that are implicitly/explicitly
00578 set to None will be assigned a default value. The recommend
00579 use is keyword arguments as this is more robust to future message
00580 changes. You cannot mix in-order arguments and keyword arguments.
00581
00582 The available fields are:
00583 action_goal,action_result,action_feedback
00584
00585 :param args: complete set of field values, in .msg order
00586 :param kwds: use keyword arguments corresponding to message field names
00587 to set specific fields.
00588 """
00589 if args or kwds:
00590 super(PlaceAction, self).__init__(*args, **kwds)
00591
00592 if self.action_goal is None:
00593 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal()
00594 if self.action_result is None:
00595 self.action_result = object_manipulation_msgs.msg.PlaceActionResult()
00596 if self.action_feedback is None:
00597 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback()
00598 else:
00599 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal()
00600 self.action_result = object_manipulation_msgs.msg.PlaceActionResult()
00601 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback()
00602
00603 def _get_types(self):
00604 """
00605 internal API method
00606 """
00607 return self._slot_types
00608
00609 def serialize(self, buff):
00610 """
00611 serialize message into buffer
00612 :param buff: buffer, ``StringIO``
00613 """
00614 try:
00615 _x = self
00616 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00617 _x = self.action_goal.header.frame_id
00618 length = len(_x)
00619 if python3 or type(_x) == unicode:
00620 _x = _x.encode('utf-8')
00621 length = len(_x)
00622 buff.write(struct.pack('<I%ss'%length, length, _x))
00623 _x = self
00624 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00625 _x = self.action_goal.goal_id.id
00626 length = len(_x)
00627 if python3 or type(_x) == unicode:
00628 _x = _x.encode('utf-8')
00629 length = len(_x)
00630 buff.write(struct.pack('<I%ss'%length, length, _x))
00631 _x = self.action_goal.goal.arm_name
00632 length = len(_x)
00633 if python3 or type(_x) == unicode:
00634 _x = _x.encode('utf-8')
00635 length = len(_x)
00636 buff.write(struct.pack('<I%ss'%length, length, _x))
00637 length = len(self.action_goal.goal.place_locations)
00638 buff.write(_struct_I.pack(length))
00639 for val1 in self.action_goal.goal.place_locations:
00640 _v1 = val1.header
00641 buff.write(_struct_I.pack(_v1.seq))
00642 _v2 = _v1.stamp
00643 _x = _v2
00644 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00645 _x = _v1.frame_id
00646 length = len(_x)
00647 if python3 or type(_x) == unicode:
00648 _x = _x.encode('utf-8')
00649 length = len(_x)
00650 buff.write(struct.pack('<I%ss'%length, length, _x))
00651 _v3 = val1.pose
00652 _v4 = _v3.position
00653 _x = _v4
00654 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00655 _v5 = _v3.orientation
00656 _x = _v5
00657 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00658 _x = self.action_goal.goal.grasp.id
00659 length = len(_x)
00660 if python3 or type(_x) == unicode:
00661 _x = _x.encode('utf-8')
00662 length = len(_x)
00663 buff.write(struct.pack('<I%ss'%length, length, _x))
00664 _x = self
00665 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))
00666 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id
00667 length = len(_x)
00668 if python3 or type(_x) == unicode:
00669 _x = _x.encode('utf-8')
00670 length = len(_x)
00671 buff.write(struct.pack('<I%ss'%length, length, _x))
00672 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name)
00673 buff.write(_struct_I.pack(length))
00674 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name:
00675 length = len(val1)
00676 if python3 or type(val1) == unicode:
00677 val1 = val1.encode('utf-8')
00678 length = len(val1)
00679 buff.write(struct.pack('<I%ss'%length, length, val1))
00680 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position)
00681 buff.write(_struct_I.pack(length))
00682 pattern = '<%sd'%length
00683 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.position))
00684 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity)
00685 buff.write(_struct_I.pack(length))
00686 pattern = '<%sd'%length
00687 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.velocity))
00688 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort)
00689 buff.write(_struct_I.pack(length))
00690 pattern = '<%sd'%length
00691 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.effort))
00692 _x = self
00693 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))
00694 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id
00695 length = len(_x)
00696 if python3 or type(_x) == unicode:
00697 _x = _x.encode('utf-8')
00698 length = len(_x)
00699 buff.write(struct.pack('<I%ss'%length, length, _x))
00700 length = len(self.action_goal.goal.grasp.grasp_posture.name)
00701 buff.write(_struct_I.pack(length))
00702 for val1 in self.action_goal.goal.grasp.grasp_posture.name:
00703 length = len(val1)
00704 if python3 or type(val1) == unicode:
00705 val1 = val1.encode('utf-8')
00706 length = len(val1)
00707 buff.write(struct.pack('<I%ss'%length, length, val1))
00708 length = len(self.action_goal.goal.grasp.grasp_posture.position)
00709 buff.write(_struct_I.pack(length))
00710 pattern = '<%sd'%length
00711 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.position))
00712 length = len(self.action_goal.goal.grasp.grasp_posture.velocity)
00713 buff.write(_struct_I.pack(length))
00714 pattern = '<%sd'%length
00715 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.velocity))
00716 length = len(self.action_goal.goal.grasp.grasp_posture.effort)
00717 buff.write(_struct_I.pack(length))
00718 pattern = '<%sd'%length
00719 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.effort))
00720 _x = self
00721 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs))
00722 _x = self.action_goal.goal.grasp.grasp_pose.header.frame_id
00723 length = len(_x)
00724 if python3 or type(_x) == unicode:
00725 _x = _x.encode('utf-8')
00726 length = len(_x)
00727 buff.write(struct.pack('<I%ss'%length, length, _x))
00728 _x = self
00729 buff.write(_struct_8d3I.pack(_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs))
00730 _x = self.action_goal.goal.grasp.approach.direction.header.frame_id
00731 length = len(_x)
00732 if python3 or type(_x) == unicode:
00733 _x = _x.encode('utf-8')
00734 length = len(_x)
00735 buff.write(struct.pack('<I%ss'%length, length, _x))
00736 _x = self
00737 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs))
00738 _x = self.action_goal.goal.grasp.retreat.direction.header.frame_id
00739 length = len(_x)
00740 if python3 or type(_x) == unicode:
00741 _x = _x.encode('utf-8')
00742 length = len(_x)
00743 buff.write(struct.pack('<I%ss'%length, length, _x))
00744 _x = self
00745 buff.write(_struct_3d3f.pack(_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force))
00746 length = len(self.action_goal.goal.grasp.allowed_touch_objects)
00747 buff.write(_struct_I.pack(length))
00748 for val1 in self.action_goal.goal.grasp.allowed_touch_objects:
00749 length = len(val1)
00750 if python3 or type(val1) == unicode:
00751 val1 = val1.encode('utf-8')
00752 length = len(val1)
00753 buff.write(struct.pack('<I%ss'%length, length, val1))
00754 _x = self
00755 buff.write(_struct_2f3I.pack(_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))
00756 _x = self.action_goal.goal.approach.direction.header.frame_id
00757 length = len(_x)
00758 if python3 or type(_x) == unicode:
00759 _x = _x.encode('utf-8')
00760 length = len(_x)
00761 buff.write(struct.pack('<I%ss'%length, length, _x))
00762 _x = self
00763 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))
00764 _x = self.action_goal.goal.collision_object_name
00765 length = len(_x)
00766 if python3 or type(_x) == unicode:
00767 _x = _x.encode('utf-8')
00768 length = len(_x)
00769 buff.write(struct.pack('<I%ss'%length, length, _x))
00770 _x = self.action_goal.goal.collision_support_surface_name
00771 length = len(_x)
00772 if python3 or type(_x) == unicode:
00773 _x = _x.encode('utf-8')
00774 length = len(_x)
00775 buff.write(struct.pack('<I%ss'%length, length, _x))
00776 _x = self
00777 buff.write(_struct_2BdB.pack(_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test))
00778 length = len(self.action_goal.goal.path_constraints.joint_constraints)
00779 buff.write(_struct_I.pack(length))
00780 for val1 in self.action_goal.goal.path_constraints.joint_constraints:
00781 _x = val1.joint_name
00782 length = len(_x)
00783 if python3 or type(_x) == unicode:
00784 _x = _x.encode('utf-8')
00785 length = len(_x)
00786 buff.write(struct.pack('<I%ss'%length, length, _x))
00787 _x = val1
00788 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00789 length = len(self.action_goal.goal.path_constraints.position_constraints)
00790 buff.write(_struct_I.pack(length))
00791 for val1 in self.action_goal.goal.path_constraints.position_constraints:
00792 _v6 = val1.header
00793 buff.write(_struct_I.pack(_v6.seq))
00794 _v7 = _v6.stamp
00795 _x = _v7
00796 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00797 _x = _v6.frame_id
00798 length = len(_x)
00799 if python3 or type(_x) == unicode:
00800 _x = _x.encode('utf-8')
00801 length = len(_x)
00802 buff.write(struct.pack('<I%ss'%length, length, _x))
00803 _x = val1.link_name
00804 length = len(_x)
00805 if python3 or type(_x) == unicode:
00806 _x = _x.encode('utf-8')
00807 length = len(_x)
00808 buff.write(struct.pack('<I%ss'%length, length, _x))
00809 _v8 = val1.target_point_offset
00810 _x = _v8
00811 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00812 _v9 = val1.position
00813 _x = _v9
00814 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00815 _v10 = val1.constraint_region_shape
00816 buff.write(_struct_b.pack(_v10.type))
00817 length = len(_v10.dimensions)
00818 buff.write(_struct_I.pack(length))
00819 pattern = '<%sd'%length
00820 buff.write(struct.pack(pattern, *_v10.dimensions))
00821 length = len(_v10.triangles)
00822 buff.write(_struct_I.pack(length))
00823 pattern = '<%si'%length
00824 buff.write(struct.pack(pattern, *_v10.triangles))
00825 length = len(_v10.vertices)
00826 buff.write(_struct_I.pack(length))
00827 for val3 in _v10.vertices:
00828 _x = val3
00829 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00830 _v11 = val1.constraint_region_orientation
00831 _x = _v11
00832 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00833 buff.write(_struct_d.pack(val1.weight))
00834 length = len(self.action_goal.goal.path_constraints.orientation_constraints)
00835 buff.write(_struct_I.pack(length))
00836 for val1 in self.action_goal.goal.path_constraints.orientation_constraints:
00837 _v12 = val1.header
00838 buff.write(_struct_I.pack(_v12.seq))
00839 _v13 = _v12.stamp
00840 _x = _v13
00841 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00842 _x = _v12.frame_id
00843 length = len(_x)
00844 if python3 or type(_x) == unicode:
00845 _x = _x.encode('utf-8')
00846 length = len(_x)
00847 buff.write(struct.pack('<I%ss'%length, length, _x))
00848 _x = val1.link_name
00849 length = len(_x)
00850 if python3 or type(_x) == unicode:
00851 _x = _x.encode('utf-8')
00852 length = len(_x)
00853 buff.write(struct.pack('<I%ss'%length, length, _x))
00854 buff.write(_struct_i.pack(val1.type))
00855 _v14 = val1.orientation
00856 _x = _v14
00857 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00858 _x = val1
00859 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00860 length = len(self.action_goal.goal.path_constraints.visibility_constraints)
00861 buff.write(_struct_I.pack(length))
00862 for val1 in self.action_goal.goal.path_constraints.visibility_constraints:
00863 _v15 = val1.header
00864 buff.write(_struct_I.pack(_v15.seq))
00865 _v16 = _v15.stamp
00866 _x = _v16
00867 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00868 _x = _v15.frame_id
00869 length = len(_x)
00870 if python3 or type(_x) == unicode:
00871 _x = _x.encode('utf-8')
00872 length = len(_x)
00873 buff.write(struct.pack('<I%ss'%length, length, _x))
00874 _v17 = val1.target
00875 _v18 = _v17.header
00876 buff.write(_struct_I.pack(_v18.seq))
00877 _v19 = _v18.stamp
00878 _x = _v19
00879 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00880 _x = _v18.frame_id
00881 length = len(_x)
00882 if python3 or type(_x) == unicode:
00883 _x = _x.encode('utf-8')
00884 length = len(_x)
00885 buff.write(struct.pack('<I%ss'%length, length, _x))
00886 _v20 = _v17.point
00887 _x = _v20
00888 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00889 _v21 = val1.sensor_pose
00890 _v22 = _v21.header
00891 buff.write(_struct_I.pack(_v22.seq))
00892 _v23 = _v22.stamp
00893 _x = _v23
00894 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00895 _x = _v22.frame_id
00896 length = len(_x)
00897 if python3 or type(_x) == unicode:
00898 _x = _x.encode('utf-8')
00899 length = len(_x)
00900 buff.write(struct.pack('<I%ss'%length, length, _x))
00901 _v24 = _v21.pose
00902 _v25 = _v24.position
00903 _x = _v25
00904 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00905 _v26 = _v24.orientation
00906 _x = _v26
00907 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00908 buff.write(_struct_d.pack(val1.absolute_tolerance))
00909 length = len(self.action_goal.goal.additional_collision_operations.collision_operations)
00910 buff.write(_struct_I.pack(length))
00911 for val1 in self.action_goal.goal.additional_collision_operations.collision_operations:
00912 _x = val1.object1
00913 length = len(_x)
00914 if python3 or type(_x) == unicode:
00915 _x = _x.encode('utf-8')
00916 length = len(_x)
00917 buff.write(struct.pack('<I%ss'%length, length, _x))
00918 _x = val1.object2
00919 length = len(_x)
00920 if python3 or type(_x) == unicode:
00921 _x = _x.encode('utf-8')
00922 length = len(_x)
00923 buff.write(struct.pack('<I%ss'%length, length, _x))
00924 _x = val1
00925 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
00926 length = len(self.action_goal.goal.additional_link_padding)
00927 buff.write(_struct_I.pack(length))
00928 for val1 in self.action_goal.goal.additional_link_padding:
00929 _x = val1.link_name
00930 length = len(_x)
00931 if python3 or type(_x) == unicode:
00932 _x = _x.encode('utf-8')
00933 length = len(_x)
00934 buff.write(struct.pack('<I%ss'%length, length, _x))
00935 buff.write(_struct_d.pack(val1.padding))
00936 _x = self
00937 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00938 _x = self.action_result.header.frame_id
00939 length = len(_x)
00940 if python3 or type(_x) == unicode:
00941 _x = _x.encode('utf-8')
00942 length = len(_x)
00943 buff.write(struct.pack('<I%ss'%length, length, _x))
00944 _x = self
00945 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00946 _x = self.action_result.status.goal_id.id
00947 length = len(_x)
00948 if python3 or type(_x) == unicode:
00949 _x = _x.encode('utf-8')
00950 length = len(_x)
00951 buff.write(struct.pack('<I%ss'%length, length, _x))
00952 buff.write(_struct_B.pack(self.action_result.status.status))
00953 _x = self.action_result.status.text
00954 length = len(_x)
00955 if python3 or type(_x) == unicode:
00956 _x = _x.encode('utf-8')
00957 length = len(_x)
00958 buff.write(struct.pack('<I%ss'%length, length, _x))
00959 _x = self
00960 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))
00961 _x = self.action_result.result.place_location.header.frame_id
00962 length = len(_x)
00963 if python3 or type(_x) == unicode:
00964 _x = _x.encode('utf-8')
00965 length = len(_x)
00966 buff.write(struct.pack('<I%ss'%length, length, _x))
00967 _x = self
00968 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))
00969 length = len(self.action_result.result.attempted_locations)
00970 buff.write(_struct_I.pack(length))
00971 for val1 in self.action_result.result.attempted_locations:
00972 _v27 = val1.header
00973 buff.write(_struct_I.pack(_v27.seq))
00974 _v28 = _v27.stamp
00975 _x = _v28
00976 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00977 _x = _v27.frame_id
00978 length = len(_x)
00979 if python3 or type(_x) == unicode:
00980 _x = _x.encode('utf-8')
00981 length = len(_x)
00982 buff.write(struct.pack('<I%ss'%length, length, _x))
00983 _v29 = val1.pose
00984 _v30 = _v29.position
00985 _x = _v30
00986 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00987 _v31 = _v29.orientation
00988 _x = _v31
00989 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00990 length = len(self.action_result.result.attempted_location_results)
00991 buff.write(_struct_I.pack(length))
00992 for val1 in self.action_result.result.attempted_location_results:
00993 _x = val1
00994 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
00995 _x = self
00996 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00997 _x = self.action_feedback.header.frame_id
00998 length = len(_x)
00999 if python3 or type(_x) == unicode:
01000 _x = _x.encode('utf-8')
01001 length = len(_x)
01002 buff.write(struct.pack('<I%ss'%length, length, _x))
01003 _x = self
01004 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
01005 _x = self.action_feedback.status.goal_id.id
01006 length = len(_x)
01007 if python3 or type(_x) == unicode:
01008 _x = _x.encode('utf-8')
01009 length = len(_x)
01010 buff.write(struct.pack('<I%ss'%length, length, _x))
01011 buff.write(_struct_B.pack(self.action_feedback.status.status))
01012 _x = self.action_feedback.status.text
01013 length = len(_x)
01014 if python3 or type(_x) == unicode:
01015 _x = _x.encode('utf-8')
01016 length = len(_x)
01017 buff.write(struct.pack('<I%ss'%length, length, _x))
01018 _x = self
01019 buff.write(_struct_2i.pack(_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations))
01020 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
01021 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
01022
01023 def deserialize(self, str):
01024 """
01025 unpack serialized message in str into this message instance
01026 :param str: byte array of serialized message, ``str``
01027 """
01028 try:
01029 if self.action_goal is None:
01030 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal()
01031 if self.action_result is None:
01032 self.action_result = object_manipulation_msgs.msg.PlaceActionResult()
01033 if self.action_feedback is None:
01034 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback()
01035 end = 0
01036 _x = self
01037 start = end
01038 end += 12
01039 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01040 start = end
01041 end += 4
01042 (length,) = _struct_I.unpack(str[start:end])
01043 start = end
01044 end += length
01045 if python3:
01046 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
01047 else:
01048 self.action_goal.header.frame_id = str[start:end]
01049 _x = self
01050 start = end
01051 end += 8
01052 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01053 start = end
01054 end += 4
01055 (length,) = _struct_I.unpack(str[start:end])
01056 start = end
01057 end += length
01058 if python3:
01059 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
01060 else:
01061 self.action_goal.goal_id.id = str[start:end]
01062 start = end
01063 end += 4
01064 (length,) = _struct_I.unpack(str[start:end])
01065 start = end
01066 end += length
01067 if python3:
01068 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
01069 else:
01070 self.action_goal.goal.arm_name = str[start:end]
01071 start = end
01072 end += 4
01073 (length,) = _struct_I.unpack(str[start:end])
01074 self.action_goal.goal.place_locations = []
01075 for i in range(0, length):
01076 val1 = geometry_msgs.msg.PoseStamped()
01077 _v32 = val1.header
01078 start = end
01079 end += 4
01080 (_v32.seq,) = _struct_I.unpack(str[start:end])
01081 _v33 = _v32.stamp
01082 _x = _v33
01083 start = end
01084 end += 8
01085 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01086 start = end
01087 end += 4
01088 (length,) = _struct_I.unpack(str[start:end])
01089 start = end
01090 end += length
01091 if python3:
01092 _v32.frame_id = str[start:end].decode('utf-8')
01093 else:
01094 _v32.frame_id = str[start:end]
01095 _v34 = val1.pose
01096 _v35 = _v34.position
01097 _x = _v35
01098 start = end
01099 end += 24
01100 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01101 _v36 = _v34.orientation
01102 _x = _v36
01103 start = end
01104 end += 32
01105 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01106 self.action_goal.goal.place_locations.append(val1)
01107 start = end
01108 end += 4
01109 (length,) = _struct_I.unpack(str[start:end])
01110 start = end
01111 end += length
01112 if python3:
01113 self.action_goal.goal.grasp.id = str[start:end].decode('utf-8')
01114 else:
01115 self.action_goal.goal.grasp.id = str[start:end]
01116 _x = self
01117 start = end
01118 end += 12
01119 (_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])
01120 start = end
01121 end += 4
01122 (length,) = _struct_I.unpack(str[start:end])
01123 start = end
01124 end += length
01125 if python3:
01126 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01127 else:
01128 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01129 start = end
01130 end += 4
01131 (length,) = _struct_I.unpack(str[start:end])
01132 self.action_goal.goal.grasp.pre_grasp_posture.name = []
01133 for i in range(0, length):
01134 start = end
01135 end += 4
01136 (length,) = _struct_I.unpack(str[start:end])
01137 start = end
01138 end += length
01139 if python3:
01140 val1 = str[start:end].decode('utf-8')
01141 else:
01142 val1 = str[start:end]
01143 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1)
01144 start = end
01145 end += 4
01146 (length,) = _struct_I.unpack(str[start:end])
01147 pattern = '<%sd'%length
01148 start = end
01149 end += struct.calcsize(pattern)
01150 self.action_goal.goal.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 pattern = '<%sd'%length
01155 start = end
01156 end += struct.calcsize(pattern)
01157 self.action_goal.goal.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01158 start = end
01159 end += 4
01160 (length,) = _struct_I.unpack(str[start:end])
01161 pattern = '<%sd'%length
01162 start = end
01163 end += struct.calcsize(pattern)
01164 self.action_goal.goal.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01165 _x = self
01166 start = end
01167 end += 12
01168 (_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])
01169 start = end
01170 end += 4
01171 (length,) = _struct_I.unpack(str[start:end])
01172 start = end
01173 end += length
01174 if python3:
01175 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01176 else:
01177 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end]
01178 start = end
01179 end += 4
01180 (length,) = _struct_I.unpack(str[start:end])
01181 self.action_goal.goal.grasp.grasp_posture.name = []
01182 for i in range(0, length):
01183 start = end
01184 end += 4
01185 (length,) = _struct_I.unpack(str[start:end])
01186 start = end
01187 end += length
01188 if python3:
01189 val1 = str[start:end].decode('utf-8')
01190 else:
01191 val1 = str[start:end]
01192 self.action_goal.goal.grasp.grasp_posture.name.append(val1)
01193 start = end
01194 end += 4
01195 (length,) = _struct_I.unpack(str[start:end])
01196 pattern = '<%sd'%length
01197 start = end
01198 end += struct.calcsize(pattern)
01199 self.action_goal.goal.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
01200 start = end
01201 end += 4
01202 (length,) = _struct_I.unpack(str[start:end])
01203 pattern = '<%sd'%length
01204 start = end
01205 end += struct.calcsize(pattern)
01206 self.action_goal.goal.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01207 start = end
01208 end += 4
01209 (length,) = _struct_I.unpack(str[start:end])
01210 pattern = '<%sd'%length
01211 start = end
01212 end += struct.calcsize(pattern)
01213 self.action_goal.goal.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01214 _x = self
01215 start = end
01216 end += 12
01217 (_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.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 if python3:
01224 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
01225 else:
01226 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end]
01227 _x = self
01228 start = end
01229 end += 76
01230 (_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
01231 start = end
01232 end += 4
01233 (length,) = _struct_I.unpack(str[start:end])
01234 start = end
01235 end += length
01236 if python3:
01237 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01238 else:
01239 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end]
01240 _x = self
01241 start = end
01242 end += 44
01243 (_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
01244 start = end
01245 end += 4
01246 (length,) = _struct_I.unpack(str[start:end])
01247 start = end
01248 end += length
01249 if python3:
01250 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
01251 else:
01252 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end]
01253 _x = self
01254 start = end
01255 end += 36
01256 (_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
01257 start = end
01258 end += 4
01259 (length,) = _struct_I.unpack(str[start:end])
01260 self.action_goal.goal.grasp.allowed_touch_objects = []
01261 for i in range(0, length):
01262 start = end
01263 end += 4
01264 (length,) = _struct_I.unpack(str[start:end])
01265 start = end
01266 end += length
01267 if python3:
01268 val1 = str[start:end].decode('utf-8')
01269 else:
01270 val1 = str[start:end]
01271 self.action_goal.goal.grasp.allowed_touch_objects.append(val1)
01272 _x = self
01273 start = end
01274 end += 20
01275 (_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_2f3I.unpack(str[start:end])
01276 start = end
01277 end += 4
01278 (length,) = _struct_I.unpack(str[start:end])
01279 start = end
01280 end += length
01281 if python3:
01282 self.action_goal.goal.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01283 else:
01284 self.action_goal.goal.approach.direction.header.frame_id = str[start:end]
01285 _x = self
01286 start = end
01287 end += 32
01288 (_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])
01289 start = end
01290 end += 4
01291 (length,) = _struct_I.unpack(str[start:end])
01292 start = end
01293 end += length
01294 if python3:
01295 self.action_goal.goal.collision_object_name = str[start:end].decode('utf-8')
01296 else:
01297 self.action_goal.goal.collision_object_name = str[start:end]
01298 start = end
01299 end += 4
01300 (length,) = _struct_I.unpack(str[start:end])
01301 start = end
01302 end += length
01303 if python3:
01304 self.action_goal.goal.collision_support_surface_name = str[start:end].decode('utf-8')
01305 else:
01306 self.action_goal.goal.collision_support_surface_name = str[start:end]
01307 _x = self
01308 start = end
01309 end += 11
01310 (_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end])
01311 self.action_goal.goal.allow_gripper_support_collision = bool(self.action_goal.goal.allow_gripper_support_collision)
01312 self.action_goal.goal.use_reactive_place = bool(self.action_goal.goal.use_reactive_place)
01313 self.action_goal.goal.only_perform_feasibility_test = bool(self.action_goal.goal.only_perform_feasibility_test)
01314 start = end
01315 end += 4
01316 (length,) = _struct_I.unpack(str[start:end])
01317 self.action_goal.goal.path_constraints.joint_constraints = []
01318 for i in range(0, length):
01319 val1 = arm_navigation_msgs.msg.JointConstraint()
01320 start = end
01321 end += 4
01322 (length,) = _struct_I.unpack(str[start:end])
01323 start = end
01324 end += length
01325 if python3:
01326 val1.joint_name = str[start:end].decode('utf-8')
01327 else:
01328 val1.joint_name = str[start:end]
01329 _x = val1
01330 start = end
01331 end += 32
01332 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01333 self.action_goal.goal.path_constraints.joint_constraints.append(val1)
01334 start = end
01335 end += 4
01336 (length,) = _struct_I.unpack(str[start:end])
01337 self.action_goal.goal.path_constraints.position_constraints = []
01338 for i in range(0, length):
01339 val1 = arm_navigation_msgs.msg.PositionConstraint()
01340 _v37 = val1.header
01341 start = end
01342 end += 4
01343 (_v37.seq,) = _struct_I.unpack(str[start:end])
01344 _v38 = _v37.stamp
01345 _x = _v38
01346 start = end
01347 end += 8
01348 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01349 start = end
01350 end += 4
01351 (length,) = _struct_I.unpack(str[start:end])
01352 start = end
01353 end += length
01354 if python3:
01355 _v37.frame_id = str[start:end].decode('utf-8')
01356 else:
01357 _v37.frame_id = str[start:end]
01358 start = end
01359 end += 4
01360 (length,) = _struct_I.unpack(str[start:end])
01361 start = end
01362 end += length
01363 if python3:
01364 val1.link_name = str[start:end].decode('utf-8')
01365 else:
01366 val1.link_name = str[start:end]
01367 _v39 = val1.target_point_offset
01368 _x = _v39
01369 start = end
01370 end += 24
01371 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01372 _v40 = val1.position
01373 _x = _v40
01374 start = end
01375 end += 24
01376 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01377 _v41 = val1.constraint_region_shape
01378 start = end
01379 end += 1
01380 (_v41.type,) = _struct_b.unpack(str[start:end])
01381 start = end
01382 end += 4
01383 (length,) = _struct_I.unpack(str[start:end])
01384 pattern = '<%sd'%length
01385 start = end
01386 end += struct.calcsize(pattern)
01387 _v41.dimensions = struct.unpack(pattern, str[start:end])
01388 start = end
01389 end += 4
01390 (length,) = _struct_I.unpack(str[start:end])
01391 pattern = '<%si'%length
01392 start = end
01393 end += struct.calcsize(pattern)
01394 _v41.triangles = struct.unpack(pattern, str[start:end])
01395 start = end
01396 end += 4
01397 (length,) = _struct_I.unpack(str[start:end])
01398 _v41.vertices = []
01399 for i in range(0, length):
01400 val3 = geometry_msgs.msg.Point()
01401 _x = val3
01402 start = end
01403 end += 24
01404 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01405 _v41.vertices.append(val3)
01406 _v42 = val1.constraint_region_orientation
01407 _x = _v42
01408 start = end
01409 end += 32
01410 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01411 start = end
01412 end += 8
01413 (val1.weight,) = _struct_d.unpack(str[start:end])
01414 self.action_goal.goal.path_constraints.position_constraints.append(val1)
01415 start = end
01416 end += 4
01417 (length,) = _struct_I.unpack(str[start:end])
01418 self.action_goal.goal.path_constraints.orientation_constraints = []
01419 for i in range(0, length):
01420 val1 = arm_navigation_msgs.msg.OrientationConstraint()
01421 _v43 = val1.header
01422 start = end
01423 end += 4
01424 (_v43.seq,) = _struct_I.unpack(str[start:end])
01425 _v44 = _v43.stamp
01426 _x = _v44
01427 start = end
01428 end += 8
01429 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01430 start = end
01431 end += 4
01432 (length,) = _struct_I.unpack(str[start:end])
01433 start = end
01434 end += length
01435 if python3:
01436 _v43.frame_id = str[start:end].decode('utf-8')
01437 else:
01438 _v43.frame_id = str[start:end]
01439 start = end
01440 end += 4
01441 (length,) = _struct_I.unpack(str[start:end])
01442 start = end
01443 end += length
01444 if python3:
01445 val1.link_name = str[start:end].decode('utf-8')
01446 else:
01447 val1.link_name = str[start:end]
01448 start = end
01449 end += 4
01450 (val1.type,) = _struct_i.unpack(str[start:end])
01451 _v45 = val1.orientation
01452 _x = _v45
01453 start = end
01454 end += 32
01455 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01456 _x = val1
01457 start = end
01458 end += 32
01459 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
01460 self.action_goal.goal.path_constraints.orientation_constraints.append(val1)
01461 start = end
01462 end += 4
01463 (length,) = _struct_I.unpack(str[start:end])
01464 self.action_goal.goal.path_constraints.visibility_constraints = []
01465 for i in range(0, length):
01466 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
01467 _v46 = val1.header
01468 start = end
01469 end += 4
01470 (_v46.seq,) = _struct_I.unpack(str[start:end])
01471 _v47 = _v46.stamp
01472 _x = _v47
01473 start = end
01474 end += 8
01475 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01476 start = end
01477 end += 4
01478 (length,) = _struct_I.unpack(str[start:end])
01479 start = end
01480 end += length
01481 if python3:
01482 _v46.frame_id = str[start:end].decode('utf-8')
01483 else:
01484 _v46.frame_id = str[start:end]
01485 _v48 = val1.target
01486 _v49 = _v48.header
01487 start = end
01488 end += 4
01489 (_v49.seq,) = _struct_I.unpack(str[start:end])
01490 _v50 = _v49.stamp
01491 _x = _v50
01492 start = end
01493 end += 8
01494 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01495 start = end
01496 end += 4
01497 (length,) = _struct_I.unpack(str[start:end])
01498 start = end
01499 end += length
01500 if python3:
01501 _v49.frame_id = str[start:end].decode('utf-8')
01502 else:
01503 _v49.frame_id = str[start:end]
01504 _v51 = _v48.point
01505 _x = _v51
01506 start = end
01507 end += 24
01508 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01509 _v52 = val1.sensor_pose
01510 _v53 = _v52.header
01511 start = end
01512 end += 4
01513 (_v53.seq,) = _struct_I.unpack(str[start:end])
01514 _v54 = _v53.stamp
01515 _x = _v54
01516 start = end
01517 end += 8
01518 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01519 start = end
01520 end += 4
01521 (length,) = _struct_I.unpack(str[start:end])
01522 start = end
01523 end += length
01524 if python3:
01525 _v53.frame_id = str[start:end].decode('utf-8')
01526 else:
01527 _v53.frame_id = str[start:end]
01528 _v55 = _v52.pose
01529 _v56 = _v55.position
01530 _x = _v56
01531 start = end
01532 end += 24
01533 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01534 _v57 = _v55.orientation
01535 _x = _v57
01536 start = end
01537 end += 32
01538 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01539 start = end
01540 end += 8
01541 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
01542 self.action_goal.goal.path_constraints.visibility_constraints.append(val1)
01543 start = end
01544 end += 4
01545 (length,) = _struct_I.unpack(str[start:end])
01546 self.action_goal.goal.additional_collision_operations.collision_operations = []
01547 for i in range(0, length):
01548 val1 = arm_navigation_msgs.msg.CollisionOperation()
01549 start = end
01550 end += 4
01551 (length,) = _struct_I.unpack(str[start:end])
01552 start = end
01553 end += length
01554 if python3:
01555 val1.object1 = str[start:end].decode('utf-8')
01556 else:
01557 val1.object1 = str[start:end]
01558 start = end
01559 end += 4
01560 (length,) = _struct_I.unpack(str[start:end])
01561 start = end
01562 end += length
01563 if python3:
01564 val1.object2 = str[start:end].decode('utf-8')
01565 else:
01566 val1.object2 = str[start:end]
01567 _x = val1
01568 start = end
01569 end += 12
01570 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
01571 self.action_goal.goal.additional_collision_operations.collision_operations.append(val1)
01572 start = end
01573 end += 4
01574 (length,) = _struct_I.unpack(str[start:end])
01575 self.action_goal.goal.additional_link_padding = []
01576 for i in range(0, length):
01577 val1 = arm_navigation_msgs.msg.LinkPadding()
01578 start = end
01579 end += 4
01580 (length,) = _struct_I.unpack(str[start:end])
01581 start = end
01582 end += length
01583 if python3:
01584 val1.link_name = str[start:end].decode('utf-8')
01585 else:
01586 val1.link_name = str[start:end]
01587 start = end
01588 end += 8
01589 (val1.padding,) = _struct_d.unpack(str[start:end])
01590 self.action_goal.goal.additional_link_padding.append(val1)
01591 _x = self
01592 start = end
01593 end += 12
01594 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01595 start = end
01596 end += 4
01597 (length,) = _struct_I.unpack(str[start:end])
01598 start = end
01599 end += length
01600 if python3:
01601 self.action_result.header.frame_id = str[start:end].decode('utf-8')
01602 else:
01603 self.action_result.header.frame_id = str[start:end]
01604 _x = self
01605 start = end
01606 end += 8
01607 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01608 start = end
01609 end += 4
01610 (length,) = _struct_I.unpack(str[start:end])
01611 start = end
01612 end += length
01613 if python3:
01614 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
01615 else:
01616 self.action_result.status.goal_id.id = str[start:end]
01617 start = end
01618 end += 1
01619 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01620 start = end
01621 end += 4
01622 (length,) = _struct_I.unpack(str[start:end])
01623 start = end
01624 end += length
01625 if python3:
01626 self.action_result.status.text = str[start:end].decode('utf-8')
01627 else:
01628 self.action_result.status.text = str[start:end]
01629 _x = self
01630 start = end
01631 end += 16
01632 (_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])
01633 start = end
01634 end += 4
01635 (length,) = _struct_I.unpack(str[start:end])
01636 start = end
01637 end += length
01638 if python3:
01639 self.action_result.result.place_location.header.frame_id = str[start:end].decode('utf-8')
01640 else:
01641 self.action_result.result.place_location.header.frame_id = str[start:end]
01642 _x = self
01643 start = end
01644 end += 56
01645 (_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])
01646 start = end
01647 end += 4
01648 (length,) = _struct_I.unpack(str[start:end])
01649 self.action_result.result.attempted_locations = []
01650 for i in range(0, length):
01651 val1 = geometry_msgs.msg.PoseStamped()
01652 _v58 = val1.header
01653 start = end
01654 end += 4
01655 (_v58.seq,) = _struct_I.unpack(str[start:end])
01656 _v59 = _v58.stamp
01657 _x = _v59
01658 start = end
01659 end += 8
01660 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01661 start = end
01662 end += 4
01663 (length,) = _struct_I.unpack(str[start:end])
01664 start = end
01665 end += length
01666 if python3:
01667 _v58.frame_id = str[start:end].decode('utf-8')
01668 else:
01669 _v58.frame_id = str[start:end]
01670 _v60 = val1.pose
01671 _v61 = _v60.position
01672 _x = _v61
01673 start = end
01674 end += 24
01675 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01676 _v62 = _v60.orientation
01677 _x = _v62
01678 start = end
01679 end += 32
01680 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01681 self.action_result.result.attempted_locations.append(val1)
01682 start = end
01683 end += 4
01684 (length,) = _struct_I.unpack(str[start:end])
01685 self.action_result.result.attempted_location_results = []
01686 for i in range(0, length):
01687 val1 = object_manipulation_msgs.msg.PlaceLocationResult()
01688 _x = val1
01689 start = end
01690 end += 5
01691 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
01692 val1.continuation_possible = bool(val1.continuation_possible)
01693 self.action_result.result.attempted_location_results.append(val1)
01694 _x = self
01695 start = end
01696 end += 12
01697 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01698 start = end
01699 end += 4
01700 (length,) = _struct_I.unpack(str[start:end])
01701 start = end
01702 end += length
01703 if python3:
01704 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
01705 else:
01706 self.action_feedback.header.frame_id = str[start:end]
01707 _x = self
01708 start = end
01709 end += 8
01710 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.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 if python3:
01717 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
01718 else:
01719 self.action_feedback.status.goal_id.id = str[start:end]
01720 start = end
01721 end += 1
01722 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01723 start = end
01724 end += 4
01725 (length,) = _struct_I.unpack(str[start:end])
01726 start = end
01727 end += length
01728 if python3:
01729 self.action_feedback.status.text = str[start:end].decode('utf-8')
01730 else:
01731 self.action_feedback.status.text = str[start:end]
01732 _x = self
01733 start = end
01734 end += 8
01735 (_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations,) = _struct_2i.unpack(str[start:end])
01736 return self
01737 except struct.error as e:
01738 raise genpy.DeserializationError(e)
01739
01740
01741 def serialize_numpy(self, buff, numpy):
01742 """
01743 serialize message with numpy array types into buffer
01744 :param buff: buffer, ``StringIO``
01745 :param numpy: numpy python module
01746 """
01747 try:
01748 _x = self
01749 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
01750 _x = self.action_goal.header.frame_id
01751 length = len(_x)
01752 if python3 or type(_x) == unicode:
01753 _x = _x.encode('utf-8')
01754 length = len(_x)
01755 buff.write(struct.pack('<I%ss'%length, length, _x))
01756 _x = self
01757 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
01758 _x = self.action_goal.goal_id.id
01759 length = len(_x)
01760 if python3 or type(_x) == unicode:
01761 _x = _x.encode('utf-8')
01762 length = len(_x)
01763 buff.write(struct.pack('<I%ss'%length, length, _x))
01764 _x = self.action_goal.goal.arm_name
01765 length = len(_x)
01766 if python3 or type(_x) == unicode:
01767 _x = _x.encode('utf-8')
01768 length = len(_x)
01769 buff.write(struct.pack('<I%ss'%length, length, _x))
01770 length = len(self.action_goal.goal.place_locations)
01771 buff.write(_struct_I.pack(length))
01772 for val1 in self.action_goal.goal.place_locations:
01773 _v63 = val1.header
01774 buff.write(_struct_I.pack(_v63.seq))
01775 _v64 = _v63.stamp
01776 _x = _v64
01777 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01778 _x = _v63.frame_id
01779 length = len(_x)
01780 if python3 or type(_x) == unicode:
01781 _x = _x.encode('utf-8')
01782 length = len(_x)
01783 buff.write(struct.pack('<I%ss'%length, length, _x))
01784 _v65 = val1.pose
01785 _v66 = _v65.position
01786 _x = _v66
01787 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01788 _v67 = _v65.orientation
01789 _x = _v67
01790 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01791 _x = self.action_goal.goal.grasp.id
01792 length = len(_x)
01793 if python3 or type(_x) == unicode:
01794 _x = _x.encode('utf-8')
01795 length = len(_x)
01796 buff.write(struct.pack('<I%ss'%length, length, _x))
01797 _x = self
01798 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))
01799 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id
01800 length = len(_x)
01801 if python3 or type(_x) == unicode:
01802 _x = _x.encode('utf-8')
01803 length = len(_x)
01804 buff.write(struct.pack('<I%ss'%length, length, _x))
01805 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name)
01806 buff.write(_struct_I.pack(length))
01807 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name:
01808 length = len(val1)
01809 if python3 or type(val1) == unicode:
01810 val1 = val1.encode('utf-8')
01811 length = len(val1)
01812 buff.write(struct.pack('<I%ss'%length, length, val1))
01813 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position)
01814 buff.write(_struct_I.pack(length))
01815 pattern = '<%sd'%length
01816 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.position.tostring())
01817 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity)
01818 buff.write(_struct_I.pack(length))
01819 pattern = '<%sd'%length
01820 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.velocity.tostring())
01821 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort)
01822 buff.write(_struct_I.pack(length))
01823 pattern = '<%sd'%length
01824 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.effort.tostring())
01825 _x = self
01826 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))
01827 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id
01828 length = len(_x)
01829 if python3 or type(_x) == unicode:
01830 _x = _x.encode('utf-8')
01831 length = len(_x)
01832 buff.write(struct.pack('<I%ss'%length, length, _x))
01833 length = len(self.action_goal.goal.grasp.grasp_posture.name)
01834 buff.write(_struct_I.pack(length))
01835 for val1 in self.action_goal.goal.grasp.grasp_posture.name:
01836 length = len(val1)
01837 if python3 or type(val1) == unicode:
01838 val1 = val1.encode('utf-8')
01839 length = len(val1)
01840 buff.write(struct.pack('<I%ss'%length, length, val1))
01841 length = len(self.action_goal.goal.grasp.grasp_posture.position)
01842 buff.write(_struct_I.pack(length))
01843 pattern = '<%sd'%length
01844 buff.write(self.action_goal.goal.grasp.grasp_posture.position.tostring())
01845 length = len(self.action_goal.goal.grasp.grasp_posture.velocity)
01846 buff.write(_struct_I.pack(length))
01847 pattern = '<%sd'%length
01848 buff.write(self.action_goal.goal.grasp.grasp_posture.velocity.tostring())
01849 length = len(self.action_goal.goal.grasp.grasp_posture.effort)
01850 buff.write(_struct_I.pack(length))
01851 pattern = '<%sd'%length
01852 buff.write(self.action_goal.goal.grasp.grasp_posture.effort.tostring())
01853 _x = self
01854 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs))
01855 _x = self.action_goal.goal.grasp.grasp_pose.header.frame_id
01856 length = len(_x)
01857 if python3 or type(_x) == unicode:
01858 _x = _x.encode('utf-8')
01859 length = len(_x)
01860 buff.write(struct.pack('<I%ss'%length, length, _x))
01861 _x = self
01862 buff.write(_struct_8d3I.pack(_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs))
01863 _x = self.action_goal.goal.grasp.approach.direction.header.frame_id
01864 length = len(_x)
01865 if python3 or type(_x) == unicode:
01866 _x = _x.encode('utf-8')
01867 length = len(_x)
01868 buff.write(struct.pack('<I%ss'%length, length, _x))
01869 _x = self
01870 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs))
01871 _x = self.action_goal.goal.grasp.retreat.direction.header.frame_id
01872 length = len(_x)
01873 if python3 or type(_x) == unicode:
01874 _x = _x.encode('utf-8')
01875 length = len(_x)
01876 buff.write(struct.pack('<I%ss'%length, length, _x))
01877 _x = self
01878 buff.write(_struct_3d3f.pack(_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force))
01879 length = len(self.action_goal.goal.grasp.allowed_touch_objects)
01880 buff.write(_struct_I.pack(length))
01881 for val1 in self.action_goal.goal.grasp.allowed_touch_objects:
01882 length = len(val1)
01883 if python3 or type(val1) == unicode:
01884 val1 = val1.encode('utf-8')
01885 length = len(val1)
01886 buff.write(struct.pack('<I%ss'%length, length, val1))
01887 _x = self
01888 buff.write(_struct_2f3I.pack(_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))
01889 _x = self.action_goal.goal.approach.direction.header.frame_id
01890 length = len(_x)
01891 if python3 or type(_x) == unicode:
01892 _x = _x.encode('utf-8')
01893 length = len(_x)
01894 buff.write(struct.pack('<I%ss'%length, length, _x))
01895 _x = self
01896 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))
01897 _x = self.action_goal.goal.collision_object_name
01898 length = len(_x)
01899 if python3 or type(_x) == unicode:
01900 _x = _x.encode('utf-8')
01901 length = len(_x)
01902 buff.write(struct.pack('<I%ss'%length, length, _x))
01903 _x = self.action_goal.goal.collision_support_surface_name
01904 length = len(_x)
01905 if python3 or type(_x) == unicode:
01906 _x = _x.encode('utf-8')
01907 length = len(_x)
01908 buff.write(struct.pack('<I%ss'%length, length, _x))
01909 _x = self
01910 buff.write(_struct_2BdB.pack(_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test))
01911 length = len(self.action_goal.goal.path_constraints.joint_constraints)
01912 buff.write(_struct_I.pack(length))
01913 for val1 in self.action_goal.goal.path_constraints.joint_constraints:
01914 _x = val1.joint_name
01915 length = len(_x)
01916 if python3 or type(_x) == unicode:
01917 _x = _x.encode('utf-8')
01918 length = len(_x)
01919 buff.write(struct.pack('<I%ss'%length, length, _x))
01920 _x = val1
01921 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01922 length = len(self.action_goal.goal.path_constraints.position_constraints)
01923 buff.write(_struct_I.pack(length))
01924 for val1 in self.action_goal.goal.path_constraints.position_constraints:
01925 _v68 = val1.header
01926 buff.write(_struct_I.pack(_v68.seq))
01927 _v69 = _v68.stamp
01928 _x = _v69
01929 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01930 _x = _v68.frame_id
01931 length = len(_x)
01932 if python3 or type(_x) == unicode:
01933 _x = _x.encode('utf-8')
01934 length = len(_x)
01935 buff.write(struct.pack('<I%ss'%length, length, _x))
01936 _x = val1.link_name
01937 length = len(_x)
01938 if python3 or type(_x) == unicode:
01939 _x = _x.encode('utf-8')
01940 length = len(_x)
01941 buff.write(struct.pack('<I%ss'%length, length, _x))
01942 _v70 = val1.target_point_offset
01943 _x = _v70
01944 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01945 _v71 = val1.position
01946 _x = _v71
01947 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01948 _v72 = val1.constraint_region_shape
01949 buff.write(_struct_b.pack(_v72.type))
01950 length = len(_v72.dimensions)
01951 buff.write(_struct_I.pack(length))
01952 pattern = '<%sd'%length
01953 buff.write(_v72.dimensions.tostring())
01954 length = len(_v72.triangles)
01955 buff.write(_struct_I.pack(length))
01956 pattern = '<%si'%length
01957 buff.write(_v72.triangles.tostring())
01958 length = len(_v72.vertices)
01959 buff.write(_struct_I.pack(length))
01960 for val3 in _v72.vertices:
01961 _x = val3
01962 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01963 _v73 = val1.constraint_region_orientation
01964 _x = _v73
01965 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01966 buff.write(_struct_d.pack(val1.weight))
01967 length = len(self.action_goal.goal.path_constraints.orientation_constraints)
01968 buff.write(_struct_I.pack(length))
01969 for val1 in self.action_goal.goal.path_constraints.orientation_constraints:
01970 _v74 = val1.header
01971 buff.write(_struct_I.pack(_v74.seq))
01972 _v75 = _v74.stamp
01973 _x = _v75
01974 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01975 _x = _v74.frame_id
01976 length = len(_x)
01977 if python3 or type(_x) == unicode:
01978 _x = _x.encode('utf-8')
01979 length = len(_x)
01980 buff.write(struct.pack('<I%ss'%length, length, _x))
01981 _x = val1.link_name
01982 length = len(_x)
01983 if python3 or type(_x) == unicode:
01984 _x = _x.encode('utf-8')
01985 length = len(_x)
01986 buff.write(struct.pack('<I%ss'%length, length, _x))
01987 buff.write(_struct_i.pack(val1.type))
01988 _v76 = val1.orientation
01989 _x = _v76
01990 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01991 _x = val1
01992 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01993 length = len(self.action_goal.goal.path_constraints.visibility_constraints)
01994 buff.write(_struct_I.pack(length))
01995 for val1 in self.action_goal.goal.path_constraints.visibility_constraints:
01996 _v77 = val1.header
01997 buff.write(_struct_I.pack(_v77.seq))
01998 _v78 = _v77.stamp
01999 _x = _v78
02000 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02001 _x = _v77.frame_id
02002 length = len(_x)
02003 if python3 or type(_x) == unicode:
02004 _x = _x.encode('utf-8')
02005 length = len(_x)
02006 buff.write(struct.pack('<I%ss'%length, length, _x))
02007 _v79 = val1.target
02008 _v80 = _v79.header
02009 buff.write(_struct_I.pack(_v80.seq))
02010 _v81 = _v80.stamp
02011 _x = _v81
02012 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02013 _x = _v80.frame_id
02014 length = len(_x)
02015 if python3 or type(_x) == unicode:
02016 _x = _x.encode('utf-8')
02017 length = len(_x)
02018 buff.write(struct.pack('<I%ss'%length, length, _x))
02019 _v82 = _v79.point
02020 _x = _v82
02021 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02022 _v83 = val1.sensor_pose
02023 _v84 = _v83.header
02024 buff.write(_struct_I.pack(_v84.seq))
02025 _v85 = _v84.stamp
02026 _x = _v85
02027 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02028 _x = _v84.frame_id
02029 length = len(_x)
02030 if python3 or type(_x) == unicode:
02031 _x = _x.encode('utf-8')
02032 length = len(_x)
02033 buff.write(struct.pack('<I%ss'%length, length, _x))
02034 _v86 = _v83.pose
02035 _v87 = _v86.position
02036 _x = _v87
02037 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02038 _v88 = _v86.orientation
02039 _x = _v88
02040 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02041 buff.write(_struct_d.pack(val1.absolute_tolerance))
02042 length = len(self.action_goal.goal.additional_collision_operations.collision_operations)
02043 buff.write(_struct_I.pack(length))
02044 for val1 in self.action_goal.goal.additional_collision_operations.collision_operations:
02045 _x = val1.object1
02046 length = len(_x)
02047 if python3 or type(_x) == unicode:
02048 _x = _x.encode('utf-8')
02049 length = len(_x)
02050 buff.write(struct.pack('<I%ss'%length, length, _x))
02051 _x = val1.object2
02052 length = len(_x)
02053 if python3 or type(_x) == unicode:
02054 _x = _x.encode('utf-8')
02055 length = len(_x)
02056 buff.write(struct.pack('<I%ss'%length, length, _x))
02057 _x = val1
02058 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
02059 length = len(self.action_goal.goal.additional_link_padding)
02060 buff.write(_struct_I.pack(length))
02061 for val1 in self.action_goal.goal.additional_link_padding:
02062 _x = val1.link_name
02063 length = len(_x)
02064 if python3 or type(_x) == unicode:
02065 _x = _x.encode('utf-8')
02066 length = len(_x)
02067 buff.write(struct.pack('<I%ss'%length, length, _x))
02068 buff.write(_struct_d.pack(val1.padding))
02069 _x = self
02070 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
02071 _x = self.action_result.header.frame_id
02072 length = len(_x)
02073 if python3 or type(_x) == unicode:
02074 _x = _x.encode('utf-8')
02075 length = len(_x)
02076 buff.write(struct.pack('<I%ss'%length, length, _x))
02077 _x = self
02078 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
02079 _x = self.action_result.status.goal_id.id
02080 length = len(_x)
02081 if python3 or type(_x) == unicode:
02082 _x = _x.encode('utf-8')
02083 length = len(_x)
02084 buff.write(struct.pack('<I%ss'%length, length, _x))
02085 buff.write(_struct_B.pack(self.action_result.status.status))
02086 _x = self.action_result.status.text
02087 length = len(_x)
02088 if python3 or type(_x) == unicode:
02089 _x = _x.encode('utf-8')
02090 length = len(_x)
02091 buff.write(struct.pack('<I%ss'%length, length, _x))
02092 _x = self
02093 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))
02094 _x = self.action_result.result.place_location.header.frame_id
02095 length = len(_x)
02096 if python3 or type(_x) == unicode:
02097 _x = _x.encode('utf-8')
02098 length = len(_x)
02099 buff.write(struct.pack('<I%ss'%length, length, _x))
02100 _x = self
02101 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))
02102 length = len(self.action_result.result.attempted_locations)
02103 buff.write(_struct_I.pack(length))
02104 for val1 in self.action_result.result.attempted_locations:
02105 _v89 = val1.header
02106 buff.write(_struct_I.pack(_v89.seq))
02107 _v90 = _v89.stamp
02108 _x = _v90
02109 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02110 _x = _v89.frame_id
02111 length = len(_x)
02112 if python3 or type(_x) == unicode:
02113 _x = _x.encode('utf-8')
02114 length = len(_x)
02115 buff.write(struct.pack('<I%ss'%length, length, _x))
02116 _v91 = val1.pose
02117 _v92 = _v91.position
02118 _x = _v92
02119 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02120 _v93 = _v91.orientation
02121 _x = _v93
02122 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02123 length = len(self.action_result.result.attempted_location_results)
02124 buff.write(_struct_I.pack(length))
02125 for val1 in self.action_result.result.attempted_location_results:
02126 _x = val1
02127 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
02128 _x = self
02129 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
02130 _x = self.action_feedback.header.frame_id
02131 length = len(_x)
02132 if python3 or type(_x) == unicode:
02133 _x = _x.encode('utf-8')
02134 length = len(_x)
02135 buff.write(struct.pack('<I%ss'%length, length, _x))
02136 _x = self
02137 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
02138 _x = self.action_feedback.status.goal_id.id
02139 length = len(_x)
02140 if python3 or type(_x) == unicode:
02141 _x = _x.encode('utf-8')
02142 length = len(_x)
02143 buff.write(struct.pack('<I%ss'%length, length, _x))
02144 buff.write(_struct_B.pack(self.action_feedback.status.status))
02145 _x = self.action_feedback.status.text
02146 length = len(_x)
02147 if python3 or type(_x) == unicode:
02148 _x = _x.encode('utf-8')
02149 length = len(_x)
02150 buff.write(struct.pack('<I%ss'%length, length, _x))
02151 _x = self
02152 buff.write(_struct_2i.pack(_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations))
02153 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
02154 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
02155
02156 def deserialize_numpy(self, str, numpy):
02157 """
02158 unpack serialized message in str into this message instance using numpy for array types
02159 :param str: byte array of serialized message, ``str``
02160 :param numpy: numpy python module
02161 """
02162 try:
02163 if self.action_goal is None:
02164 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal()
02165 if self.action_result is None:
02166 self.action_result = object_manipulation_msgs.msg.PlaceActionResult()
02167 if self.action_feedback is None:
02168 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback()
02169 end = 0
02170 _x = self
02171 start = end
02172 end += 12
02173 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02174 start = end
02175 end += 4
02176 (length,) = _struct_I.unpack(str[start:end])
02177 start = end
02178 end += length
02179 if python3:
02180 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
02181 else:
02182 self.action_goal.header.frame_id = str[start:end]
02183 _x = self
02184 start = end
02185 end += 8
02186 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.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 if python3:
02193 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
02194 else:
02195 self.action_goal.goal_id.id = str[start:end]
02196 start = end
02197 end += 4
02198 (length,) = _struct_I.unpack(str[start:end])
02199 start = end
02200 end += length
02201 if python3:
02202 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
02203 else:
02204 self.action_goal.goal.arm_name = str[start:end]
02205 start = end
02206 end += 4
02207 (length,) = _struct_I.unpack(str[start:end])
02208 self.action_goal.goal.place_locations = []
02209 for i in range(0, length):
02210 val1 = geometry_msgs.msg.PoseStamped()
02211 _v94 = val1.header
02212 start = end
02213 end += 4
02214 (_v94.seq,) = _struct_I.unpack(str[start:end])
02215 _v95 = _v94.stamp
02216 _x = _v95
02217 start = end
02218 end += 8
02219 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02220 start = end
02221 end += 4
02222 (length,) = _struct_I.unpack(str[start:end])
02223 start = end
02224 end += length
02225 if python3:
02226 _v94.frame_id = str[start:end].decode('utf-8')
02227 else:
02228 _v94.frame_id = str[start:end]
02229 _v96 = val1.pose
02230 _v97 = _v96.position
02231 _x = _v97
02232 start = end
02233 end += 24
02234 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02235 _v98 = _v96.orientation
02236 _x = _v98
02237 start = end
02238 end += 32
02239 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02240 self.action_goal.goal.place_locations.append(val1)
02241 start = end
02242 end += 4
02243 (length,) = _struct_I.unpack(str[start:end])
02244 start = end
02245 end += length
02246 if python3:
02247 self.action_goal.goal.grasp.id = str[start:end].decode('utf-8')
02248 else:
02249 self.action_goal.goal.grasp.id = str[start:end]
02250 _x = self
02251 start = end
02252 end += 12
02253 (_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])
02254 start = end
02255 end += 4
02256 (length,) = _struct_I.unpack(str[start:end])
02257 start = end
02258 end += length
02259 if python3:
02260 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02261 else:
02262 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end]
02263 start = end
02264 end += 4
02265 (length,) = _struct_I.unpack(str[start:end])
02266 self.action_goal.goal.grasp.pre_grasp_posture.name = []
02267 for i in range(0, length):
02268 start = end
02269 end += 4
02270 (length,) = _struct_I.unpack(str[start:end])
02271 start = end
02272 end += length
02273 if python3:
02274 val1 = str[start:end].decode('utf-8')
02275 else:
02276 val1 = str[start:end]
02277 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1)
02278 start = end
02279 end += 4
02280 (length,) = _struct_I.unpack(str[start:end])
02281 pattern = '<%sd'%length
02282 start = end
02283 end += struct.calcsize(pattern)
02284 self.action_goal.goal.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02285 start = end
02286 end += 4
02287 (length,) = _struct_I.unpack(str[start:end])
02288 pattern = '<%sd'%length
02289 start = end
02290 end += struct.calcsize(pattern)
02291 self.action_goal.goal.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02292 start = end
02293 end += 4
02294 (length,) = _struct_I.unpack(str[start:end])
02295 pattern = '<%sd'%length
02296 start = end
02297 end += struct.calcsize(pattern)
02298 self.action_goal.goal.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02299 _x = self
02300 start = end
02301 end += 12
02302 (_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])
02303 start = end
02304 end += 4
02305 (length,) = _struct_I.unpack(str[start:end])
02306 start = end
02307 end += length
02308 if python3:
02309 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02310 else:
02311 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end]
02312 start = end
02313 end += 4
02314 (length,) = _struct_I.unpack(str[start:end])
02315 self.action_goal.goal.grasp.grasp_posture.name = []
02316 for i in range(0, length):
02317 start = end
02318 end += 4
02319 (length,) = _struct_I.unpack(str[start:end])
02320 start = end
02321 end += length
02322 if python3:
02323 val1 = str[start:end].decode('utf-8')
02324 else:
02325 val1 = str[start:end]
02326 self.action_goal.goal.grasp.grasp_posture.name.append(val1)
02327 start = end
02328 end += 4
02329 (length,) = _struct_I.unpack(str[start:end])
02330 pattern = '<%sd'%length
02331 start = end
02332 end += struct.calcsize(pattern)
02333 self.action_goal.goal.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02334 start = end
02335 end += 4
02336 (length,) = _struct_I.unpack(str[start:end])
02337 pattern = '<%sd'%length
02338 start = end
02339 end += struct.calcsize(pattern)
02340 self.action_goal.goal.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02341 start = end
02342 end += 4
02343 (length,) = _struct_I.unpack(str[start:end])
02344 pattern = '<%sd'%length
02345 start = end
02346 end += struct.calcsize(pattern)
02347 self.action_goal.goal.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02348 _x = self
02349 start = end
02350 end += 12
02351 (_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02352 start = end
02353 end += 4
02354 (length,) = _struct_I.unpack(str[start:end])
02355 start = end
02356 end += length
02357 if python3:
02358 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
02359 else:
02360 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end]
02361 _x = self
02362 start = end
02363 end += 76
02364 (_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
02365 start = end
02366 end += 4
02367 (length,) = _struct_I.unpack(str[start:end])
02368 start = end
02369 end += length
02370 if python3:
02371 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
02372 else:
02373 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end]
02374 _x = self
02375 start = end
02376 end += 44
02377 (_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
02378 start = end
02379 end += 4
02380 (length,) = _struct_I.unpack(str[start:end])
02381 start = end
02382 end += length
02383 if python3:
02384 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
02385 else:
02386 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end]
02387 _x = self
02388 start = end
02389 end += 36
02390 (_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
02391 start = end
02392 end += 4
02393 (length,) = _struct_I.unpack(str[start:end])
02394 self.action_goal.goal.grasp.allowed_touch_objects = []
02395 for i in range(0, length):
02396 start = end
02397 end += 4
02398 (length,) = _struct_I.unpack(str[start:end])
02399 start = end
02400 end += length
02401 if python3:
02402 val1 = str[start:end].decode('utf-8')
02403 else:
02404 val1 = str[start:end]
02405 self.action_goal.goal.grasp.allowed_touch_objects.append(val1)
02406 _x = self
02407 start = end
02408 end += 20
02409 (_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_2f3I.unpack(str[start:end])
02410 start = end
02411 end += 4
02412 (length,) = _struct_I.unpack(str[start:end])
02413 start = end
02414 end += length
02415 if python3:
02416 self.action_goal.goal.approach.direction.header.frame_id = str[start:end].decode('utf-8')
02417 else:
02418 self.action_goal.goal.approach.direction.header.frame_id = str[start:end]
02419 _x = self
02420 start = end
02421 end += 32
02422 (_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])
02423 start = end
02424 end += 4
02425 (length,) = _struct_I.unpack(str[start:end])
02426 start = end
02427 end += length
02428 if python3:
02429 self.action_goal.goal.collision_object_name = str[start:end].decode('utf-8')
02430 else:
02431 self.action_goal.goal.collision_object_name = str[start:end]
02432 start = end
02433 end += 4
02434 (length,) = _struct_I.unpack(str[start:end])
02435 start = end
02436 end += length
02437 if python3:
02438 self.action_goal.goal.collision_support_surface_name = str[start:end].decode('utf-8')
02439 else:
02440 self.action_goal.goal.collision_support_surface_name = str[start:end]
02441 _x = self
02442 start = end
02443 end += 11
02444 (_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end])
02445 self.action_goal.goal.allow_gripper_support_collision = bool(self.action_goal.goal.allow_gripper_support_collision)
02446 self.action_goal.goal.use_reactive_place = bool(self.action_goal.goal.use_reactive_place)
02447 self.action_goal.goal.only_perform_feasibility_test = bool(self.action_goal.goal.only_perform_feasibility_test)
02448 start = end
02449 end += 4
02450 (length,) = _struct_I.unpack(str[start:end])
02451 self.action_goal.goal.path_constraints.joint_constraints = []
02452 for i in range(0, length):
02453 val1 = arm_navigation_msgs.msg.JointConstraint()
02454 start = end
02455 end += 4
02456 (length,) = _struct_I.unpack(str[start:end])
02457 start = end
02458 end += length
02459 if python3:
02460 val1.joint_name = str[start:end].decode('utf-8')
02461 else:
02462 val1.joint_name = str[start:end]
02463 _x = val1
02464 start = end
02465 end += 32
02466 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
02467 self.action_goal.goal.path_constraints.joint_constraints.append(val1)
02468 start = end
02469 end += 4
02470 (length,) = _struct_I.unpack(str[start:end])
02471 self.action_goal.goal.path_constraints.position_constraints = []
02472 for i in range(0, length):
02473 val1 = arm_navigation_msgs.msg.PositionConstraint()
02474 _v99 = val1.header
02475 start = end
02476 end += 4
02477 (_v99.seq,) = _struct_I.unpack(str[start:end])
02478 _v100 = _v99.stamp
02479 _x = _v100
02480 start = end
02481 end += 8
02482 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02483 start = end
02484 end += 4
02485 (length,) = _struct_I.unpack(str[start:end])
02486 start = end
02487 end += length
02488 if python3:
02489 _v99.frame_id = str[start:end].decode('utf-8')
02490 else:
02491 _v99.frame_id = str[start:end]
02492 start = end
02493 end += 4
02494 (length,) = _struct_I.unpack(str[start:end])
02495 start = end
02496 end += length
02497 if python3:
02498 val1.link_name = str[start:end].decode('utf-8')
02499 else:
02500 val1.link_name = str[start:end]
02501 _v101 = val1.target_point_offset
02502 _x = _v101
02503 start = end
02504 end += 24
02505 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02506 _v102 = val1.position
02507 _x = _v102
02508 start = end
02509 end += 24
02510 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02511 _v103 = val1.constraint_region_shape
02512 start = end
02513 end += 1
02514 (_v103.type,) = _struct_b.unpack(str[start:end])
02515 start = end
02516 end += 4
02517 (length,) = _struct_I.unpack(str[start:end])
02518 pattern = '<%sd'%length
02519 start = end
02520 end += struct.calcsize(pattern)
02521 _v103.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02522 start = end
02523 end += 4
02524 (length,) = _struct_I.unpack(str[start:end])
02525 pattern = '<%si'%length
02526 start = end
02527 end += struct.calcsize(pattern)
02528 _v103.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02529 start = end
02530 end += 4
02531 (length,) = _struct_I.unpack(str[start:end])
02532 _v103.vertices = []
02533 for i in range(0, length):
02534 val3 = geometry_msgs.msg.Point()
02535 _x = val3
02536 start = end
02537 end += 24
02538 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02539 _v103.vertices.append(val3)
02540 _v104 = val1.constraint_region_orientation
02541 _x = _v104
02542 start = end
02543 end += 32
02544 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02545 start = end
02546 end += 8
02547 (val1.weight,) = _struct_d.unpack(str[start:end])
02548 self.action_goal.goal.path_constraints.position_constraints.append(val1)
02549 start = end
02550 end += 4
02551 (length,) = _struct_I.unpack(str[start:end])
02552 self.action_goal.goal.path_constraints.orientation_constraints = []
02553 for i in range(0, length):
02554 val1 = arm_navigation_msgs.msg.OrientationConstraint()
02555 _v105 = val1.header
02556 start = end
02557 end += 4
02558 (_v105.seq,) = _struct_I.unpack(str[start:end])
02559 _v106 = _v105.stamp
02560 _x = _v106
02561 start = end
02562 end += 8
02563 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02564 start = end
02565 end += 4
02566 (length,) = _struct_I.unpack(str[start:end])
02567 start = end
02568 end += length
02569 if python3:
02570 _v105.frame_id = str[start:end].decode('utf-8')
02571 else:
02572 _v105.frame_id = str[start:end]
02573 start = end
02574 end += 4
02575 (length,) = _struct_I.unpack(str[start:end])
02576 start = end
02577 end += length
02578 if python3:
02579 val1.link_name = str[start:end].decode('utf-8')
02580 else:
02581 val1.link_name = str[start:end]
02582 start = end
02583 end += 4
02584 (val1.type,) = _struct_i.unpack(str[start:end])
02585 _v107 = val1.orientation
02586 _x = _v107
02587 start = end
02588 end += 32
02589 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02590 _x = val1
02591 start = end
02592 end += 32
02593 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02594 self.action_goal.goal.path_constraints.orientation_constraints.append(val1)
02595 start = end
02596 end += 4
02597 (length,) = _struct_I.unpack(str[start:end])
02598 self.action_goal.goal.path_constraints.visibility_constraints = []
02599 for i in range(0, length):
02600 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02601 _v108 = val1.header
02602 start = end
02603 end += 4
02604 (_v108.seq,) = _struct_I.unpack(str[start:end])
02605 _v109 = _v108.stamp
02606 _x = _v109
02607 start = end
02608 end += 8
02609 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02610 start = end
02611 end += 4
02612 (length,) = _struct_I.unpack(str[start:end])
02613 start = end
02614 end += length
02615 if python3:
02616 _v108.frame_id = str[start:end].decode('utf-8')
02617 else:
02618 _v108.frame_id = str[start:end]
02619 _v110 = val1.target
02620 _v111 = _v110.header
02621 start = end
02622 end += 4
02623 (_v111.seq,) = _struct_I.unpack(str[start:end])
02624 _v112 = _v111.stamp
02625 _x = _v112
02626 start = end
02627 end += 8
02628 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02629 start = end
02630 end += 4
02631 (length,) = _struct_I.unpack(str[start:end])
02632 start = end
02633 end += length
02634 if python3:
02635 _v111.frame_id = str[start:end].decode('utf-8')
02636 else:
02637 _v111.frame_id = str[start:end]
02638 _v113 = _v110.point
02639 _x = _v113
02640 start = end
02641 end += 24
02642 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02643 _v114 = val1.sensor_pose
02644 _v115 = _v114.header
02645 start = end
02646 end += 4
02647 (_v115.seq,) = _struct_I.unpack(str[start:end])
02648 _v116 = _v115.stamp
02649 _x = _v116
02650 start = end
02651 end += 8
02652 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02653 start = end
02654 end += 4
02655 (length,) = _struct_I.unpack(str[start:end])
02656 start = end
02657 end += length
02658 if python3:
02659 _v115.frame_id = str[start:end].decode('utf-8')
02660 else:
02661 _v115.frame_id = str[start:end]
02662 _v117 = _v114.pose
02663 _v118 = _v117.position
02664 _x = _v118
02665 start = end
02666 end += 24
02667 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02668 _v119 = _v117.orientation
02669 _x = _v119
02670 start = end
02671 end += 32
02672 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02673 start = end
02674 end += 8
02675 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02676 self.action_goal.goal.path_constraints.visibility_constraints.append(val1)
02677 start = end
02678 end += 4
02679 (length,) = _struct_I.unpack(str[start:end])
02680 self.action_goal.goal.additional_collision_operations.collision_operations = []
02681 for i in range(0, length):
02682 val1 = arm_navigation_msgs.msg.CollisionOperation()
02683 start = end
02684 end += 4
02685 (length,) = _struct_I.unpack(str[start:end])
02686 start = end
02687 end += length
02688 if python3:
02689 val1.object1 = str[start:end].decode('utf-8')
02690 else:
02691 val1.object1 = str[start:end]
02692 start = end
02693 end += 4
02694 (length,) = _struct_I.unpack(str[start:end])
02695 start = end
02696 end += length
02697 if python3:
02698 val1.object2 = str[start:end].decode('utf-8')
02699 else:
02700 val1.object2 = str[start:end]
02701 _x = val1
02702 start = end
02703 end += 12
02704 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
02705 self.action_goal.goal.additional_collision_operations.collision_operations.append(val1)
02706 start = end
02707 end += 4
02708 (length,) = _struct_I.unpack(str[start:end])
02709 self.action_goal.goal.additional_link_padding = []
02710 for i in range(0, length):
02711 val1 = arm_navigation_msgs.msg.LinkPadding()
02712 start = end
02713 end += 4
02714 (length,) = _struct_I.unpack(str[start:end])
02715 start = end
02716 end += length
02717 if python3:
02718 val1.link_name = str[start:end].decode('utf-8')
02719 else:
02720 val1.link_name = str[start:end]
02721 start = end
02722 end += 8
02723 (val1.padding,) = _struct_d.unpack(str[start:end])
02724 self.action_goal.goal.additional_link_padding.append(val1)
02725 _x = self
02726 start = end
02727 end += 12
02728 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02729 start = end
02730 end += 4
02731 (length,) = _struct_I.unpack(str[start:end])
02732 start = end
02733 end += length
02734 if python3:
02735 self.action_result.header.frame_id = str[start:end].decode('utf-8')
02736 else:
02737 self.action_result.header.frame_id = str[start:end]
02738 _x = self
02739 start = end
02740 end += 8
02741 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02742 start = end
02743 end += 4
02744 (length,) = _struct_I.unpack(str[start:end])
02745 start = end
02746 end += length
02747 if python3:
02748 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
02749 else:
02750 self.action_result.status.goal_id.id = str[start:end]
02751 start = end
02752 end += 1
02753 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
02754 start = end
02755 end += 4
02756 (length,) = _struct_I.unpack(str[start:end])
02757 start = end
02758 end += length
02759 if python3:
02760 self.action_result.status.text = str[start:end].decode('utf-8')
02761 else:
02762 self.action_result.status.text = str[start:end]
02763 _x = self
02764 start = end
02765 end += 16
02766 (_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])
02767 start = end
02768 end += 4
02769 (length,) = _struct_I.unpack(str[start:end])
02770 start = end
02771 end += length
02772 if python3:
02773 self.action_result.result.place_location.header.frame_id = str[start:end].decode('utf-8')
02774 else:
02775 self.action_result.result.place_location.header.frame_id = str[start:end]
02776 _x = self
02777 start = end
02778 end += 56
02779 (_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])
02780 start = end
02781 end += 4
02782 (length,) = _struct_I.unpack(str[start:end])
02783 self.action_result.result.attempted_locations = []
02784 for i in range(0, length):
02785 val1 = geometry_msgs.msg.PoseStamped()
02786 _v120 = val1.header
02787 start = end
02788 end += 4
02789 (_v120.seq,) = _struct_I.unpack(str[start:end])
02790 _v121 = _v120.stamp
02791 _x = _v121
02792 start = end
02793 end += 8
02794 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02795 start = end
02796 end += 4
02797 (length,) = _struct_I.unpack(str[start:end])
02798 start = end
02799 end += length
02800 if python3:
02801 _v120.frame_id = str[start:end].decode('utf-8')
02802 else:
02803 _v120.frame_id = str[start:end]
02804 _v122 = val1.pose
02805 _v123 = _v122.position
02806 _x = _v123
02807 start = end
02808 end += 24
02809 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02810 _v124 = _v122.orientation
02811 _x = _v124
02812 start = end
02813 end += 32
02814 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02815 self.action_result.result.attempted_locations.append(val1)
02816 start = end
02817 end += 4
02818 (length,) = _struct_I.unpack(str[start:end])
02819 self.action_result.result.attempted_location_results = []
02820 for i in range(0, length):
02821 val1 = object_manipulation_msgs.msg.PlaceLocationResult()
02822 _x = val1
02823 start = end
02824 end += 5
02825 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
02826 val1.continuation_possible = bool(val1.continuation_possible)
02827 self.action_result.result.attempted_location_results.append(val1)
02828 _x = self
02829 start = end
02830 end += 12
02831 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02832 start = end
02833 end += 4
02834 (length,) = _struct_I.unpack(str[start:end])
02835 start = end
02836 end += length
02837 if python3:
02838 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
02839 else:
02840 self.action_feedback.header.frame_id = str[start:end]
02841 _x = self
02842 start = end
02843 end += 8
02844 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02845 start = end
02846 end += 4
02847 (length,) = _struct_I.unpack(str[start:end])
02848 start = end
02849 end += length
02850 if python3:
02851 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
02852 else:
02853 self.action_feedback.status.goal_id.id = str[start:end]
02854 start = end
02855 end += 1
02856 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
02857 start = end
02858 end += 4
02859 (length,) = _struct_I.unpack(str[start:end])
02860 start = end
02861 end += length
02862 if python3:
02863 self.action_feedback.status.text = str[start:end].decode('utf-8')
02864 else:
02865 self.action_feedback.status.text = str[start:end]
02866 _x = self
02867 start = end
02868 end += 8
02869 (_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations,) = _struct_2i.unpack(str[start:end])
02870 return self
02871 except struct.error as e:
02872 raise genpy.DeserializationError(e)
02873
02874 _struct_I = genpy.struct_I
02875 _struct_b = struct.Struct("<b")
02876 _struct_2f3I = struct.Struct("<2f3I")
02877 _struct_d = struct.Struct("<d")
02878 _struct_i3I = struct.Struct("<i3I")
02879 _struct_di = struct.Struct("<di")
02880 _struct_i = struct.Struct("<i")
02881 _struct_3d2f = struct.Struct("<3d2f")
02882 _struct_3d3f = struct.Struct("<3d3f")
02883 _struct_2i = struct.Struct("<2i")
02884 _struct_8d3I = struct.Struct("<8d3I")
02885 _struct_3I = struct.Struct("<3I")
02886 _struct_B = struct.Struct("<B")
02887 _struct_2BdB = struct.Struct("<2BdB")
02888 _struct_3d2f3I = struct.Struct("<3d2f3I")
02889 _struct_4d = struct.Struct("<4d")
02890 _struct_iB = struct.Struct("<iB")
02891 _struct_7d = struct.Struct("<7d")
02892 _struct_2I = struct.Struct("<2I")
02893 _struct_3d = struct.Struct("<3d")