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