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