00001 """autogenerated by genmsg_py from ReactiveGraspAction.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import trajectory_msgs.msg
00006 import object_manipulation_msgs.msg
00007 import roslib.rostime
00008 import actionlib_msgs.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import std_msgs.msg
00012 import household_objects_database_msgs.msg
00013
00014 class ReactiveGraspAction(roslib.message.Message):
00015 _md5sum = "a98e439d448e3a27be746925c86b6377"
00016 _type = "object_manipulation_msgs/ReactiveGraspAction"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 ReactiveGraspActionGoal action_goal
00021 ReactiveGraspActionResult action_result
00022 ReactiveGraspActionFeedback action_feedback
00023
00024 ================================================================================
00025 MSG: object_manipulation_msgs/ReactiveGraspActionGoal
00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00027
00028 Header header
00029 actionlib_msgs/GoalID goal_id
00030 ReactiveGraspGoal goal
00031
00032 ================================================================================
00033 MSG: std_msgs/Header
00034 # Standard metadata for higher-level stamped data types.
00035 # This is generally used to communicate timestamped data
00036 # in a particular coordinate frame.
00037 #
00038 # sequence ID: consecutively increasing ID
00039 uint32 seq
00040 #Two-integer timestamp that is expressed as:
00041 # * stamp.secs: seconds (stamp_secs) since epoch
00042 # * stamp.nsecs: nanoseconds since stamp_secs
00043 # time-handling sugar is provided by the client library
00044 time stamp
00045 #Frame this data is associated with
00046 # 0: no frame
00047 # 1: global frame
00048 string frame_id
00049
00050 ================================================================================
00051 MSG: actionlib_msgs/GoalID
00052 # The stamp should store the time at which this goal was requested.
00053 # It is used by an action server when it tries to preempt all
00054 # goals that were requested before a certain time
00055 time stamp
00056
00057 # The id provides a way to associate feedback and
00058 # result message with specific goal requests. The id
00059 # specified must be unique.
00060 string id
00061
00062
00063 ================================================================================
00064 MSG: object_manipulation_msgs/ReactiveGraspGoal
00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00066 # an action for reactive grasping
00067 # a reactive grasp starts from the current pose of the gripper and ends
00068 # at a desired grasp pose, presumably using the touch sensors along the way
00069
00070 # the name of the arm being used
00071 string arm_name
00072
00073 # the object to be grasped
00074 GraspableObject target
00075
00076 # the desired grasp pose for the hand
00077 geometry_msgs/PoseStamped final_grasp_pose
00078
00079 # the joint trajectory to use for the approach (if available)
00080 # this trajectory is expected to start at the current pose of the gripper
00081 # and end at the desired grasp pose
00082 trajectory_msgs/JointTrajectory trajectory
00083
00084 # the name of the support surface in the collision environment, if any
00085 string collision_support_surface_name
00086
00087 # The internal posture of the hand for the pre-grasp
00088 # only positions are used
00089 sensor_msgs/JointState pre_grasp_posture
00090
00091 # The internal posture of the hand for the grasp
00092 # positions and efforts are used
00093 sensor_msgs/JointState grasp_posture
00094
00095
00096 ================================================================================
00097 MSG: object_manipulation_msgs/GraspableObject
00098 # an object that the object_manipulator can work on
00099
00100 # a graspable object can be represented in multiple ways. This message
00101 # can contain all of them. Which one is actually used is up to the receiver
00102 # of this message. When adding new representations, one must be careful that
00103 # they have reasonable lightweight defaults indicating that that particular
00104 # representation is not available.
00105
00106 # the tf frame to be used as a reference frame when combining information from
00107 # the different representations below
00108 string reference_frame_id
00109
00110 # potential recognition results from a database of models
00111 # all poses are relative to the object reference pose
00112 household_objects_database_msgs/DatabaseModelPose[] potential_models
00113
00114 # the point cloud itself
00115 sensor_msgs/PointCloud cluster
00116
00117 # a region of a PointCloud2 of interest
00118 object_manipulation_msgs/SceneRegion region
00119
00120
00121 ================================================================================
00122 MSG: household_objects_database_msgs/DatabaseModelPose
00123 # Informs that a specific model from the Model Database has been
00124 # identified at a certain location
00125
00126 # the database id of the model
00127 int32 model_id
00128
00129 # the pose that it can be found in
00130 geometry_msgs/PoseStamped pose
00131
00132 # a measure of the confidence level in this detection result
00133 float32 confidence
00134 ================================================================================
00135 MSG: geometry_msgs/PoseStamped
00136 # A Pose with reference coordinate frame and timestamp
00137 Header header
00138 Pose pose
00139
00140 ================================================================================
00141 MSG: geometry_msgs/Pose
00142 # A representation of pose in free space, composed of postion and orientation.
00143 Point position
00144 Quaternion orientation
00145
00146 ================================================================================
00147 MSG: geometry_msgs/Point
00148 # This contains the position of a point in free space
00149 float64 x
00150 float64 y
00151 float64 z
00152
00153 ================================================================================
00154 MSG: geometry_msgs/Quaternion
00155 # This represents an orientation in free space in quaternion form.
00156
00157 float64 x
00158 float64 y
00159 float64 z
00160 float64 w
00161
00162 ================================================================================
00163 MSG: sensor_msgs/PointCloud
00164 # This message holds a collection of 3d points, plus optional additional
00165 # information about each point.
00166
00167 # Time of sensor data acquisition, coordinate frame ID.
00168 Header header
00169
00170 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00171 # in the frame given in the header.
00172 geometry_msgs/Point32[] points
00173
00174 # Each channel should have the same number of elements as points array,
00175 # and the data in each channel should correspond 1:1 with each point.
00176 # Channel names in common practice are listed in ChannelFloat32.msg.
00177 ChannelFloat32[] channels
00178
00179 ================================================================================
00180 MSG: geometry_msgs/Point32
00181 # This contains the position of a point in free space(with 32 bits of precision).
00182 # It is recommeded to use Point wherever possible instead of Point32.
00183 #
00184 # This recommendation is to promote interoperability.
00185 #
00186 # This message is designed to take up less space when sending
00187 # lots of points at once, as in the case of a PointCloud.
00188
00189 float32 x
00190 float32 y
00191 float32 z
00192 ================================================================================
00193 MSG: sensor_msgs/ChannelFloat32
00194 # This message is used by the PointCloud message to hold optional data
00195 # associated with each point in the cloud. The length of the values
00196 # array should be the same as the length of the points array in the
00197 # PointCloud, and each value should be associated with the corresponding
00198 # point.
00199
00200 # Channel names in existing practice include:
00201 # "u", "v" - row and column (respectively) in the left stereo image.
00202 # This is opposite to usual conventions but remains for
00203 # historical reasons. The newer PointCloud2 message has no
00204 # such problem.
00205 # "rgb" - For point clouds produced by color stereo cameras. uint8
00206 # (R,G,B) values packed into the least significant 24 bits,
00207 # in order.
00208 # "intensity" - laser or pixel intensity.
00209 # "distance"
00210
00211 # The channel name should give semantics of the channel (e.g.
00212 # "intensity" instead of "value").
00213 string name
00214
00215 # The values array should be 1-1 with the elements of the associated
00216 # PointCloud.
00217 float32[] values
00218
00219 ================================================================================
00220 MSG: object_manipulation_msgs/SceneRegion
00221 # Point cloud
00222 sensor_msgs/PointCloud2 cloud
00223
00224 # Indices for the region of interest
00225 int32[] mask
00226
00227 # One of the corresponding 2D images, if applicable
00228 sensor_msgs/Image image
00229
00230 # The disparity image, if applicable
00231 sensor_msgs/Image disparity_image
00232
00233 # Camera info for the camera that took the image
00234 sensor_msgs/CameraInfo cam_info
00235
00236 ================================================================================
00237 MSG: sensor_msgs/PointCloud2
00238 # This message holds a collection of N-dimensional points, which may
00239 # contain additional information such as normals, intensity, etc. The
00240 # point data is stored as a binary blob, its layout described by the
00241 # contents of the "fields" array.
00242
00243 # The point cloud data may be organized 2d (image-like) or 1d
00244 # (unordered). Point clouds organized as 2d images may be produced by
00245 # camera depth sensors such as stereo or time-of-flight.
00246
00247 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00248 # points).
00249 Header header
00250
00251 # 2D structure of the point cloud. If the cloud is unordered, height is
00252 # 1 and width is the length of the point cloud.
00253 uint32 height
00254 uint32 width
00255
00256 # Describes the channels and their layout in the binary data blob.
00257 PointField[] fields
00258
00259 bool is_bigendian # Is this data bigendian?
00260 uint32 point_step # Length of a point in bytes
00261 uint32 row_step # Length of a row in bytes
00262 uint8[] data # Actual point data, size is (row_step*height)
00263
00264 bool is_dense # True if there are no invalid points
00265
00266 ================================================================================
00267 MSG: sensor_msgs/PointField
00268 # This message holds the description of one point entry in the
00269 # PointCloud2 message format.
00270 uint8 INT8 = 1
00271 uint8 UINT8 = 2
00272 uint8 INT16 = 3
00273 uint8 UINT16 = 4
00274 uint8 INT32 = 5
00275 uint8 UINT32 = 6
00276 uint8 FLOAT32 = 7
00277 uint8 FLOAT64 = 8
00278
00279 string name # Name of field
00280 uint32 offset # Offset from start of point struct
00281 uint8 datatype # Datatype enumeration, see above
00282 uint32 count # How many elements in the field
00283
00284 ================================================================================
00285 MSG: sensor_msgs/Image
00286 # This message contains an uncompressed image
00287 # (0, 0) is at top-left corner of image
00288 #
00289
00290 Header header # Header timestamp should be acquisition time of image
00291 # Header frame_id should be optical frame of camera
00292 # origin of frame should be optical center of cameara
00293 # +x should point to the right in the image
00294 # +y should point down in the image
00295 # +z should point into to plane of the image
00296 # If the frame_id here and the frame_id of the CameraInfo
00297 # message associated with the image conflict
00298 # the behavior is undefined
00299
00300 uint32 height # image height, that is, number of rows
00301 uint32 width # image width, that is, number of columns
00302
00303 # The legal values for encoding are in file src/image_encodings.cpp
00304 # If you want to standardize a new string format, join
00305 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00306
00307 string encoding # Encoding of pixels -- channel meaning, ordering, size
00308 # taken from the list of strings in src/image_encodings.cpp
00309
00310 uint8 is_bigendian # is this data bigendian?
00311 uint32 step # Full row length in bytes
00312 uint8[] data # actual matrix data, size is (step * rows)
00313
00314 ================================================================================
00315 MSG: sensor_msgs/CameraInfo
00316 # This message defines meta information for a camera. It should be in a
00317 # camera namespace on topic "camera_info" and accompanied by up to five
00318 # image topics named:
00319 #
00320 # image_raw - raw data from the camera driver, possibly Bayer encoded
00321 # image - monochrome, distorted
00322 # image_color - color, distorted
00323 # image_rect - monochrome, rectified
00324 # image_rect_color - color, rectified
00325 #
00326 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00327 # for producing the four processed image topics from image_raw and
00328 # camera_info. The meaning of the camera parameters are described in
00329 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00330 #
00331 # The image_geometry package provides a user-friendly interface to
00332 # common operations using this meta information. If you want to, e.g.,
00333 # project a 3d point into image coordinates, we strongly recommend
00334 # using image_geometry.
00335 #
00336 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00337 # zeroed out. In particular, clients may assume that K[0] == 0.0
00338 # indicates an uncalibrated camera.
00339
00340 #######################################################################
00341 # Image acquisition info #
00342 #######################################################################
00343
00344 # Time of image acquisition, camera coordinate frame ID
00345 Header header # Header timestamp should be acquisition time of image
00346 # Header frame_id should be optical frame of camera
00347 # origin of frame should be optical center of camera
00348 # +x should point to the right in the image
00349 # +y should point down in the image
00350 # +z should point into the plane of the image
00351
00352
00353 #######################################################################
00354 # Calibration Parameters #
00355 #######################################################################
00356 # These are fixed during camera calibration. Their values will be the #
00357 # same in all messages until the camera is recalibrated. Note that #
00358 # self-calibrating systems may "recalibrate" frequently. #
00359 # #
00360 # The internal parameters can be used to warp a raw (distorted) image #
00361 # to: #
00362 # 1. An undistorted image (requires D and K) #
00363 # 2. A rectified image (requires D, K, R) #
00364 # The projection matrix P projects 3D points into the rectified image.#
00365 #######################################################################
00366
00367 # The image dimensions with which the camera was calibrated. Normally
00368 # this will be the full camera resolution in pixels.
00369 uint32 height
00370 uint32 width
00371
00372 # The distortion model used. Supported models are listed in
00373 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00374 # simple model of radial and tangential distortion - is sufficent.
00375 string distortion_model
00376
00377 # The distortion parameters, size depending on the distortion model.
00378 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00379 float64[] D
00380
00381 # Intrinsic camera matrix for the raw (distorted) images.
00382 # [fx 0 cx]
00383 # K = [ 0 fy cy]
00384 # [ 0 0 1]
00385 # Projects 3D points in the camera coordinate frame to 2D pixel
00386 # coordinates using the focal lengths (fx, fy) and principal point
00387 # (cx, cy).
00388 float64[9] K # 3x3 row-major matrix
00389
00390 # Rectification matrix (stereo cameras only)
00391 # A rotation matrix aligning the camera coordinate system to the ideal
00392 # stereo image plane so that epipolar lines in both stereo images are
00393 # parallel.
00394 float64[9] R # 3x3 row-major matrix
00395
00396 # Projection/camera matrix
00397 # [fx' 0 cx' Tx]
00398 # P = [ 0 fy' cy' Ty]
00399 # [ 0 0 1 0]
00400 # By convention, this matrix specifies the intrinsic (camera) matrix
00401 # of the processed (rectified) image. That is, the left 3x3 portion
00402 # is the normal camera intrinsic matrix for the rectified image.
00403 # It projects 3D points in the camera coordinate frame to 2D pixel
00404 # coordinates using the focal lengths (fx', fy') and principal point
00405 # (cx', cy') - these may differ from the values in K.
00406 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00407 # also have R = the identity and P[1:3,1:3] = K.
00408 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00409 # position of the optical center of the second camera in the first
00410 # camera's frame. We assume Tz = 0 so both cameras are in the same
00411 # stereo image plane. The first camera always has Tx = Ty = 0. For
00412 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00413 # Tx = -fx' * B, where B is the baseline between the cameras.
00414 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00415 # the rectified image is given by:
00416 # [u v w]' = P * [X Y Z 1]'
00417 # x = u / w
00418 # y = v / w
00419 # This holds for both images of a stereo pair.
00420 float64[12] P # 3x4 row-major matrix
00421
00422
00423 #######################################################################
00424 # Operational Parameters #
00425 #######################################################################
00426 # These define the image region actually captured by the camera #
00427 # driver. Although they affect the geometry of the output image, they #
00428 # may be changed freely without recalibrating the camera. #
00429 #######################################################################
00430
00431 # Binning refers here to any camera setting which combines rectangular
00432 # neighborhoods of pixels into larger "super-pixels." It reduces the
00433 # resolution of the output image to
00434 # (width / binning_x) x (height / binning_y).
00435 # The default values binning_x = binning_y = 0 is considered the same
00436 # as binning_x = binning_y = 1 (no subsampling).
00437 uint32 binning_x
00438 uint32 binning_y
00439
00440 # Region of interest (subwindow of full camera resolution), given in
00441 # full resolution (unbinned) image coordinates. A particular ROI
00442 # always denotes the same window of pixels on the camera sensor,
00443 # regardless of binning settings.
00444 # The default setting of roi (all values 0) is considered the same as
00445 # full resolution (roi.width = width, roi.height = height).
00446 RegionOfInterest roi
00447
00448 ================================================================================
00449 MSG: sensor_msgs/RegionOfInterest
00450 # This message is used to specify a region of interest within an image.
00451 #
00452 # When used to specify the ROI setting of the camera when the image was
00453 # taken, the height and width fields should either match the height and
00454 # width fields for the associated image; or height = width = 0
00455 # indicates that the full resolution image was captured.
00456
00457 uint32 x_offset # Leftmost pixel of the ROI
00458 # (0 if the ROI includes the left edge of the image)
00459 uint32 y_offset # Topmost pixel of the ROI
00460 # (0 if the ROI includes the top edge of the image)
00461 uint32 height # Height of ROI
00462 uint32 width # Width of ROI
00463
00464 # True if a distinct rectified ROI should be calculated from the "raw"
00465 # ROI in this message. Typically this should be False if the full image
00466 # is captured (ROI not used), and True if a subwindow is captured (ROI
00467 # used).
00468 bool do_rectify
00469
00470 ================================================================================
00471 MSG: trajectory_msgs/JointTrajectory
00472 Header header
00473 string[] joint_names
00474 JointTrajectoryPoint[] points
00475 ================================================================================
00476 MSG: trajectory_msgs/JointTrajectoryPoint
00477 float64[] positions
00478 float64[] velocities
00479 float64[] accelerations
00480 duration time_from_start
00481 ================================================================================
00482 MSG: sensor_msgs/JointState
00483 # This is a message that holds data to describe the state of a set of torque controlled joints.
00484 #
00485 # The state of each joint (revolute or prismatic) is defined by:
00486 # * the position of the joint (rad or m),
00487 # * the velocity of the joint (rad/s or m/s) and
00488 # * the effort that is applied in the joint (Nm or N).
00489 #
00490 # Each joint is uniquely identified by its name
00491 # The header specifies the time at which the joint states were recorded. All the joint states
00492 # in one message have to be recorded at the same time.
00493 #
00494 # This message consists of a multiple arrays, one for each part of the joint state.
00495 # The goal is to make each of the fields optional. When e.g. your joints have no
00496 # effort associated with them, you can leave the effort array empty.
00497 #
00498 # All arrays in this message should have the same size, or be empty.
00499 # This is the only way to uniquely associate the joint name with the correct
00500 # states.
00501
00502
00503 Header header
00504
00505 string[] name
00506 float64[] position
00507 float64[] velocity
00508 float64[] effort
00509
00510 ================================================================================
00511 MSG: object_manipulation_msgs/ReactiveGraspActionResult
00512 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00513
00514 Header header
00515 actionlib_msgs/GoalStatus status
00516 ReactiveGraspResult result
00517
00518 ================================================================================
00519 MSG: actionlib_msgs/GoalStatus
00520 GoalID goal_id
00521 uint8 status
00522 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00523 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00524 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00525 # and has since completed its execution (Terminal State)
00526 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00527 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00528 # to some failure (Terminal State)
00529 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00530 # because the goal was unattainable or invalid (Terminal State)
00531 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00532 # and has not yet completed execution
00533 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00534 # but the action server has not yet confirmed that the goal is canceled
00535 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00536 # and was successfully cancelled (Terminal State)
00537 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00538 # sent over the wire by an action server
00539
00540 #Allow for the user to associate a string with GoalStatus for debugging
00541 string text
00542
00543
00544 ================================================================================
00545 MSG: object_manipulation_msgs/ReactiveGraspResult
00546 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00547
00548 # the result of the reactive grasp attempt
00549
00550 ManipulationResult manipulation_result
00551
00552
00553 ================================================================================
00554 MSG: object_manipulation_msgs/ManipulationResult
00555 # Result codes for manipulation tasks
00556
00557 # task completed as expected
00558 # generally means you can proceed as planned
00559 int32 SUCCESS = 1
00560
00561 # task not possible (e.g. out of reach or obstacles in the way)
00562 # generally means that the world was not disturbed, so you can try another task
00563 int32 UNFEASIBLE = -1
00564
00565 # task was thought possible, but failed due to unexpected events during execution
00566 # it is likely that the world was disturbed, so you are encouraged to refresh
00567 # your sensed world model before proceeding to another task
00568 int32 FAILED = -2
00569
00570 # a lower level error prevented task completion (e.g. joint controller not responding)
00571 # generally requires human attention
00572 int32 ERROR = -3
00573
00574 # means that at some point during execution we ended up in a state that the collision-aware
00575 # arm navigation module will not move out of. The world was likely not disturbed, but you
00576 # probably need a new collision map to move the arm out of the stuck position
00577 int32 ARM_MOVEMENT_PREVENTED = -4
00578
00579 # specific to grasp actions
00580 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00581 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00582 int32 LIFT_FAILED = -5
00583
00584 # specific to place actions
00585 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00586 # it is likely that the collision environment will see collisions between the hand and the object
00587 int32 RETREAT_FAILED = -6
00588
00589 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00590 int32 CANCELLED = -7
00591
00592 # the actual value of this error code
00593 int32 value
00594
00595 ================================================================================
00596 MSG: object_manipulation_msgs/ReactiveGraspActionFeedback
00597 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00598
00599 Header header
00600 actionlib_msgs/GoalStatus status
00601 ReactiveGraspFeedback feedback
00602
00603 ================================================================================
00604 MSG: object_manipulation_msgs/ReactiveGraspFeedback
00605 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00606
00607 # which phase the grasp is in
00608
00609 ManipulationPhase manipulation_phase
00610
00611
00612
00613
00614 ================================================================================
00615 MSG: object_manipulation_msgs/ManipulationPhase
00616 int32 CHECKING_FEASIBILITY = 0
00617 int32 MOVING_TO_PREGRASP = 1
00618 int32 MOVING_TO_GRASP = 2
00619 int32 CLOSING = 3
00620 int32 ADJUSTING_GRASP = 4
00621 int32 LIFTING = 5
00622 int32 MOVING_WITH_OBJECT = 6
00623 int32 MOVING_TO_PLACE = 7
00624 int32 PLACING = 8
00625 int32 OPENING = 9
00626 int32 RETREATING = 10
00627 int32 MOVING_WITHOUT_OBJECT = 11
00628 int32 SHAKING = 12
00629 int32 SUCCEEDED = 13
00630 int32 FAILED = 14
00631 int32 ABORTED = 15
00632 int32 HOLDING_OBJECT = 16
00633
00634 int32 phase
00635 """
00636 __slots__ = ['action_goal','action_result','action_feedback']
00637 _slot_types = ['object_manipulation_msgs/ReactiveGraspActionGoal','object_manipulation_msgs/ReactiveGraspActionResult','object_manipulation_msgs/ReactiveGraspActionFeedback']
00638
00639 def __init__(self, *args, **kwds):
00640 """
00641 Constructor. Any message fields that are implicitly/explicitly
00642 set to None will be assigned a default value. The recommend
00643 use is keyword arguments as this is more robust to future message
00644 changes. You cannot mix in-order arguments and keyword arguments.
00645
00646 The available fields are:
00647 action_goal,action_result,action_feedback
00648
00649 @param args: complete set of field values, in .msg order
00650 @param kwds: use keyword arguments corresponding to message field names
00651 to set specific fields.
00652 """
00653 if args or kwds:
00654 super(ReactiveGraspAction, self).__init__(*args, **kwds)
00655 #message fields cannot be None, assign default values for those that are
00656 if self.action_goal is None:
00657 self.action_goal = object_manipulation_msgs.msg.ReactiveGraspActionGoal()
00658 if self.action_result is None:
00659 self.action_result = object_manipulation_msgs.msg.ReactiveGraspActionResult()
00660 if self.action_feedback is None:
00661 self.action_feedback = object_manipulation_msgs.msg.ReactiveGraspActionFeedback()
00662 else:
00663 self.action_goal = object_manipulation_msgs.msg.ReactiveGraspActionGoal()
00664 self.action_result = object_manipulation_msgs.msg.ReactiveGraspActionResult()
00665 self.action_feedback = object_manipulation_msgs.msg.ReactiveGraspActionFeedback()
00666
00667 def _get_types(self):
00668 """
00669 internal API method
00670 """
00671 return self._slot_types
00672
00673 def serialize(self, buff):
00674 """
00675 serialize message into buffer
00676 @param buff: buffer
00677 @type buff: StringIO
00678 """
00679 try:
00680 _x = self
00681 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00682 _x = self.action_goal.header.frame_id
00683 length = len(_x)
00684 buff.write(struct.pack('<I%ss'%length, length, _x))
00685 _x = self
00686 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00687 _x = self.action_goal.goal_id.id
00688 length = len(_x)
00689 buff.write(struct.pack('<I%ss'%length, length, _x))
00690 _x = self.action_goal.goal.arm_name
00691 length = len(_x)
00692 buff.write(struct.pack('<I%ss'%length, length, _x))
00693 _x = self.action_goal.goal.target.reference_frame_id
00694 length = len(_x)
00695 buff.write(struct.pack('<I%ss'%length, length, _x))
00696 length = len(self.action_goal.goal.target.potential_models)
00697 buff.write(_struct_I.pack(length))
00698 for val1 in self.action_goal.goal.target.potential_models:
00699 buff.write(_struct_i.pack(val1.model_id))
00700 _v1 = val1.pose
00701 _v2 = _v1.header
00702 buff.write(_struct_I.pack(_v2.seq))
00703 _v3 = _v2.stamp
00704 _x = _v3
00705 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00706 _x = _v2.frame_id
00707 length = len(_x)
00708 buff.write(struct.pack('<I%ss'%length, length, _x))
00709 _v4 = _v1.pose
00710 _v5 = _v4.position
00711 _x = _v5
00712 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00713 _v6 = _v4.orientation
00714 _x = _v6
00715 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00716 buff.write(_struct_f.pack(val1.confidence))
00717 _x = self
00718 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs))
00719 _x = self.action_goal.goal.target.cluster.header.frame_id
00720 length = len(_x)
00721 buff.write(struct.pack('<I%ss'%length, length, _x))
00722 length = len(self.action_goal.goal.target.cluster.points)
00723 buff.write(_struct_I.pack(length))
00724 for val1 in self.action_goal.goal.target.cluster.points:
00725 _x = val1
00726 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00727 length = len(self.action_goal.goal.target.cluster.channels)
00728 buff.write(_struct_I.pack(length))
00729 for val1 in self.action_goal.goal.target.cluster.channels:
00730 _x = val1.name
00731 length = len(_x)
00732 buff.write(struct.pack('<I%ss'%length, length, _x))
00733 length = len(val1.values)
00734 buff.write(_struct_I.pack(length))
00735 pattern = '<%sf'%length
00736 buff.write(struct.pack(pattern, *val1.values))
00737 _x = self
00738 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs))
00739 _x = self.action_goal.goal.target.region.cloud.header.frame_id
00740 length = len(_x)
00741 buff.write(struct.pack('<I%ss'%length, length, _x))
00742 _x = self
00743 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width))
00744 length = len(self.action_goal.goal.target.region.cloud.fields)
00745 buff.write(_struct_I.pack(length))
00746 for val1 in self.action_goal.goal.target.region.cloud.fields:
00747 _x = val1.name
00748 length = len(_x)
00749 buff.write(struct.pack('<I%ss'%length, length, _x))
00750 _x = val1
00751 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00752 _x = self
00753 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step))
00754 _x = self.action_goal.goal.target.region.cloud.data
00755 length = len(_x)
00756 # - if encoded as a list instead, serialize as bytes instead of string
00757 if type(_x) in [list, tuple]:
00758 buff.write(struct.pack('<I%sB'%length, length, *_x))
00759 else:
00760 buff.write(struct.pack('<I%ss'%length, length, _x))
00761 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense))
00762 length = len(self.action_goal.goal.target.region.mask)
00763 buff.write(_struct_I.pack(length))
00764 pattern = '<%si'%length
00765 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.mask))
00766 _x = self
00767 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs))
00768 _x = self.action_goal.goal.target.region.image.header.frame_id
00769 length = len(_x)
00770 buff.write(struct.pack('<I%ss'%length, length, _x))
00771 _x = self
00772 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width))
00773 _x = self.action_goal.goal.target.region.image.encoding
00774 length = len(_x)
00775 buff.write(struct.pack('<I%ss'%length, length, _x))
00776 _x = self
00777 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step))
00778 _x = self.action_goal.goal.target.region.image.data
00779 length = len(_x)
00780 # - if encoded as a list instead, serialize as bytes instead of string
00781 if type(_x) in [list, tuple]:
00782 buff.write(struct.pack('<I%sB'%length, length, *_x))
00783 else:
00784 buff.write(struct.pack('<I%ss'%length, length, _x))
00785 _x = self
00786 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs))
00787 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id
00788 length = len(_x)
00789 buff.write(struct.pack('<I%ss'%length, length, _x))
00790 _x = self
00791 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width))
00792 _x = self.action_goal.goal.target.region.disparity_image.encoding
00793 length = len(_x)
00794 buff.write(struct.pack('<I%ss'%length, length, _x))
00795 _x = self
00796 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step))
00797 _x = self.action_goal.goal.target.region.disparity_image.data
00798 length = len(_x)
00799 # - if encoded as a list instead, serialize as bytes instead of string
00800 if type(_x) in [list, tuple]:
00801 buff.write(struct.pack('<I%sB'%length, length, *_x))
00802 else:
00803 buff.write(struct.pack('<I%ss'%length, length, _x))
00804 _x = self
00805 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs))
00806 _x = self.action_goal.goal.target.region.cam_info.header.frame_id
00807 length = len(_x)
00808 buff.write(struct.pack('<I%ss'%length, length, _x))
00809 _x = self
00810 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width))
00811 _x = self.action_goal.goal.target.region.cam_info.distortion_model
00812 length = len(_x)
00813 buff.write(struct.pack('<I%ss'%length, length, _x))
00814 length = len(self.action_goal.goal.target.region.cam_info.D)
00815 buff.write(_struct_I.pack(length))
00816 pattern = '<%sd'%length
00817 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.cam_info.D))
00818 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.K))
00819 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.R))
00820 buff.write(_struct_12d.pack(*self.action_goal.goal.target.region.cam_info.P))
00821 _x = self
00822 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.final_grasp_pose.header.seq, _x.action_goal.goal.final_grasp_pose.header.stamp.secs, _x.action_goal.goal.final_grasp_pose.header.stamp.nsecs))
00823 _x = self.action_goal.goal.final_grasp_pose.header.frame_id
00824 length = len(_x)
00825 buff.write(struct.pack('<I%ss'%length, length, _x))
00826 _x = self
00827 buff.write(_struct_7d3I.pack(_x.action_goal.goal.final_grasp_pose.pose.position.x, _x.action_goal.goal.final_grasp_pose.pose.position.y, _x.action_goal.goal.final_grasp_pose.pose.position.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.x, _x.action_goal.goal.final_grasp_pose.pose.orientation.y, _x.action_goal.goal.final_grasp_pose.pose.orientation.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.w, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs))
00828 _x = self.action_goal.goal.trajectory.header.frame_id
00829 length = len(_x)
00830 buff.write(struct.pack('<I%ss'%length, length, _x))
00831 length = len(self.action_goal.goal.trajectory.joint_names)
00832 buff.write(_struct_I.pack(length))
00833 for val1 in self.action_goal.goal.trajectory.joint_names:
00834 length = len(val1)
00835 buff.write(struct.pack('<I%ss'%length, length, val1))
00836 length = len(self.action_goal.goal.trajectory.points)
00837 buff.write(_struct_I.pack(length))
00838 for val1 in self.action_goal.goal.trajectory.points:
00839 length = len(val1.positions)
00840 buff.write(_struct_I.pack(length))
00841 pattern = '<%sd'%length
00842 buff.write(struct.pack(pattern, *val1.positions))
00843 length = len(val1.velocities)
00844 buff.write(_struct_I.pack(length))
00845 pattern = '<%sd'%length
00846 buff.write(struct.pack(pattern, *val1.velocities))
00847 length = len(val1.accelerations)
00848 buff.write(_struct_I.pack(length))
00849 pattern = '<%sd'%length
00850 buff.write(struct.pack(pattern, *val1.accelerations))
00851 _v7 = val1.time_from_start
00852 _x = _v7
00853 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00854 _x = self.action_goal.goal.collision_support_surface_name
00855 length = len(_x)
00856 buff.write(struct.pack('<I%ss'%length, length, _x))
00857 _x = self
00858 buff.write(_struct_3I.pack(_x.action_goal.goal.pre_grasp_posture.header.seq, _x.action_goal.goal.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.pre_grasp_posture.header.stamp.nsecs))
00859 _x = self.action_goal.goal.pre_grasp_posture.header.frame_id
00860 length = len(_x)
00861 buff.write(struct.pack('<I%ss'%length, length, _x))
00862 length = len(self.action_goal.goal.pre_grasp_posture.name)
00863 buff.write(_struct_I.pack(length))
00864 for val1 in self.action_goal.goal.pre_grasp_posture.name:
00865 length = len(val1)
00866 buff.write(struct.pack('<I%ss'%length, length, val1))
00867 length = len(self.action_goal.goal.pre_grasp_posture.position)
00868 buff.write(_struct_I.pack(length))
00869 pattern = '<%sd'%length
00870 buff.write(struct.pack(pattern, *self.action_goal.goal.pre_grasp_posture.position))
00871 length = len(self.action_goal.goal.pre_grasp_posture.velocity)
00872 buff.write(_struct_I.pack(length))
00873 pattern = '<%sd'%length
00874 buff.write(struct.pack(pattern, *self.action_goal.goal.pre_grasp_posture.velocity))
00875 length = len(self.action_goal.goal.pre_grasp_posture.effort)
00876 buff.write(_struct_I.pack(length))
00877 pattern = '<%sd'%length
00878 buff.write(struct.pack(pattern, *self.action_goal.goal.pre_grasp_posture.effort))
00879 _x = self
00880 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp_posture.header.seq, _x.action_goal.goal.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp_posture.header.stamp.nsecs))
00881 _x = self.action_goal.goal.grasp_posture.header.frame_id
00882 length = len(_x)
00883 buff.write(struct.pack('<I%ss'%length, length, _x))
00884 length = len(self.action_goal.goal.grasp_posture.name)
00885 buff.write(_struct_I.pack(length))
00886 for val1 in self.action_goal.goal.grasp_posture.name:
00887 length = len(val1)
00888 buff.write(struct.pack('<I%ss'%length, length, val1))
00889 length = len(self.action_goal.goal.grasp_posture.position)
00890 buff.write(_struct_I.pack(length))
00891 pattern = '<%sd'%length
00892 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp_posture.position))
00893 length = len(self.action_goal.goal.grasp_posture.velocity)
00894 buff.write(_struct_I.pack(length))
00895 pattern = '<%sd'%length
00896 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp_posture.velocity))
00897 length = len(self.action_goal.goal.grasp_posture.effort)
00898 buff.write(_struct_I.pack(length))
00899 pattern = '<%sd'%length
00900 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp_posture.effort))
00901 _x = self
00902 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00903 _x = self.action_result.header.frame_id
00904 length = len(_x)
00905 buff.write(struct.pack('<I%ss'%length, length, _x))
00906 _x = self
00907 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00908 _x = self.action_result.status.goal_id.id
00909 length = len(_x)
00910 buff.write(struct.pack('<I%ss'%length, length, _x))
00911 buff.write(_struct_B.pack(self.action_result.status.status))
00912 _x = self.action_result.status.text
00913 length = len(_x)
00914 buff.write(struct.pack('<I%ss'%length, length, _x))
00915 _x = self
00916 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00917 _x = self.action_feedback.header.frame_id
00918 length = len(_x)
00919 buff.write(struct.pack('<I%ss'%length, length, _x))
00920 _x = self
00921 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00922 _x = self.action_feedback.status.goal_id.id
00923 length = len(_x)
00924 buff.write(struct.pack('<I%ss'%length, length, _x))
00925 buff.write(_struct_B.pack(self.action_feedback.status.status))
00926 _x = self.action_feedback.status.text
00927 length = len(_x)
00928 buff.write(struct.pack('<I%ss'%length, length, _x))
00929 buff.write(_struct_i.pack(self.action_feedback.feedback.manipulation_phase.phase))
00930 except struct.error, se: self._check_types(se)
00931 except TypeError, te: self._check_types(te)
00932
00933 def deserialize(self, str):
00934 """
00935 unpack serialized message in str into this message instance
00936 @param str: byte array of serialized message
00937 @type str: str
00938 """
00939 try:
00940 if self.action_goal is None:
00941 self.action_goal = object_manipulation_msgs.msg.ReactiveGraspActionGoal()
00942 if self.action_result is None:
00943 self.action_result = object_manipulation_msgs.msg.ReactiveGraspActionResult()
00944 if self.action_feedback is None:
00945 self.action_feedback = object_manipulation_msgs.msg.ReactiveGraspActionFeedback()
00946 end = 0
00947 _x = self
00948 start = end
00949 end += 12
00950 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00951 start = end
00952 end += 4
00953 (length,) = _struct_I.unpack(str[start:end])
00954 start = end
00955 end += length
00956 self.action_goal.header.frame_id = str[start:end]
00957 _x = self
00958 start = end
00959 end += 8
00960 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00961 start = end
00962 end += 4
00963 (length,) = _struct_I.unpack(str[start:end])
00964 start = end
00965 end += length
00966 self.action_goal.goal_id.id = str[start:end]
00967 start = end
00968 end += 4
00969 (length,) = _struct_I.unpack(str[start:end])
00970 start = end
00971 end += length
00972 self.action_goal.goal.arm_name = str[start:end]
00973 start = end
00974 end += 4
00975 (length,) = _struct_I.unpack(str[start:end])
00976 start = end
00977 end += length
00978 self.action_goal.goal.target.reference_frame_id = str[start:end]
00979 start = end
00980 end += 4
00981 (length,) = _struct_I.unpack(str[start:end])
00982 self.action_goal.goal.target.potential_models = []
00983 for i in xrange(0, length):
00984 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
00985 start = end
00986 end += 4
00987 (val1.model_id,) = _struct_i.unpack(str[start:end])
00988 _v8 = val1.pose
00989 _v9 = _v8.header
00990 start = end
00991 end += 4
00992 (_v9.seq,) = _struct_I.unpack(str[start:end])
00993 _v10 = _v9.stamp
00994 _x = _v10
00995 start = end
00996 end += 8
00997 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00998 start = end
00999 end += 4
01000 (length,) = _struct_I.unpack(str[start:end])
01001 start = end
01002 end += length
01003 _v9.frame_id = str[start:end]
01004 _v11 = _v8.pose
01005 _v12 = _v11.position
01006 _x = _v12
01007 start = end
01008 end += 24
01009 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01010 _v13 = _v11.orientation
01011 _x = _v13
01012 start = end
01013 end += 32
01014 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01015 start = end
01016 end += 4
01017 (val1.confidence,) = _struct_f.unpack(str[start:end])
01018 self.action_goal.goal.target.potential_models.append(val1)
01019 _x = self
01020 start = end
01021 end += 12
01022 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01023 start = end
01024 end += 4
01025 (length,) = _struct_I.unpack(str[start:end])
01026 start = end
01027 end += length
01028 self.action_goal.goal.target.cluster.header.frame_id = str[start:end]
01029 start = end
01030 end += 4
01031 (length,) = _struct_I.unpack(str[start:end])
01032 self.action_goal.goal.target.cluster.points = []
01033 for i in xrange(0, length):
01034 val1 = geometry_msgs.msg.Point32()
01035 _x = val1
01036 start = end
01037 end += 12
01038 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01039 self.action_goal.goal.target.cluster.points.append(val1)
01040 start = end
01041 end += 4
01042 (length,) = _struct_I.unpack(str[start:end])
01043 self.action_goal.goal.target.cluster.channels = []
01044 for i in xrange(0, length):
01045 val1 = sensor_msgs.msg.ChannelFloat32()
01046 start = end
01047 end += 4
01048 (length,) = _struct_I.unpack(str[start:end])
01049 start = end
01050 end += length
01051 val1.name = str[start:end]
01052 start = end
01053 end += 4
01054 (length,) = _struct_I.unpack(str[start:end])
01055 pattern = '<%sf'%length
01056 start = end
01057 end += struct.calcsize(pattern)
01058 val1.values = struct.unpack(pattern, str[start:end])
01059 self.action_goal.goal.target.cluster.channels.append(val1)
01060 _x = self
01061 start = end
01062 end += 12
01063 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01064 start = end
01065 end += 4
01066 (length,) = _struct_I.unpack(str[start:end])
01067 start = end
01068 end += length
01069 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end]
01070 _x = self
01071 start = end
01072 end += 8
01073 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01074 start = end
01075 end += 4
01076 (length,) = _struct_I.unpack(str[start:end])
01077 self.action_goal.goal.target.region.cloud.fields = []
01078 for i in xrange(0, length):
01079 val1 = sensor_msgs.msg.PointField()
01080 start = end
01081 end += 4
01082 (length,) = _struct_I.unpack(str[start:end])
01083 start = end
01084 end += length
01085 val1.name = str[start:end]
01086 _x = val1
01087 start = end
01088 end += 9
01089 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01090 self.action_goal.goal.target.region.cloud.fields.append(val1)
01091 _x = self
01092 start = end
01093 end += 9
01094 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01095 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian)
01096 start = end
01097 end += 4
01098 (length,) = _struct_I.unpack(str[start:end])
01099 start = end
01100 end += length
01101 self.action_goal.goal.target.region.cloud.data = str[start:end]
01102 start = end
01103 end += 1
01104 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01105 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense)
01106 start = end
01107 end += 4
01108 (length,) = _struct_I.unpack(str[start:end])
01109 pattern = '<%si'%length
01110 start = end
01111 end += struct.calcsize(pattern)
01112 self.action_goal.goal.target.region.mask = struct.unpack(pattern, str[start:end])
01113 _x = self
01114 start = end
01115 end += 12
01116 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01117 start = end
01118 end += 4
01119 (length,) = _struct_I.unpack(str[start:end])
01120 start = end
01121 end += length
01122 self.action_goal.goal.target.region.image.header.frame_id = str[start:end]
01123 _x = self
01124 start = end
01125 end += 8
01126 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01127 start = end
01128 end += 4
01129 (length,) = _struct_I.unpack(str[start:end])
01130 start = end
01131 end += length
01132 self.action_goal.goal.target.region.image.encoding = str[start:end]
01133 _x = self
01134 start = end
01135 end += 5
01136 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01137 start = end
01138 end += 4
01139 (length,) = _struct_I.unpack(str[start:end])
01140 start = end
01141 end += length
01142 self.action_goal.goal.target.region.image.data = str[start:end]
01143 _x = self
01144 start = end
01145 end += 12
01146 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01147 start = end
01148 end += 4
01149 (length,) = _struct_I.unpack(str[start:end])
01150 start = end
01151 end += length
01152 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end]
01153 _x = self
01154 start = end
01155 end += 8
01156 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01157 start = end
01158 end += 4
01159 (length,) = _struct_I.unpack(str[start:end])
01160 start = end
01161 end += length
01162 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end]
01163 _x = self
01164 start = end
01165 end += 5
01166 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01167 start = end
01168 end += 4
01169 (length,) = _struct_I.unpack(str[start:end])
01170 start = end
01171 end += length
01172 self.action_goal.goal.target.region.disparity_image.data = str[start:end]
01173 _x = self
01174 start = end
01175 end += 12
01176 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01177 start = end
01178 end += 4
01179 (length,) = _struct_I.unpack(str[start:end])
01180 start = end
01181 end += length
01182 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end]
01183 _x = self
01184 start = end
01185 end += 8
01186 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01187 start = end
01188 end += 4
01189 (length,) = _struct_I.unpack(str[start:end])
01190 start = end
01191 end += length
01192 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end]
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.target.region.cam_info.D = struct.unpack(pattern, str[start:end])
01200 start = end
01201 end += 72
01202 self.action_goal.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end])
01203 start = end
01204 end += 72
01205 self.action_goal.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end])
01206 start = end
01207 end += 96
01208 self.action_goal.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end])
01209 _x = self
01210 start = end
01211 end += 37
01212 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.final_grasp_pose.header.seq, _x.action_goal.goal.final_grasp_pose.header.stamp.secs, _x.action_goal.goal.final_grasp_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01213 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify)
01214 start = end
01215 end += 4
01216 (length,) = _struct_I.unpack(str[start:end])
01217 start = end
01218 end += length
01219 self.action_goal.goal.final_grasp_pose.header.frame_id = str[start:end]
01220 _x = self
01221 start = end
01222 end += 68
01223 (_x.action_goal.goal.final_grasp_pose.pose.position.x, _x.action_goal.goal.final_grasp_pose.pose.position.y, _x.action_goal.goal.final_grasp_pose.pose.position.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.x, _x.action_goal.goal.final_grasp_pose.pose.orientation.y, _x.action_goal.goal.final_grasp_pose.pose.orientation.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.w, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01224 start = end
01225 end += 4
01226 (length,) = _struct_I.unpack(str[start:end])
01227 start = end
01228 end += length
01229 self.action_goal.goal.trajectory.header.frame_id = str[start:end]
01230 start = end
01231 end += 4
01232 (length,) = _struct_I.unpack(str[start:end])
01233 self.action_goal.goal.trajectory.joint_names = []
01234 for i in xrange(0, length):
01235 start = end
01236 end += 4
01237 (length,) = _struct_I.unpack(str[start:end])
01238 start = end
01239 end += length
01240 val1 = str[start:end]
01241 self.action_goal.goal.trajectory.joint_names.append(val1)
01242 start = end
01243 end += 4
01244 (length,) = _struct_I.unpack(str[start:end])
01245 self.action_goal.goal.trajectory.points = []
01246 for i in xrange(0, length):
01247 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01248 start = end
01249 end += 4
01250 (length,) = _struct_I.unpack(str[start:end])
01251 pattern = '<%sd'%length
01252 start = end
01253 end += struct.calcsize(pattern)
01254 val1.positions = struct.unpack(pattern, str[start:end])
01255 start = end
01256 end += 4
01257 (length,) = _struct_I.unpack(str[start:end])
01258 pattern = '<%sd'%length
01259 start = end
01260 end += struct.calcsize(pattern)
01261 val1.velocities = struct.unpack(pattern, str[start:end])
01262 start = end
01263 end += 4
01264 (length,) = _struct_I.unpack(str[start:end])
01265 pattern = '<%sd'%length
01266 start = end
01267 end += struct.calcsize(pattern)
01268 val1.accelerations = struct.unpack(pattern, str[start:end])
01269 _v14 = val1.time_from_start
01270 _x = _v14
01271 start = end
01272 end += 8
01273 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01274 self.action_goal.goal.trajectory.points.append(val1)
01275 start = end
01276 end += 4
01277 (length,) = _struct_I.unpack(str[start:end])
01278 start = end
01279 end += length
01280 self.action_goal.goal.collision_support_surface_name = str[start:end]
01281 _x = self
01282 start = end
01283 end += 12
01284 (_x.action_goal.goal.pre_grasp_posture.header.seq, _x.action_goal.goal.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01285 start = end
01286 end += 4
01287 (length,) = _struct_I.unpack(str[start:end])
01288 start = end
01289 end += length
01290 self.action_goal.goal.pre_grasp_posture.header.frame_id = str[start:end]
01291 start = end
01292 end += 4
01293 (length,) = _struct_I.unpack(str[start:end])
01294 self.action_goal.goal.pre_grasp_posture.name = []
01295 for i in xrange(0, length):
01296 start = end
01297 end += 4
01298 (length,) = _struct_I.unpack(str[start:end])
01299 start = end
01300 end += length
01301 val1 = str[start:end]
01302 self.action_goal.goal.pre_grasp_posture.name.append(val1)
01303 start = end
01304 end += 4
01305 (length,) = _struct_I.unpack(str[start:end])
01306 pattern = '<%sd'%length
01307 start = end
01308 end += struct.calcsize(pattern)
01309 self.action_goal.goal.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01310 start = end
01311 end += 4
01312 (length,) = _struct_I.unpack(str[start:end])
01313 pattern = '<%sd'%length
01314 start = end
01315 end += struct.calcsize(pattern)
01316 self.action_goal.goal.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01317 start = end
01318 end += 4
01319 (length,) = _struct_I.unpack(str[start:end])
01320 pattern = '<%sd'%length
01321 start = end
01322 end += struct.calcsize(pattern)
01323 self.action_goal.goal.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01324 _x = self
01325 start = end
01326 end += 12
01327 (_x.action_goal.goal.grasp_posture.header.seq, _x.action_goal.goal.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01328 start = end
01329 end += 4
01330 (length,) = _struct_I.unpack(str[start:end])
01331 start = end
01332 end += length
01333 self.action_goal.goal.grasp_posture.header.frame_id = str[start:end]
01334 start = end
01335 end += 4
01336 (length,) = _struct_I.unpack(str[start:end])
01337 self.action_goal.goal.grasp_posture.name = []
01338 for i in xrange(0, length):
01339 start = end
01340 end += 4
01341 (length,) = _struct_I.unpack(str[start:end])
01342 start = end
01343 end += length
01344 val1 = str[start:end]
01345 self.action_goal.goal.grasp_posture.name.append(val1)
01346 start = end
01347 end += 4
01348 (length,) = _struct_I.unpack(str[start:end])
01349 pattern = '<%sd'%length
01350 start = end
01351 end += struct.calcsize(pattern)
01352 self.action_goal.goal.grasp_posture.position = struct.unpack(pattern, str[start:end])
01353 start = end
01354 end += 4
01355 (length,) = _struct_I.unpack(str[start:end])
01356 pattern = '<%sd'%length
01357 start = end
01358 end += struct.calcsize(pattern)
01359 self.action_goal.goal.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01360 start = end
01361 end += 4
01362 (length,) = _struct_I.unpack(str[start:end])
01363 pattern = '<%sd'%length
01364 start = end
01365 end += struct.calcsize(pattern)
01366 self.action_goal.goal.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01367 _x = self
01368 start = end
01369 end += 12
01370 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01371 start = end
01372 end += 4
01373 (length,) = _struct_I.unpack(str[start:end])
01374 start = end
01375 end += length
01376 self.action_result.header.frame_id = str[start:end]
01377 _x = self
01378 start = end
01379 end += 8
01380 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01381 start = end
01382 end += 4
01383 (length,) = _struct_I.unpack(str[start:end])
01384 start = end
01385 end += length
01386 self.action_result.status.goal_id.id = str[start:end]
01387 start = end
01388 end += 1
01389 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01390 start = end
01391 end += 4
01392 (length,) = _struct_I.unpack(str[start:end])
01393 start = end
01394 end += length
01395 self.action_result.status.text = str[start:end]
01396 _x = self
01397 start = end
01398 end += 16
01399 (_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
01400 start = end
01401 end += 4
01402 (length,) = _struct_I.unpack(str[start:end])
01403 start = end
01404 end += length
01405 self.action_feedback.header.frame_id = str[start:end]
01406 _x = self
01407 start = end
01408 end += 8
01409 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01410 start = end
01411 end += 4
01412 (length,) = _struct_I.unpack(str[start:end])
01413 start = end
01414 end += length
01415 self.action_feedback.status.goal_id.id = str[start:end]
01416 start = end
01417 end += 1
01418 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01419 start = end
01420 end += 4
01421 (length,) = _struct_I.unpack(str[start:end])
01422 start = end
01423 end += length
01424 self.action_feedback.status.text = str[start:end]
01425 start = end
01426 end += 4
01427 (self.action_feedback.feedback.manipulation_phase.phase,) = _struct_i.unpack(str[start:end])
01428 return self
01429 except struct.error, e:
01430 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01431
01432
01433 def serialize_numpy(self, buff, numpy):
01434 """
01435 serialize message with numpy array types into buffer
01436 @param buff: buffer
01437 @type buff: StringIO
01438 @param numpy: numpy python module
01439 @type numpy module
01440 """
01441 try:
01442 _x = self
01443 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
01444 _x = self.action_goal.header.frame_id
01445 length = len(_x)
01446 buff.write(struct.pack('<I%ss'%length, length, _x))
01447 _x = self
01448 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
01449 _x = self.action_goal.goal_id.id
01450 length = len(_x)
01451 buff.write(struct.pack('<I%ss'%length, length, _x))
01452 _x = self.action_goal.goal.arm_name
01453 length = len(_x)
01454 buff.write(struct.pack('<I%ss'%length, length, _x))
01455 _x = self.action_goal.goal.target.reference_frame_id
01456 length = len(_x)
01457 buff.write(struct.pack('<I%ss'%length, length, _x))
01458 length = len(self.action_goal.goal.target.potential_models)
01459 buff.write(_struct_I.pack(length))
01460 for val1 in self.action_goal.goal.target.potential_models:
01461 buff.write(_struct_i.pack(val1.model_id))
01462 _v15 = val1.pose
01463 _v16 = _v15.header
01464 buff.write(_struct_I.pack(_v16.seq))
01465 _v17 = _v16.stamp
01466 _x = _v17
01467 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01468 _x = _v16.frame_id
01469 length = len(_x)
01470 buff.write(struct.pack('<I%ss'%length, length, _x))
01471 _v18 = _v15.pose
01472 _v19 = _v18.position
01473 _x = _v19
01474 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01475 _v20 = _v18.orientation
01476 _x = _v20
01477 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01478 buff.write(_struct_f.pack(val1.confidence))
01479 _x = self
01480 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs))
01481 _x = self.action_goal.goal.target.cluster.header.frame_id
01482 length = len(_x)
01483 buff.write(struct.pack('<I%ss'%length, length, _x))
01484 length = len(self.action_goal.goal.target.cluster.points)
01485 buff.write(_struct_I.pack(length))
01486 for val1 in self.action_goal.goal.target.cluster.points:
01487 _x = val1
01488 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01489 length = len(self.action_goal.goal.target.cluster.channels)
01490 buff.write(_struct_I.pack(length))
01491 for val1 in self.action_goal.goal.target.cluster.channels:
01492 _x = val1.name
01493 length = len(_x)
01494 buff.write(struct.pack('<I%ss'%length, length, _x))
01495 length = len(val1.values)
01496 buff.write(_struct_I.pack(length))
01497 pattern = '<%sf'%length
01498 buff.write(val1.values.tostring())
01499 _x = self
01500 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs))
01501 _x = self.action_goal.goal.target.region.cloud.header.frame_id
01502 length = len(_x)
01503 buff.write(struct.pack('<I%ss'%length, length, _x))
01504 _x = self
01505 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width))
01506 length = len(self.action_goal.goal.target.region.cloud.fields)
01507 buff.write(_struct_I.pack(length))
01508 for val1 in self.action_goal.goal.target.region.cloud.fields:
01509 _x = val1.name
01510 length = len(_x)
01511 buff.write(struct.pack('<I%ss'%length, length, _x))
01512 _x = val1
01513 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01514 _x = self
01515 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step))
01516 _x = self.action_goal.goal.target.region.cloud.data
01517 length = len(_x)
01518 # - if encoded as a list instead, serialize as bytes instead of string
01519 if type(_x) in [list, tuple]:
01520 buff.write(struct.pack('<I%sB'%length, length, *_x))
01521 else:
01522 buff.write(struct.pack('<I%ss'%length, length, _x))
01523 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense))
01524 length = len(self.action_goal.goal.target.region.mask)
01525 buff.write(_struct_I.pack(length))
01526 pattern = '<%si'%length
01527 buff.write(self.action_goal.goal.target.region.mask.tostring())
01528 _x = self
01529 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs))
01530 _x = self.action_goal.goal.target.region.image.header.frame_id
01531 length = len(_x)
01532 buff.write(struct.pack('<I%ss'%length, length, _x))
01533 _x = self
01534 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width))
01535 _x = self.action_goal.goal.target.region.image.encoding
01536 length = len(_x)
01537 buff.write(struct.pack('<I%ss'%length, length, _x))
01538 _x = self
01539 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step))
01540 _x = self.action_goal.goal.target.region.image.data
01541 length = len(_x)
01542 # - if encoded as a list instead, serialize as bytes instead of string
01543 if type(_x) in [list, tuple]:
01544 buff.write(struct.pack('<I%sB'%length, length, *_x))
01545 else:
01546 buff.write(struct.pack('<I%ss'%length, length, _x))
01547 _x = self
01548 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs))
01549 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id
01550 length = len(_x)
01551 buff.write(struct.pack('<I%ss'%length, length, _x))
01552 _x = self
01553 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width))
01554 _x = self.action_goal.goal.target.region.disparity_image.encoding
01555 length = len(_x)
01556 buff.write(struct.pack('<I%ss'%length, length, _x))
01557 _x = self
01558 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step))
01559 _x = self.action_goal.goal.target.region.disparity_image.data
01560 length = len(_x)
01561 # - if encoded as a list instead, serialize as bytes instead of string
01562 if type(_x) in [list, tuple]:
01563 buff.write(struct.pack('<I%sB'%length, length, *_x))
01564 else:
01565 buff.write(struct.pack('<I%ss'%length, length, _x))
01566 _x = self
01567 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs))
01568 _x = self.action_goal.goal.target.region.cam_info.header.frame_id
01569 length = len(_x)
01570 buff.write(struct.pack('<I%ss'%length, length, _x))
01571 _x = self
01572 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width))
01573 _x = self.action_goal.goal.target.region.cam_info.distortion_model
01574 length = len(_x)
01575 buff.write(struct.pack('<I%ss'%length, length, _x))
01576 length = len(self.action_goal.goal.target.region.cam_info.D)
01577 buff.write(_struct_I.pack(length))
01578 pattern = '<%sd'%length
01579 buff.write(self.action_goal.goal.target.region.cam_info.D.tostring())
01580 buff.write(self.action_goal.goal.target.region.cam_info.K.tostring())
01581 buff.write(self.action_goal.goal.target.region.cam_info.R.tostring())
01582 buff.write(self.action_goal.goal.target.region.cam_info.P.tostring())
01583 _x = self
01584 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.final_grasp_pose.header.seq, _x.action_goal.goal.final_grasp_pose.header.stamp.secs, _x.action_goal.goal.final_grasp_pose.header.stamp.nsecs))
01585 _x = self.action_goal.goal.final_grasp_pose.header.frame_id
01586 length = len(_x)
01587 buff.write(struct.pack('<I%ss'%length, length, _x))
01588 _x = self
01589 buff.write(_struct_7d3I.pack(_x.action_goal.goal.final_grasp_pose.pose.position.x, _x.action_goal.goal.final_grasp_pose.pose.position.y, _x.action_goal.goal.final_grasp_pose.pose.position.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.x, _x.action_goal.goal.final_grasp_pose.pose.orientation.y, _x.action_goal.goal.final_grasp_pose.pose.orientation.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.w, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs))
01590 _x = self.action_goal.goal.trajectory.header.frame_id
01591 length = len(_x)
01592 buff.write(struct.pack('<I%ss'%length, length, _x))
01593 length = len(self.action_goal.goal.trajectory.joint_names)
01594 buff.write(_struct_I.pack(length))
01595 for val1 in self.action_goal.goal.trajectory.joint_names:
01596 length = len(val1)
01597 buff.write(struct.pack('<I%ss'%length, length, val1))
01598 length = len(self.action_goal.goal.trajectory.points)
01599 buff.write(_struct_I.pack(length))
01600 for val1 in self.action_goal.goal.trajectory.points:
01601 length = len(val1.positions)
01602 buff.write(_struct_I.pack(length))
01603 pattern = '<%sd'%length
01604 buff.write(val1.positions.tostring())
01605 length = len(val1.velocities)
01606 buff.write(_struct_I.pack(length))
01607 pattern = '<%sd'%length
01608 buff.write(val1.velocities.tostring())
01609 length = len(val1.accelerations)
01610 buff.write(_struct_I.pack(length))
01611 pattern = '<%sd'%length
01612 buff.write(val1.accelerations.tostring())
01613 _v21 = val1.time_from_start
01614 _x = _v21
01615 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01616 _x = self.action_goal.goal.collision_support_surface_name
01617 length = len(_x)
01618 buff.write(struct.pack('<I%ss'%length, length, _x))
01619 _x = self
01620 buff.write(_struct_3I.pack(_x.action_goal.goal.pre_grasp_posture.header.seq, _x.action_goal.goal.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.pre_grasp_posture.header.stamp.nsecs))
01621 _x = self.action_goal.goal.pre_grasp_posture.header.frame_id
01622 length = len(_x)
01623 buff.write(struct.pack('<I%ss'%length, length, _x))
01624 length = len(self.action_goal.goal.pre_grasp_posture.name)
01625 buff.write(_struct_I.pack(length))
01626 for val1 in self.action_goal.goal.pre_grasp_posture.name:
01627 length = len(val1)
01628 buff.write(struct.pack('<I%ss'%length, length, val1))
01629 length = len(self.action_goal.goal.pre_grasp_posture.position)
01630 buff.write(_struct_I.pack(length))
01631 pattern = '<%sd'%length
01632 buff.write(self.action_goal.goal.pre_grasp_posture.position.tostring())
01633 length = len(self.action_goal.goal.pre_grasp_posture.velocity)
01634 buff.write(_struct_I.pack(length))
01635 pattern = '<%sd'%length
01636 buff.write(self.action_goal.goal.pre_grasp_posture.velocity.tostring())
01637 length = len(self.action_goal.goal.pre_grasp_posture.effort)
01638 buff.write(_struct_I.pack(length))
01639 pattern = '<%sd'%length
01640 buff.write(self.action_goal.goal.pre_grasp_posture.effort.tostring())
01641 _x = self
01642 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp_posture.header.seq, _x.action_goal.goal.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp_posture.header.stamp.nsecs))
01643 _x = self.action_goal.goal.grasp_posture.header.frame_id
01644 length = len(_x)
01645 buff.write(struct.pack('<I%ss'%length, length, _x))
01646 length = len(self.action_goal.goal.grasp_posture.name)
01647 buff.write(_struct_I.pack(length))
01648 for val1 in self.action_goal.goal.grasp_posture.name:
01649 length = len(val1)
01650 buff.write(struct.pack('<I%ss'%length, length, val1))
01651 length = len(self.action_goal.goal.grasp_posture.position)
01652 buff.write(_struct_I.pack(length))
01653 pattern = '<%sd'%length
01654 buff.write(self.action_goal.goal.grasp_posture.position.tostring())
01655 length = len(self.action_goal.goal.grasp_posture.velocity)
01656 buff.write(_struct_I.pack(length))
01657 pattern = '<%sd'%length
01658 buff.write(self.action_goal.goal.grasp_posture.velocity.tostring())
01659 length = len(self.action_goal.goal.grasp_posture.effort)
01660 buff.write(_struct_I.pack(length))
01661 pattern = '<%sd'%length
01662 buff.write(self.action_goal.goal.grasp_posture.effort.tostring())
01663 _x = self
01664 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
01665 _x = self.action_result.header.frame_id
01666 length = len(_x)
01667 buff.write(struct.pack('<I%ss'%length, length, _x))
01668 _x = self
01669 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
01670 _x = self.action_result.status.goal_id.id
01671 length = len(_x)
01672 buff.write(struct.pack('<I%ss'%length, length, _x))
01673 buff.write(_struct_B.pack(self.action_result.status.status))
01674 _x = self.action_result.status.text
01675 length = len(_x)
01676 buff.write(struct.pack('<I%ss'%length, length, _x))
01677 _x = self
01678 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
01679 _x = self.action_feedback.header.frame_id
01680 length = len(_x)
01681 buff.write(struct.pack('<I%ss'%length, length, _x))
01682 _x = self
01683 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
01684 _x = self.action_feedback.status.goal_id.id
01685 length = len(_x)
01686 buff.write(struct.pack('<I%ss'%length, length, _x))
01687 buff.write(_struct_B.pack(self.action_feedback.status.status))
01688 _x = self.action_feedback.status.text
01689 length = len(_x)
01690 buff.write(struct.pack('<I%ss'%length, length, _x))
01691 buff.write(_struct_i.pack(self.action_feedback.feedback.manipulation_phase.phase))
01692 except struct.error, se: self._check_types(se)
01693 except TypeError, te: self._check_types(te)
01694
01695 def deserialize_numpy(self, str, numpy):
01696 """
01697 unpack serialized message in str into this message instance using numpy for array types
01698 @param str: byte array of serialized message
01699 @type str: str
01700 @param numpy: numpy python module
01701 @type numpy: module
01702 """
01703 try:
01704 if self.action_goal is None:
01705 self.action_goal = object_manipulation_msgs.msg.ReactiveGraspActionGoal()
01706 if self.action_result is None:
01707 self.action_result = object_manipulation_msgs.msg.ReactiveGraspActionResult()
01708 if self.action_feedback is None:
01709 self.action_feedback = object_manipulation_msgs.msg.ReactiveGraspActionFeedback()
01710 end = 0
01711 _x = self
01712 start = end
01713 end += 12
01714 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01715 start = end
01716 end += 4
01717 (length,) = _struct_I.unpack(str[start:end])
01718 start = end
01719 end += length
01720 self.action_goal.header.frame_id = str[start:end]
01721 _x = self
01722 start = end
01723 end += 8
01724 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01725 start = end
01726 end += 4
01727 (length,) = _struct_I.unpack(str[start:end])
01728 start = end
01729 end += length
01730 self.action_goal.goal_id.id = str[start:end]
01731 start = end
01732 end += 4
01733 (length,) = _struct_I.unpack(str[start:end])
01734 start = end
01735 end += length
01736 self.action_goal.goal.arm_name = str[start:end]
01737 start = end
01738 end += 4
01739 (length,) = _struct_I.unpack(str[start:end])
01740 start = end
01741 end += length
01742 self.action_goal.goal.target.reference_frame_id = str[start:end]
01743 start = end
01744 end += 4
01745 (length,) = _struct_I.unpack(str[start:end])
01746 self.action_goal.goal.target.potential_models = []
01747 for i in xrange(0, length):
01748 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01749 start = end
01750 end += 4
01751 (val1.model_id,) = _struct_i.unpack(str[start:end])
01752 _v22 = val1.pose
01753 _v23 = _v22.header
01754 start = end
01755 end += 4
01756 (_v23.seq,) = _struct_I.unpack(str[start:end])
01757 _v24 = _v23.stamp
01758 _x = _v24
01759 start = end
01760 end += 8
01761 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01762 start = end
01763 end += 4
01764 (length,) = _struct_I.unpack(str[start:end])
01765 start = end
01766 end += length
01767 _v23.frame_id = str[start:end]
01768 _v25 = _v22.pose
01769 _v26 = _v25.position
01770 _x = _v26
01771 start = end
01772 end += 24
01773 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01774 _v27 = _v25.orientation
01775 _x = _v27
01776 start = end
01777 end += 32
01778 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01779 start = end
01780 end += 4
01781 (val1.confidence,) = _struct_f.unpack(str[start:end])
01782 self.action_goal.goal.target.potential_models.append(val1)
01783 _x = self
01784 start = end
01785 end += 12
01786 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01787 start = end
01788 end += 4
01789 (length,) = _struct_I.unpack(str[start:end])
01790 start = end
01791 end += length
01792 self.action_goal.goal.target.cluster.header.frame_id = str[start:end]
01793 start = end
01794 end += 4
01795 (length,) = _struct_I.unpack(str[start:end])
01796 self.action_goal.goal.target.cluster.points = []
01797 for i in xrange(0, length):
01798 val1 = geometry_msgs.msg.Point32()
01799 _x = val1
01800 start = end
01801 end += 12
01802 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01803 self.action_goal.goal.target.cluster.points.append(val1)
01804 start = end
01805 end += 4
01806 (length,) = _struct_I.unpack(str[start:end])
01807 self.action_goal.goal.target.cluster.channels = []
01808 for i in xrange(0, length):
01809 val1 = sensor_msgs.msg.ChannelFloat32()
01810 start = end
01811 end += 4
01812 (length,) = _struct_I.unpack(str[start:end])
01813 start = end
01814 end += length
01815 val1.name = str[start:end]
01816 start = end
01817 end += 4
01818 (length,) = _struct_I.unpack(str[start:end])
01819 pattern = '<%sf'%length
01820 start = end
01821 end += struct.calcsize(pattern)
01822 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01823 self.action_goal.goal.target.cluster.channels.append(val1)
01824 _x = self
01825 start = end
01826 end += 12
01827 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01828 start = end
01829 end += 4
01830 (length,) = _struct_I.unpack(str[start:end])
01831 start = end
01832 end += length
01833 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end]
01834 _x = self
01835 start = end
01836 end += 8
01837 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01838 start = end
01839 end += 4
01840 (length,) = _struct_I.unpack(str[start:end])
01841 self.action_goal.goal.target.region.cloud.fields = []
01842 for i in xrange(0, length):
01843 val1 = sensor_msgs.msg.PointField()
01844 start = end
01845 end += 4
01846 (length,) = _struct_I.unpack(str[start:end])
01847 start = end
01848 end += length
01849 val1.name = str[start:end]
01850 _x = val1
01851 start = end
01852 end += 9
01853 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01854 self.action_goal.goal.target.region.cloud.fields.append(val1)
01855 _x = self
01856 start = end
01857 end += 9
01858 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01859 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian)
01860 start = end
01861 end += 4
01862 (length,) = _struct_I.unpack(str[start:end])
01863 start = end
01864 end += length
01865 self.action_goal.goal.target.region.cloud.data = str[start:end]
01866 start = end
01867 end += 1
01868 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01869 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense)
01870 start = end
01871 end += 4
01872 (length,) = _struct_I.unpack(str[start:end])
01873 pattern = '<%si'%length
01874 start = end
01875 end += struct.calcsize(pattern)
01876 self.action_goal.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01877 _x = self
01878 start = end
01879 end += 12
01880 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01881 start = end
01882 end += 4
01883 (length,) = _struct_I.unpack(str[start:end])
01884 start = end
01885 end += length
01886 self.action_goal.goal.target.region.image.header.frame_id = str[start:end]
01887 _x = self
01888 start = end
01889 end += 8
01890 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01891 start = end
01892 end += 4
01893 (length,) = _struct_I.unpack(str[start:end])
01894 start = end
01895 end += length
01896 self.action_goal.goal.target.region.image.encoding = str[start:end]
01897 _x = self
01898 start = end
01899 end += 5
01900 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01901 start = end
01902 end += 4
01903 (length,) = _struct_I.unpack(str[start:end])
01904 start = end
01905 end += length
01906 self.action_goal.goal.target.region.image.data = str[start:end]
01907 _x = self
01908 start = end
01909 end += 12
01910 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01911 start = end
01912 end += 4
01913 (length,) = _struct_I.unpack(str[start:end])
01914 start = end
01915 end += length
01916 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end]
01917 _x = self
01918 start = end
01919 end += 8
01920 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01921 start = end
01922 end += 4
01923 (length,) = _struct_I.unpack(str[start:end])
01924 start = end
01925 end += length
01926 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end]
01927 _x = self
01928 start = end
01929 end += 5
01930 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01931 start = end
01932 end += 4
01933 (length,) = _struct_I.unpack(str[start:end])
01934 start = end
01935 end += length
01936 self.action_goal.goal.target.region.disparity_image.data = str[start:end]
01937 _x = self
01938 start = end
01939 end += 12
01940 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01941 start = end
01942 end += 4
01943 (length,) = _struct_I.unpack(str[start:end])
01944 start = end
01945 end += length
01946 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end]
01947 _x = self
01948 start = end
01949 end += 8
01950 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01951 start = end
01952 end += 4
01953 (length,) = _struct_I.unpack(str[start:end])
01954 start = end
01955 end += length
01956 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end]
01957 start = end
01958 end += 4
01959 (length,) = _struct_I.unpack(str[start:end])
01960 pattern = '<%sd'%length
01961 start = end
01962 end += struct.calcsize(pattern)
01963 self.action_goal.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01964 start = end
01965 end += 72
01966 self.action_goal.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01967 start = end
01968 end += 72
01969 self.action_goal.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01970 start = end
01971 end += 96
01972 self.action_goal.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
01973 _x = self
01974 start = end
01975 end += 37
01976 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.final_grasp_pose.header.seq, _x.action_goal.goal.final_grasp_pose.header.stamp.secs, _x.action_goal.goal.final_grasp_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01977 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify)
01978 start = end
01979 end += 4
01980 (length,) = _struct_I.unpack(str[start:end])
01981 start = end
01982 end += length
01983 self.action_goal.goal.final_grasp_pose.header.frame_id = str[start:end]
01984 _x = self
01985 start = end
01986 end += 68
01987 (_x.action_goal.goal.final_grasp_pose.pose.position.x, _x.action_goal.goal.final_grasp_pose.pose.position.y, _x.action_goal.goal.final_grasp_pose.pose.position.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.x, _x.action_goal.goal.final_grasp_pose.pose.orientation.y, _x.action_goal.goal.final_grasp_pose.pose.orientation.z, _x.action_goal.goal.final_grasp_pose.pose.orientation.w, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01988 start = end
01989 end += 4
01990 (length,) = _struct_I.unpack(str[start:end])
01991 start = end
01992 end += length
01993 self.action_goal.goal.trajectory.header.frame_id = str[start:end]
01994 start = end
01995 end += 4
01996 (length,) = _struct_I.unpack(str[start:end])
01997 self.action_goal.goal.trajectory.joint_names = []
01998 for i in xrange(0, length):
01999 start = end
02000 end += 4
02001 (length,) = _struct_I.unpack(str[start:end])
02002 start = end
02003 end += length
02004 val1 = str[start:end]
02005 self.action_goal.goal.trajectory.joint_names.append(val1)
02006 start = end
02007 end += 4
02008 (length,) = _struct_I.unpack(str[start:end])
02009 self.action_goal.goal.trajectory.points = []
02010 for i in xrange(0, length):
02011 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
02012 start = end
02013 end += 4
02014 (length,) = _struct_I.unpack(str[start:end])
02015 pattern = '<%sd'%length
02016 start = end
02017 end += struct.calcsize(pattern)
02018 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02019 start = end
02020 end += 4
02021 (length,) = _struct_I.unpack(str[start:end])
02022 pattern = '<%sd'%length
02023 start = end
02024 end += struct.calcsize(pattern)
02025 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02026 start = end
02027 end += 4
02028 (length,) = _struct_I.unpack(str[start:end])
02029 pattern = '<%sd'%length
02030 start = end
02031 end += struct.calcsize(pattern)
02032 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02033 _v28 = val1.time_from_start
02034 _x = _v28
02035 start = end
02036 end += 8
02037 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
02038 self.action_goal.goal.trajectory.points.append(val1)
02039 start = end
02040 end += 4
02041 (length,) = _struct_I.unpack(str[start:end])
02042 start = end
02043 end += length
02044 self.action_goal.goal.collision_support_surface_name = str[start:end]
02045 _x = self
02046 start = end
02047 end += 12
02048 (_x.action_goal.goal.pre_grasp_posture.header.seq, _x.action_goal.goal.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02049 start = end
02050 end += 4
02051 (length,) = _struct_I.unpack(str[start:end])
02052 start = end
02053 end += length
02054 self.action_goal.goal.pre_grasp_posture.header.frame_id = str[start:end]
02055 start = end
02056 end += 4
02057 (length,) = _struct_I.unpack(str[start:end])
02058 self.action_goal.goal.pre_grasp_posture.name = []
02059 for i in xrange(0, length):
02060 start = end
02061 end += 4
02062 (length,) = _struct_I.unpack(str[start:end])
02063 start = end
02064 end += length
02065 val1 = str[start:end]
02066 self.action_goal.goal.pre_grasp_posture.name.append(val1)
02067 start = end
02068 end += 4
02069 (length,) = _struct_I.unpack(str[start:end])
02070 pattern = '<%sd'%length
02071 start = end
02072 end += struct.calcsize(pattern)
02073 self.action_goal.goal.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02074 start = end
02075 end += 4
02076 (length,) = _struct_I.unpack(str[start:end])
02077 pattern = '<%sd'%length
02078 start = end
02079 end += struct.calcsize(pattern)
02080 self.action_goal.goal.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02081 start = end
02082 end += 4
02083 (length,) = _struct_I.unpack(str[start:end])
02084 pattern = '<%sd'%length
02085 start = end
02086 end += struct.calcsize(pattern)
02087 self.action_goal.goal.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02088 _x = self
02089 start = end
02090 end += 12
02091 (_x.action_goal.goal.grasp_posture.header.seq, _x.action_goal.goal.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02092 start = end
02093 end += 4
02094 (length,) = _struct_I.unpack(str[start:end])
02095 start = end
02096 end += length
02097 self.action_goal.goal.grasp_posture.header.frame_id = str[start:end]
02098 start = end
02099 end += 4
02100 (length,) = _struct_I.unpack(str[start:end])
02101 self.action_goal.goal.grasp_posture.name = []
02102 for i in xrange(0, length):
02103 start = end
02104 end += 4
02105 (length,) = _struct_I.unpack(str[start:end])
02106 start = end
02107 end += length
02108 val1 = str[start:end]
02109 self.action_goal.goal.grasp_posture.name.append(val1)
02110 start = end
02111 end += 4
02112 (length,) = _struct_I.unpack(str[start:end])
02113 pattern = '<%sd'%length
02114 start = end
02115 end += struct.calcsize(pattern)
02116 self.action_goal.goal.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02117 start = end
02118 end += 4
02119 (length,) = _struct_I.unpack(str[start:end])
02120 pattern = '<%sd'%length
02121 start = end
02122 end += struct.calcsize(pattern)
02123 self.action_goal.goal.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02124 start = end
02125 end += 4
02126 (length,) = _struct_I.unpack(str[start:end])
02127 pattern = '<%sd'%length
02128 start = end
02129 end += struct.calcsize(pattern)
02130 self.action_goal.goal.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02131 _x = self
02132 start = end
02133 end += 12
02134 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02135 start = end
02136 end += 4
02137 (length,) = _struct_I.unpack(str[start:end])
02138 start = end
02139 end += length
02140 self.action_result.header.frame_id = str[start:end]
02141 _x = self
02142 start = end
02143 end += 8
02144 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02145 start = end
02146 end += 4
02147 (length,) = _struct_I.unpack(str[start:end])
02148 start = end
02149 end += length
02150 self.action_result.status.goal_id.id = str[start:end]
02151 start = end
02152 end += 1
02153 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
02154 start = end
02155 end += 4
02156 (length,) = _struct_I.unpack(str[start:end])
02157 start = end
02158 end += length
02159 self.action_result.status.text = str[start:end]
02160 _x = self
02161 start = end
02162 end += 16
02163 (_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
02164 start = end
02165 end += 4
02166 (length,) = _struct_I.unpack(str[start:end])
02167 start = end
02168 end += length
02169 self.action_feedback.header.frame_id = str[start:end]
02170 _x = self
02171 start = end
02172 end += 8
02173 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.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 self.action_feedback.status.goal_id.id = str[start:end]
02180 start = end
02181 end += 1
02182 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
02183 start = end
02184 end += 4
02185 (length,) = _struct_I.unpack(str[start:end])
02186 start = end
02187 end += length
02188 self.action_feedback.status.text = str[start:end]
02189 start = end
02190 end += 4
02191 (self.action_feedback.feedback.manipulation_phase.phase,) = _struct_i.unpack(str[start:end])
02192 return self
02193 except struct.error, e:
02194 raise roslib.message.DeserializationError(e) #most likely buffer underfill
02195
02196 _struct_I = roslib.message.struct_I
02197 _struct_7d3I = struct.Struct("<7d3I")
02198 _struct_IBI = struct.Struct("<IBI")
02199 _struct_6IB3I = struct.Struct("<6IB3I")
02200 _struct_B = struct.Struct("<B")
02201 _struct_12d = struct.Struct("<12d")
02202 _struct_f = struct.Struct("<f")
02203 _struct_i = struct.Struct("<i")
02204 _struct_BI = struct.Struct("<BI")
02205 _struct_3f = struct.Struct("<3f")
02206 _struct_2i = struct.Struct("<2i")
02207 _struct_3I = struct.Struct("<3I")
02208 _struct_9d = struct.Struct("<9d")
02209 _struct_B2I = struct.Struct("<B2I")
02210 _struct_i3I = struct.Struct("<i3I")
02211 _struct_4d = struct.Struct("<4d")
02212 _struct_2I = struct.Struct("<2I")
02213 _struct_3d = struct.Struct("<3d")