00001 """autogenerated by genpy from pr2_object_manipulation_msgs/IMGUIGoal.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 pr2_object_manipulation_msgs.msg
00008 import manipulation_msgs.msg
00009 import geometry_msgs.msg
00010 import object_recognition_msgs.msg
00011 import sensor_msgs.msg
00012 import household_objects_database_msgs.msg
00013 import std_msgs.msg
00014
00015 class IMGUIGoal(genpy.Message):
00016 _md5sum = "0e5b7f8584a8f04fd13e1dbb3c14d7c5"
00017 _type = "pr2_object_manipulation_msgs/IMGUIGoal"
00018 _has_header = False
00019 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00020
00021 IMGUIOptions options
00022 IMGUICommand command
00023
00024
00025 ================================================================================
00026 MSG: pr2_object_manipulation_msgs/IMGUIOptions
00027
00028 # collision checking enabled
00029 bool collision_checked
00030
00031 # 0=call gripper click
00032 # 1=grasp the provided graspable object
00033 int32 grasp_selection
00034
00035 # 0=right, 1=left arm
00036 int32 arm_selection
00037
00038 # for RESET commands
00039 # 0=reset collision objects
00040 # 1=reset attached objects
00041 int32 reset_choice
00042
00043 # for MOVE_ARM commands
00044 # 0=side
00045 # 1=front
00046 # 2=side handoff
00047 int32 arm_action_choice
00048
00049 # for MOVE_ARM commands
00050 # 0=open-loop
00051 # 1=with planner
00052 int32 arm_planner_choice
00053
00054 # for MOVE_GRIPPER commands
00055 # opening of gripper (0=closed..100=open)
00056 int32 gripper_slider_position
00057
00058 # used if grasp_selection == 1
00059 manipulation_msgs/GraspableObject selected_object
00060
00061 # indicates obstacles that can be moved during grasping
00062 # presumably, the operator has marked these in some fashion
00063 manipulation_msgs/GraspableObject[] movable_obstacles
00064
00065 # more options..
00066 IMGUIAdvancedOptions adv_options
00067
00068 ================================================================================
00069 MSG: manipulation_msgs/GraspableObject
00070 # an object that the object_manipulator can work on
00071
00072 # a graspable object can be represented in multiple ways. This message
00073 # can contain all of them. Which one is actually used is up to the receiver
00074 # of this message. When adding new representations, one must be careful that
00075 # they have reasonable lightweight defaults indicating that that particular
00076 # representation is not available.
00077
00078 # the tf frame to be used as a reference frame when combining information from
00079 # the different representations below
00080 string reference_frame_id
00081
00082 # potential recognition results from a database of models
00083 # all poses are relative to the object reference pose
00084 household_objects_database_msgs/DatabaseModelPose[] potential_models
00085
00086 # the point cloud itself
00087 sensor_msgs/PointCloud cluster
00088
00089 # a region of a PointCloud2 of interest
00090 SceneRegion region
00091
00092 # the name that this object has in the collision environment
00093 string collision_name
00094 ================================================================================
00095 MSG: household_objects_database_msgs/DatabaseModelPose
00096 # Informs that a specific model from the Model Database has been
00097 # identified at a certain location
00098
00099 # the database id of the model
00100 int32 model_id
00101
00102 # if the object was recognized by the ORK pipeline, its type will be in here
00103 # if this is not empty, then the string in here will be converted to a household_objects_database id
00104 # leave this empty if providing an id in the model_id field
00105 object_recognition_msgs/ObjectType type
00106
00107 # the pose that it can be found in
00108 geometry_msgs/PoseStamped pose
00109
00110 # a measure of the confidence level in this detection result
00111 float32 confidence
00112
00113 # the name of the object detector that generated this detection result
00114 string detector_name
00115
00116 ================================================================================
00117 MSG: object_recognition_msgs/ObjectType
00118 ################################################## OBJECT ID #########################################################
00119
00120 # Contains information about the type of a found object. Those two sets of parameters together uniquely define an
00121 # object
00122
00123 # The key of the found object: the unique identifier in the given db
00124 string key
00125
00126 # The db parameters stored as a JSON/compressed YAML string. An object id does not make sense without the corresponding
00127 # database. E.g., in object_recognition, it can look like: "{'type':'CouchDB', 'root':'http://localhost'}"
00128 # There is no conventional format for those parameters and it's nice to keep that flexibility.
00129 # The object_recognition_core as a generic DB type that can read those fields
00130 # Current examples:
00131 # For CouchDB:
00132 # type: 'CouchDB'
00133 # root: 'http://localhost:5984'
00134 # collection: 'object_recognition'
00135 # For SQL household database:
00136 # type: 'SqlHousehold'
00137 # host: 'wgs36'
00138 # port: 5432
00139 # user: 'willow'
00140 # password: 'willow'
00141 # name: 'household_objects'
00142 # module: 'tabletop'
00143 string db
00144
00145 ================================================================================
00146 MSG: geometry_msgs/PoseStamped
00147 # A Pose with reference coordinate frame and timestamp
00148 Header header
00149 Pose pose
00150
00151 ================================================================================
00152 MSG: std_msgs/Header
00153 # Standard metadata for higher-level stamped data types.
00154 # This is generally used to communicate timestamped data
00155 # in a particular coordinate frame.
00156 #
00157 # sequence ID: consecutively increasing ID
00158 uint32 seq
00159 #Two-integer timestamp that is expressed as:
00160 # * stamp.secs: seconds (stamp_secs) since epoch
00161 # * stamp.nsecs: nanoseconds since stamp_secs
00162 # time-handling sugar is provided by the client library
00163 time stamp
00164 #Frame this data is associated with
00165 # 0: no frame
00166 # 1: global frame
00167 string frame_id
00168
00169 ================================================================================
00170 MSG: geometry_msgs/Pose
00171 # A representation of pose in free space, composed of postion and orientation.
00172 Point position
00173 Quaternion orientation
00174
00175 ================================================================================
00176 MSG: geometry_msgs/Point
00177 # This contains the position of a point in free space
00178 float64 x
00179 float64 y
00180 float64 z
00181
00182 ================================================================================
00183 MSG: geometry_msgs/Quaternion
00184 # This represents an orientation in free space in quaternion form.
00185
00186 float64 x
00187 float64 y
00188 float64 z
00189 float64 w
00190
00191 ================================================================================
00192 MSG: sensor_msgs/PointCloud
00193 # This message holds a collection of 3d points, plus optional additional
00194 # information about each point.
00195
00196 # Time of sensor data acquisition, coordinate frame ID.
00197 Header header
00198
00199 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00200 # in the frame given in the header.
00201 geometry_msgs/Point32[] points
00202
00203 # Each channel should have the same number of elements as points array,
00204 # and the data in each channel should correspond 1:1 with each point.
00205 # Channel names in common practice are listed in ChannelFloat32.msg.
00206 ChannelFloat32[] channels
00207
00208 ================================================================================
00209 MSG: geometry_msgs/Point32
00210 # This contains the position of a point in free space(with 32 bits of precision).
00211 # It is recommeded to use Point wherever possible instead of Point32.
00212 #
00213 # This recommendation is to promote interoperability.
00214 #
00215 # This message is designed to take up less space when sending
00216 # lots of points at once, as in the case of a PointCloud.
00217
00218 float32 x
00219 float32 y
00220 float32 z
00221 ================================================================================
00222 MSG: sensor_msgs/ChannelFloat32
00223 # This message is used by the PointCloud message to hold optional data
00224 # associated with each point in the cloud. The length of the values
00225 # array should be the same as the length of the points array in the
00226 # PointCloud, and each value should be associated with the corresponding
00227 # point.
00228
00229 # Channel names in existing practice include:
00230 # "u", "v" - row and column (respectively) in the left stereo image.
00231 # This is opposite to usual conventions but remains for
00232 # historical reasons. The newer PointCloud2 message has no
00233 # such problem.
00234 # "rgb" - For point clouds produced by color stereo cameras. uint8
00235 # (R,G,B) values packed into the least significant 24 bits,
00236 # in order.
00237 # "intensity" - laser or pixel intensity.
00238 # "distance"
00239
00240 # The channel name should give semantics of the channel (e.g.
00241 # "intensity" instead of "value").
00242 string name
00243
00244 # The values array should be 1-1 with the elements of the associated
00245 # PointCloud.
00246 float32[] values
00247
00248 ================================================================================
00249 MSG: manipulation_msgs/SceneRegion
00250 # Point cloud
00251 sensor_msgs/PointCloud2 cloud
00252
00253 # Indices for the region of interest
00254 int32[] mask
00255
00256 # One of the corresponding 2D images, if applicable
00257 sensor_msgs/Image image
00258
00259 # The disparity image, if applicable
00260 sensor_msgs/Image disparity_image
00261
00262 # Camera info for the camera that took the image
00263 sensor_msgs/CameraInfo cam_info
00264
00265 # a 3D region of interest for grasp planning
00266 geometry_msgs/PoseStamped roi_box_pose
00267 geometry_msgs/Vector3 roi_box_dims
00268
00269 ================================================================================
00270 MSG: sensor_msgs/PointCloud2
00271 # This message holds a collection of N-dimensional points, which may
00272 # contain additional information such as normals, intensity, etc. The
00273 # point data is stored as a binary blob, its layout described by the
00274 # contents of the "fields" array.
00275
00276 # The point cloud data may be organized 2d (image-like) or 1d
00277 # (unordered). Point clouds organized as 2d images may be produced by
00278 # camera depth sensors such as stereo or time-of-flight.
00279
00280 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00281 # points).
00282 Header header
00283
00284 # 2D structure of the point cloud. If the cloud is unordered, height is
00285 # 1 and width is the length of the point cloud.
00286 uint32 height
00287 uint32 width
00288
00289 # Describes the channels and their layout in the binary data blob.
00290 PointField[] fields
00291
00292 bool is_bigendian # Is this data bigendian?
00293 uint32 point_step # Length of a point in bytes
00294 uint32 row_step # Length of a row in bytes
00295 uint8[] data # Actual point data, size is (row_step*height)
00296
00297 bool is_dense # True if there are no invalid points
00298
00299 ================================================================================
00300 MSG: sensor_msgs/PointField
00301 # This message holds the description of one point entry in the
00302 # PointCloud2 message format.
00303 uint8 INT8 = 1
00304 uint8 UINT8 = 2
00305 uint8 INT16 = 3
00306 uint8 UINT16 = 4
00307 uint8 INT32 = 5
00308 uint8 UINT32 = 6
00309 uint8 FLOAT32 = 7
00310 uint8 FLOAT64 = 8
00311
00312 string name # Name of field
00313 uint32 offset # Offset from start of point struct
00314 uint8 datatype # Datatype enumeration, see above
00315 uint32 count # How many elements in the field
00316
00317 ================================================================================
00318 MSG: sensor_msgs/Image
00319 # This message contains an uncompressed image
00320 # (0, 0) is at top-left corner of image
00321 #
00322
00323 Header header # Header timestamp should be acquisition time of image
00324 # Header frame_id should be optical frame of camera
00325 # origin of frame should be optical center of cameara
00326 # +x should point to the right in the image
00327 # +y should point down in the image
00328 # +z should point into to plane of the image
00329 # If the frame_id here and the frame_id of the CameraInfo
00330 # message associated with the image conflict
00331 # the behavior is undefined
00332
00333 uint32 height # image height, that is, number of rows
00334 uint32 width # image width, that is, number of columns
00335
00336 # The legal values for encoding are in file src/image_encodings.cpp
00337 # If you want to standardize a new string format, join
00338 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00339
00340 string encoding # Encoding of pixels -- channel meaning, ordering, size
00341 # taken from the list of strings in include/sensor_msgs/image_encodings.h
00342
00343 uint8 is_bigendian # is this data bigendian?
00344 uint32 step # Full row length in bytes
00345 uint8[] data # actual matrix data, size is (step * rows)
00346
00347 ================================================================================
00348 MSG: sensor_msgs/CameraInfo
00349 # This message defines meta information for a camera. It should be in a
00350 # camera namespace on topic "camera_info" and accompanied by up to five
00351 # image topics named:
00352 #
00353 # image_raw - raw data from the camera driver, possibly Bayer encoded
00354 # image - monochrome, distorted
00355 # image_color - color, distorted
00356 # image_rect - monochrome, rectified
00357 # image_rect_color - color, rectified
00358 #
00359 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00360 # for producing the four processed image topics from image_raw and
00361 # camera_info. The meaning of the camera parameters are described in
00362 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00363 #
00364 # The image_geometry package provides a user-friendly interface to
00365 # common operations using this meta information. If you want to, e.g.,
00366 # project a 3d point into image coordinates, we strongly recommend
00367 # using image_geometry.
00368 #
00369 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00370 # zeroed out. In particular, clients may assume that K[0] == 0.0
00371 # indicates an uncalibrated camera.
00372
00373 #######################################################################
00374 # Image acquisition info #
00375 #######################################################################
00376
00377 # Time of image acquisition, camera coordinate frame ID
00378 Header header # Header timestamp should be acquisition time of image
00379 # Header frame_id should be optical frame of camera
00380 # origin of frame should be optical center of camera
00381 # +x should point to the right in the image
00382 # +y should point down in the image
00383 # +z should point into the plane of the image
00384
00385
00386 #######################################################################
00387 # Calibration Parameters #
00388 #######################################################################
00389 # These are fixed during camera calibration. Their values will be the #
00390 # same in all messages until the camera is recalibrated. Note that #
00391 # self-calibrating systems may "recalibrate" frequently. #
00392 # #
00393 # The internal parameters can be used to warp a raw (distorted) image #
00394 # to: #
00395 # 1. An undistorted image (requires D and K) #
00396 # 2. A rectified image (requires D, K, R) #
00397 # The projection matrix P projects 3D points into the rectified image.#
00398 #######################################################################
00399
00400 # The image dimensions with which the camera was calibrated. Normally
00401 # this will be the full camera resolution in pixels.
00402 uint32 height
00403 uint32 width
00404
00405 # The distortion model used. Supported models are listed in
00406 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00407 # simple model of radial and tangential distortion - is sufficent.
00408 string distortion_model
00409
00410 # The distortion parameters, size depending on the distortion model.
00411 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00412 float64[] D
00413
00414 # Intrinsic camera matrix for the raw (distorted) images.
00415 # [fx 0 cx]
00416 # K = [ 0 fy cy]
00417 # [ 0 0 1]
00418 # Projects 3D points in the camera coordinate frame to 2D pixel
00419 # coordinates using the focal lengths (fx, fy) and principal point
00420 # (cx, cy).
00421 float64[9] K # 3x3 row-major matrix
00422
00423 # Rectification matrix (stereo cameras only)
00424 # A rotation matrix aligning the camera coordinate system to the ideal
00425 # stereo image plane so that epipolar lines in both stereo images are
00426 # parallel.
00427 float64[9] R # 3x3 row-major matrix
00428
00429 # Projection/camera matrix
00430 # [fx' 0 cx' Tx]
00431 # P = [ 0 fy' cy' Ty]
00432 # [ 0 0 1 0]
00433 # By convention, this matrix specifies the intrinsic (camera) matrix
00434 # of the processed (rectified) image. That is, the left 3x3 portion
00435 # is the normal camera intrinsic matrix for the rectified image.
00436 # It projects 3D points in the camera coordinate frame to 2D pixel
00437 # coordinates using the focal lengths (fx', fy') and principal point
00438 # (cx', cy') - these may differ from the values in K.
00439 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00440 # also have R = the identity and P[1:3,1:3] = K.
00441 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00442 # position of the optical center of the second camera in the first
00443 # camera's frame. We assume Tz = 0 so both cameras are in the same
00444 # stereo image plane. The first camera always has Tx = Ty = 0. For
00445 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00446 # Tx = -fx' * B, where B is the baseline between the cameras.
00447 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00448 # the rectified image is given by:
00449 # [u v w]' = P * [X Y Z 1]'
00450 # x = u / w
00451 # y = v / w
00452 # This holds for both images of a stereo pair.
00453 float64[12] P # 3x4 row-major matrix
00454
00455
00456 #######################################################################
00457 # Operational Parameters #
00458 #######################################################################
00459 # These define the image region actually captured by the camera #
00460 # driver. Although they affect the geometry of the output image, they #
00461 # may be changed freely without recalibrating the camera. #
00462 #######################################################################
00463
00464 # Binning refers here to any camera setting which combines rectangular
00465 # neighborhoods of pixels into larger "super-pixels." It reduces the
00466 # resolution of the output image to
00467 # (width / binning_x) x (height / binning_y).
00468 # The default values binning_x = binning_y = 0 is considered the same
00469 # as binning_x = binning_y = 1 (no subsampling).
00470 uint32 binning_x
00471 uint32 binning_y
00472
00473 # Region of interest (subwindow of full camera resolution), given in
00474 # full resolution (unbinned) image coordinates. A particular ROI
00475 # always denotes the same window of pixels on the camera sensor,
00476 # regardless of binning settings.
00477 # The default setting of roi (all values 0) is considered the same as
00478 # full resolution (roi.width = width, roi.height = height).
00479 RegionOfInterest roi
00480
00481 ================================================================================
00482 MSG: sensor_msgs/RegionOfInterest
00483 # This message is used to specify a region of interest within an image.
00484 #
00485 # When used to specify the ROI setting of the camera when the image was
00486 # taken, the height and width fields should either match the height and
00487 # width fields for the associated image; or height = width = 0
00488 # indicates that the full resolution image was captured.
00489
00490 uint32 x_offset # Leftmost pixel of the ROI
00491 # (0 if the ROI includes the left edge of the image)
00492 uint32 y_offset # Topmost pixel of the ROI
00493 # (0 if the ROI includes the top edge of the image)
00494 uint32 height # Height of ROI
00495 uint32 width # Width of ROI
00496
00497 # True if a distinct rectified ROI should be calculated from the "raw"
00498 # ROI in this message. Typically this should be False if the full image
00499 # is captured (ROI not used), and True if a subwindow is captured (ROI
00500 # used).
00501 bool do_rectify
00502
00503 ================================================================================
00504 MSG: geometry_msgs/Vector3
00505 # This represents a vector in free space.
00506
00507 float64 x
00508 float64 y
00509 float64 z
00510 ================================================================================
00511 MSG: pr2_object_manipulation_msgs/IMGUIAdvancedOptions
00512
00513 bool reactive_grasping
00514 bool reactive_force
00515 bool reactive_place
00516 int32 lift_steps
00517 int32 retreat_steps
00518 int32 lift_direction_choice
00519 int32 desired_approach
00520 int32 min_approach
00521 float32 max_contact_force
00522 bool find_alternatives
00523 bool always_plan_grasps
00524 bool cycle_gripper_opening
00525
00526 ================================================================================
00527 MSG: pr2_object_manipulation_msgs/IMGUICommand
00528
00529 int32 PICKUP = 0
00530 int32 PLACE = 1
00531 int32 PLANNED_MOVE = 2
00532 int32 RESET = 3
00533 int32 MOVE_ARM = 4
00534 int32 LOOK_AT_TABLE = 5
00535 int32 MODEL_OBJECT = 6
00536 int32 MOVE_GRIPPER = 7
00537 int32 SCRIPTED_ACTION = 8
00538 int32 STOP_NAV = 9
00539
00540 int32 command
00541 string script_name
00542 string script_group_name
00543
00544 """
00545 __slots__ = ['options','command']
00546 _slot_types = ['pr2_object_manipulation_msgs/IMGUIOptions','pr2_object_manipulation_msgs/IMGUICommand']
00547
00548 def __init__(self, *args, **kwds):
00549 """
00550 Constructor. Any message fields that are implicitly/explicitly
00551 set to None will be assigned a default value. The recommend
00552 use is keyword arguments as this is more robust to future message
00553 changes. You cannot mix in-order arguments and keyword arguments.
00554
00555 The available fields are:
00556 options,command
00557
00558 :param args: complete set of field values, in .msg order
00559 :param kwds: use keyword arguments corresponding to message field names
00560 to set specific fields.
00561 """
00562 if args or kwds:
00563 super(IMGUIGoal, self).__init__(*args, **kwds)
00564 #message fields cannot be None, assign default values for those that are
00565 if self.options is None:
00566 self.options = pr2_object_manipulation_msgs.msg.IMGUIOptions()
00567 if self.command is None:
00568 self.command = pr2_object_manipulation_msgs.msg.IMGUICommand()
00569 else:
00570 self.options = pr2_object_manipulation_msgs.msg.IMGUIOptions()
00571 self.command = pr2_object_manipulation_msgs.msg.IMGUICommand()
00572
00573 def _get_types(self):
00574 """
00575 internal API method
00576 """
00577 return self._slot_types
00578
00579 def serialize(self, buff):
00580 """
00581 serialize message into buffer
00582 :param buff: buffer, ``StringIO``
00583 """
00584 try:
00585 _x = self
00586 buff.write(_struct_B6i.pack(_x.options.collision_checked, _x.options.grasp_selection, _x.options.arm_selection, _x.options.reset_choice, _x.options.arm_action_choice, _x.options.arm_planner_choice, _x.options.gripper_slider_position))
00587 _x = self.options.selected_object.reference_frame_id
00588 length = len(_x)
00589 if python3 or type(_x) == unicode:
00590 _x = _x.encode('utf-8')
00591 length = len(_x)
00592 buff.write(struct.pack('<I%ss'%length, length, _x))
00593 length = len(self.options.selected_object.potential_models)
00594 buff.write(_struct_I.pack(length))
00595 for val1 in self.options.selected_object.potential_models:
00596 buff.write(_struct_i.pack(val1.model_id))
00597 _v1 = val1.type
00598 _x = _v1.key
00599 length = len(_x)
00600 if python3 or type(_x) == unicode:
00601 _x = _x.encode('utf-8')
00602 length = len(_x)
00603 buff.write(struct.pack('<I%ss'%length, length, _x))
00604 _x = _v1.db
00605 length = len(_x)
00606 if python3 or type(_x) == unicode:
00607 _x = _x.encode('utf-8')
00608 length = len(_x)
00609 buff.write(struct.pack('<I%ss'%length, length, _x))
00610 _v2 = val1.pose
00611 _v3 = _v2.header
00612 buff.write(_struct_I.pack(_v3.seq))
00613 _v4 = _v3.stamp
00614 _x = _v4
00615 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00616 _x = _v3.frame_id
00617 length = len(_x)
00618 if python3 or type(_x) == unicode:
00619 _x = _x.encode('utf-8')
00620 length = len(_x)
00621 buff.write(struct.pack('<I%ss'%length, length, _x))
00622 _v5 = _v2.pose
00623 _v6 = _v5.position
00624 _x = _v6
00625 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00626 _v7 = _v5.orientation
00627 _x = _v7
00628 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00629 buff.write(_struct_f.pack(val1.confidence))
00630 _x = val1.detector_name
00631 length = len(_x)
00632 if python3 or type(_x) == unicode:
00633 _x = _x.encode('utf-8')
00634 length = len(_x)
00635 buff.write(struct.pack('<I%ss'%length, length, _x))
00636 _x = self
00637 buff.write(_struct_3I.pack(_x.options.selected_object.cluster.header.seq, _x.options.selected_object.cluster.header.stamp.secs, _x.options.selected_object.cluster.header.stamp.nsecs))
00638 _x = self.options.selected_object.cluster.header.frame_id
00639 length = len(_x)
00640 if python3 or type(_x) == unicode:
00641 _x = _x.encode('utf-8')
00642 length = len(_x)
00643 buff.write(struct.pack('<I%ss'%length, length, _x))
00644 length = len(self.options.selected_object.cluster.points)
00645 buff.write(_struct_I.pack(length))
00646 for val1 in self.options.selected_object.cluster.points:
00647 _x = val1
00648 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00649 length = len(self.options.selected_object.cluster.channels)
00650 buff.write(_struct_I.pack(length))
00651 for val1 in self.options.selected_object.cluster.channels:
00652 _x = val1.name
00653 length = len(_x)
00654 if python3 or type(_x) == unicode:
00655 _x = _x.encode('utf-8')
00656 length = len(_x)
00657 buff.write(struct.pack('<I%ss'%length, length, _x))
00658 length = len(val1.values)
00659 buff.write(_struct_I.pack(length))
00660 pattern = '<%sf'%length
00661 buff.write(struct.pack(pattern, *val1.values))
00662 _x = self
00663 buff.write(_struct_3I.pack(_x.options.selected_object.region.cloud.header.seq, _x.options.selected_object.region.cloud.header.stamp.secs, _x.options.selected_object.region.cloud.header.stamp.nsecs))
00664 _x = self.options.selected_object.region.cloud.header.frame_id
00665 length = len(_x)
00666 if python3 or type(_x) == unicode:
00667 _x = _x.encode('utf-8')
00668 length = len(_x)
00669 buff.write(struct.pack('<I%ss'%length, length, _x))
00670 _x = self
00671 buff.write(_struct_2I.pack(_x.options.selected_object.region.cloud.height, _x.options.selected_object.region.cloud.width))
00672 length = len(self.options.selected_object.region.cloud.fields)
00673 buff.write(_struct_I.pack(length))
00674 for val1 in self.options.selected_object.region.cloud.fields:
00675 _x = val1.name
00676 length = len(_x)
00677 if python3 or type(_x) == unicode:
00678 _x = _x.encode('utf-8')
00679 length = len(_x)
00680 buff.write(struct.pack('<I%ss'%length, length, _x))
00681 _x = val1
00682 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00683 _x = self
00684 buff.write(_struct_B2I.pack(_x.options.selected_object.region.cloud.is_bigendian, _x.options.selected_object.region.cloud.point_step, _x.options.selected_object.region.cloud.row_step))
00685 _x = self.options.selected_object.region.cloud.data
00686 length = len(_x)
00687 # - if encoded as a list instead, serialize as bytes instead of string
00688 if type(_x) in [list, tuple]:
00689 buff.write(struct.pack('<I%sB'%length, length, *_x))
00690 else:
00691 buff.write(struct.pack('<I%ss'%length, length, _x))
00692 buff.write(_struct_B.pack(self.options.selected_object.region.cloud.is_dense))
00693 length = len(self.options.selected_object.region.mask)
00694 buff.write(_struct_I.pack(length))
00695 pattern = '<%si'%length
00696 buff.write(struct.pack(pattern, *self.options.selected_object.region.mask))
00697 _x = self
00698 buff.write(_struct_3I.pack(_x.options.selected_object.region.image.header.seq, _x.options.selected_object.region.image.header.stamp.secs, _x.options.selected_object.region.image.header.stamp.nsecs))
00699 _x = self.options.selected_object.region.image.header.frame_id
00700 length = len(_x)
00701 if python3 or type(_x) == unicode:
00702 _x = _x.encode('utf-8')
00703 length = len(_x)
00704 buff.write(struct.pack('<I%ss'%length, length, _x))
00705 _x = self
00706 buff.write(_struct_2I.pack(_x.options.selected_object.region.image.height, _x.options.selected_object.region.image.width))
00707 _x = self.options.selected_object.region.image.encoding
00708 length = len(_x)
00709 if python3 or type(_x) == unicode:
00710 _x = _x.encode('utf-8')
00711 length = len(_x)
00712 buff.write(struct.pack('<I%ss'%length, length, _x))
00713 _x = self
00714 buff.write(_struct_BI.pack(_x.options.selected_object.region.image.is_bigendian, _x.options.selected_object.region.image.step))
00715 _x = self.options.selected_object.region.image.data
00716 length = len(_x)
00717 # - if encoded as a list instead, serialize as bytes instead of string
00718 if type(_x) in [list, tuple]:
00719 buff.write(struct.pack('<I%sB'%length, length, *_x))
00720 else:
00721 buff.write(struct.pack('<I%ss'%length, length, _x))
00722 _x = self
00723 buff.write(_struct_3I.pack(_x.options.selected_object.region.disparity_image.header.seq, _x.options.selected_object.region.disparity_image.header.stamp.secs, _x.options.selected_object.region.disparity_image.header.stamp.nsecs))
00724 _x = self.options.selected_object.region.disparity_image.header.frame_id
00725 length = len(_x)
00726 if python3 or type(_x) == unicode:
00727 _x = _x.encode('utf-8')
00728 length = len(_x)
00729 buff.write(struct.pack('<I%ss'%length, length, _x))
00730 _x = self
00731 buff.write(_struct_2I.pack(_x.options.selected_object.region.disparity_image.height, _x.options.selected_object.region.disparity_image.width))
00732 _x = self.options.selected_object.region.disparity_image.encoding
00733 length = len(_x)
00734 if python3 or type(_x) == unicode:
00735 _x = _x.encode('utf-8')
00736 length = len(_x)
00737 buff.write(struct.pack('<I%ss'%length, length, _x))
00738 _x = self
00739 buff.write(_struct_BI.pack(_x.options.selected_object.region.disparity_image.is_bigendian, _x.options.selected_object.region.disparity_image.step))
00740 _x = self.options.selected_object.region.disparity_image.data
00741 length = len(_x)
00742 # - if encoded as a list instead, serialize as bytes instead of string
00743 if type(_x) in [list, tuple]:
00744 buff.write(struct.pack('<I%sB'%length, length, *_x))
00745 else:
00746 buff.write(struct.pack('<I%ss'%length, length, _x))
00747 _x = self
00748 buff.write(_struct_3I.pack(_x.options.selected_object.region.cam_info.header.seq, _x.options.selected_object.region.cam_info.header.stamp.secs, _x.options.selected_object.region.cam_info.header.stamp.nsecs))
00749 _x = self.options.selected_object.region.cam_info.header.frame_id
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 _x = self
00756 buff.write(_struct_2I.pack(_x.options.selected_object.region.cam_info.height, _x.options.selected_object.region.cam_info.width))
00757 _x = self.options.selected_object.region.cam_info.distortion_model
00758 length = len(_x)
00759 if python3 or type(_x) == unicode:
00760 _x = _x.encode('utf-8')
00761 length = len(_x)
00762 buff.write(struct.pack('<I%ss'%length, length, _x))
00763 length = len(self.options.selected_object.region.cam_info.D)
00764 buff.write(_struct_I.pack(length))
00765 pattern = '<%sd'%length
00766 buff.write(struct.pack(pattern, *self.options.selected_object.region.cam_info.D))
00767 buff.write(_struct_9d.pack(*self.options.selected_object.region.cam_info.K))
00768 buff.write(_struct_9d.pack(*self.options.selected_object.region.cam_info.R))
00769 buff.write(_struct_12d.pack(*self.options.selected_object.region.cam_info.P))
00770 _x = self
00771 buff.write(_struct_6IB3I.pack(_x.options.selected_object.region.cam_info.binning_x, _x.options.selected_object.region.cam_info.binning_y, _x.options.selected_object.region.cam_info.roi.x_offset, _x.options.selected_object.region.cam_info.roi.y_offset, _x.options.selected_object.region.cam_info.roi.height, _x.options.selected_object.region.cam_info.roi.width, _x.options.selected_object.region.cam_info.roi.do_rectify, _x.options.selected_object.region.roi_box_pose.header.seq, _x.options.selected_object.region.roi_box_pose.header.stamp.secs, _x.options.selected_object.region.roi_box_pose.header.stamp.nsecs))
00772 _x = self.options.selected_object.region.roi_box_pose.header.frame_id
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 = self
00779 buff.write(_struct_10d.pack(_x.options.selected_object.region.roi_box_pose.pose.position.x, _x.options.selected_object.region.roi_box_pose.pose.position.y, _x.options.selected_object.region.roi_box_pose.pose.position.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.x, _x.options.selected_object.region.roi_box_pose.pose.orientation.y, _x.options.selected_object.region.roi_box_pose.pose.orientation.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.w, _x.options.selected_object.region.roi_box_dims.x, _x.options.selected_object.region.roi_box_dims.y, _x.options.selected_object.region.roi_box_dims.z))
00780 _x = self.options.selected_object.collision_name
00781 length = len(_x)
00782 if python3 or type(_x) == unicode:
00783 _x = _x.encode('utf-8')
00784 length = len(_x)
00785 buff.write(struct.pack('<I%ss'%length, length, _x))
00786 length = len(self.options.movable_obstacles)
00787 buff.write(_struct_I.pack(length))
00788 for val1 in self.options.movable_obstacles:
00789 _x = val1.reference_frame_id
00790 length = len(_x)
00791 if python3 or type(_x) == unicode:
00792 _x = _x.encode('utf-8')
00793 length = len(_x)
00794 buff.write(struct.pack('<I%ss'%length, length, _x))
00795 length = len(val1.potential_models)
00796 buff.write(_struct_I.pack(length))
00797 for val2 in val1.potential_models:
00798 buff.write(_struct_i.pack(val2.model_id))
00799 _v8 = val2.type
00800 _x = _v8.key
00801 length = len(_x)
00802 if python3 or type(_x) == unicode:
00803 _x = _x.encode('utf-8')
00804 length = len(_x)
00805 buff.write(struct.pack('<I%ss'%length, length, _x))
00806 _x = _v8.db
00807 length = len(_x)
00808 if python3 or type(_x) == unicode:
00809 _x = _x.encode('utf-8')
00810 length = len(_x)
00811 buff.write(struct.pack('<I%ss'%length, length, _x))
00812 _v9 = val2.pose
00813 _v10 = _v9.header
00814 buff.write(_struct_I.pack(_v10.seq))
00815 _v11 = _v10.stamp
00816 _x = _v11
00817 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00818 _x = _v10.frame_id
00819 length = len(_x)
00820 if python3 or type(_x) == unicode:
00821 _x = _x.encode('utf-8')
00822 length = len(_x)
00823 buff.write(struct.pack('<I%ss'%length, length, _x))
00824 _v12 = _v9.pose
00825 _v13 = _v12.position
00826 _x = _v13
00827 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00828 _v14 = _v12.orientation
00829 _x = _v14
00830 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00831 buff.write(_struct_f.pack(val2.confidence))
00832 _x = val2.detector_name
00833 length = len(_x)
00834 if python3 or type(_x) == unicode:
00835 _x = _x.encode('utf-8')
00836 length = len(_x)
00837 buff.write(struct.pack('<I%ss'%length, length, _x))
00838 _v15 = val1.cluster
00839 _v16 = _v15.header
00840 buff.write(_struct_I.pack(_v16.seq))
00841 _v17 = _v16.stamp
00842 _x = _v17
00843 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00844 _x = _v16.frame_id
00845 length = len(_x)
00846 if python3 or type(_x) == unicode:
00847 _x = _x.encode('utf-8')
00848 length = len(_x)
00849 buff.write(struct.pack('<I%ss'%length, length, _x))
00850 length = len(_v15.points)
00851 buff.write(_struct_I.pack(length))
00852 for val3 in _v15.points:
00853 _x = val3
00854 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00855 length = len(_v15.channels)
00856 buff.write(_struct_I.pack(length))
00857 for val3 in _v15.channels:
00858 _x = val3.name
00859 length = len(_x)
00860 if python3 or type(_x) == unicode:
00861 _x = _x.encode('utf-8')
00862 length = len(_x)
00863 buff.write(struct.pack('<I%ss'%length, length, _x))
00864 length = len(val3.values)
00865 buff.write(_struct_I.pack(length))
00866 pattern = '<%sf'%length
00867 buff.write(struct.pack(pattern, *val3.values))
00868 _v18 = val1.region
00869 _v19 = _v18.cloud
00870 _v20 = _v19.header
00871 buff.write(_struct_I.pack(_v20.seq))
00872 _v21 = _v20.stamp
00873 _x = _v21
00874 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00875 _x = _v20.frame_id
00876 length = len(_x)
00877 if python3 or type(_x) == unicode:
00878 _x = _x.encode('utf-8')
00879 length = len(_x)
00880 buff.write(struct.pack('<I%ss'%length, length, _x))
00881 _x = _v19
00882 buff.write(_struct_2I.pack(_x.height, _x.width))
00883 length = len(_v19.fields)
00884 buff.write(_struct_I.pack(length))
00885 for val4 in _v19.fields:
00886 _x = val4.name
00887 length = len(_x)
00888 if python3 or type(_x) == unicode:
00889 _x = _x.encode('utf-8')
00890 length = len(_x)
00891 buff.write(struct.pack('<I%ss'%length, length, _x))
00892 _x = val4
00893 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00894 _x = _v19
00895 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
00896 _x = _v19.data
00897 length = len(_x)
00898 # - if encoded as a list instead, serialize as bytes instead of string
00899 if type(_x) in [list, tuple]:
00900 buff.write(struct.pack('<I%sB'%length, length, *_x))
00901 else:
00902 buff.write(struct.pack('<I%ss'%length, length, _x))
00903 buff.write(_struct_B.pack(_v19.is_dense))
00904 length = len(_v18.mask)
00905 buff.write(_struct_I.pack(length))
00906 pattern = '<%si'%length
00907 buff.write(struct.pack(pattern, *_v18.mask))
00908 _v22 = _v18.image
00909 _v23 = _v22.header
00910 buff.write(_struct_I.pack(_v23.seq))
00911 _v24 = _v23.stamp
00912 _x = _v24
00913 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00914 _x = _v23.frame_id
00915 length = len(_x)
00916 if python3 or type(_x) == unicode:
00917 _x = _x.encode('utf-8')
00918 length = len(_x)
00919 buff.write(struct.pack('<I%ss'%length, length, _x))
00920 _x = _v22
00921 buff.write(_struct_2I.pack(_x.height, _x.width))
00922 _x = _v22.encoding
00923 length = len(_x)
00924 if python3 or type(_x) == unicode:
00925 _x = _x.encode('utf-8')
00926 length = len(_x)
00927 buff.write(struct.pack('<I%ss'%length, length, _x))
00928 _x = _v22
00929 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00930 _x = _v22.data
00931 length = len(_x)
00932 # - if encoded as a list instead, serialize as bytes instead of string
00933 if type(_x) in [list, tuple]:
00934 buff.write(struct.pack('<I%sB'%length, length, *_x))
00935 else:
00936 buff.write(struct.pack('<I%ss'%length, length, _x))
00937 _v25 = _v18.disparity_image
00938 _v26 = _v25.header
00939 buff.write(_struct_I.pack(_v26.seq))
00940 _v27 = _v26.stamp
00941 _x = _v27
00942 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00943 _x = _v26.frame_id
00944 length = len(_x)
00945 if python3 or type(_x) == unicode:
00946 _x = _x.encode('utf-8')
00947 length = len(_x)
00948 buff.write(struct.pack('<I%ss'%length, length, _x))
00949 _x = _v25
00950 buff.write(_struct_2I.pack(_x.height, _x.width))
00951 _x = _v25.encoding
00952 length = len(_x)
00953 if python3 or type(_x) == unicode:
00954 _x = _x.encode('utf-8')
00955 length = len(_x)
00956 buff.write(struct.pack('<I%ss'%length, length, _x))
00957 _x = _v25
00958 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00959 _x = _v25.data
00960 length = len(_x)
00961 # - if encoded as a list instead, serialize as bytes instead of string
00962 if type(_x) in [list, tuple]:
00963 buff.write(struct.pack('<I%sB'%length, length, *_x))
00964 else:
00965 buff.write(struct.pack('<I%ss'%length, length, _x))
00966 _v28 = _v18.cam_info
00967 _v29 = _v28.header
00968 buff.write(_struct_I.pack(_v29.seq))
00969 _v30 = _v29.stamp
00970 _x = _v30
00971 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00972 _x = _v29.frame_id
00973 length = len(_x)
00974 if python3 or type(_x) == unicode:
00975 _x = _x.encode('utf-8')
00976 length = len(_x)
00977 buff.write(struct.pack('<I%ss'%length, length, _x))
00978 _x = _v28
00979 buff.write(_struct_2I.pack(_x.height, _x.width))
00980 _x = _v28.distortion_model
00981 length = len(_x)
00982 if python3 or type(_x) == unicode:
00983 _x = _x.encode('utf-8')
00984 length = len(_x)
00985 buff.write(struct.pack('<I%ss'%length, length, _x))
00986 length = len(_v28.D)
00987 buff.write(_struct_I.pack(length))
00988 pattern = '<%sd'%length
00989 buff.write(struct.pack(pattern, *_v28.D))
00990 buff.write(_struct_9d.pack(*_v28.K))
00991 buff.write(_struct_9d.pack(*_v28.R))
00992 buff.write(_struct_12d.pack(*_v28.P))
00993 _x = _v28
00994 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
00995 _v31 = _v28.roi
00996 _x = _v31
00997 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
00998 _v32 = _v18.roi_box_pose
00999 _v33 = _v32.header
01000 buff.write(_struct_I.pack(_v33.seq))
01001 _v34 = _v33.stamp
01002 _x = _v34
01003 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01004 _x = _v33.frame_id
01005 length = len(_x)
01006 if python3 or type(_x) == unicode:
01007 _x = _x.encode('utf-8')
01008 length = len(_x)
01009 buff.write(struct.pack('<I%ss'%length, length, _x))
01010 _v35 = _v32.pose
01011 _v36 = _v35.position
01012 _x = _v36
01013 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01014 _v37 = _v35.orientation
01015 _x = _v37
01016 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01017 _v38 = _v18.roi_box_dims
01018 _x = _v38
01019 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01020 _x = val1.collision_name
01021 length = len(_x)
01022 if python3 or type(_x) == unicode:
01023 _x = _x.encode('utf-8')
01024 length = len(_x)
01025 buff.write(struct.pack('<I%ss'%length, length, _x))
01026 _x = self
01027 buff.write(_struct_3B5if3Bi.pack(_x.options.adv_options.reactive_grasping, _x.options.adv_options.reactive_force, _x.options.adv_options.reactive_place, _x.options.adv_options.lift_steps, _x.options.adv_options.retreat_steps, _x.options.adv_options.lift_direction_choice, _x.options.adv_options.desired_approach, _x.options.adv_options.min_approach, _x.options.adv_options.max_contact_force, _x.options.adv_options.find_alternatives, _x.options.adv_options.always_plan_grasps, _x.options.adv_options.cycle_gripper_opening, _x.command.command))
01028 _x = self.command.script_name
01029 length = len(_x)
01030 if python3 or type(_x) == unicode:
01031 _x = _x.encode('utf-8')
01032 length = len(_x)
01033 buff.write(struct.pack('<I%ss'%length, length, _x))
01034 _x = self.command.script_group_name
01035 length = len(_x)
01036 if python3 or type(_x) == unicode:
01037 _x = _x.encode('utf-8')
01038 length = len(_x)
01039 buff.write(struct.pack('<I%ss'%length, length, _x))
01040 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
01041 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
01042
01043 def deserialize(self, str):
01044 """
01045 unpack serialized message in str into this message instance
01046 :param str: byte array of serialized message, ``str``
01047 """
01048 try:
01049 if self.options is None:
01050 self.options = pr2_object_manipulation_msgs.msg.IMGUIOptions()
01051 if self.command is None:
01052 self.command = pr2_object_manipulation_msgs.msg.IMGUICommand()
01053 end = 0
01054 _x = self
01055 start = end
01056 end += 25
01057 (_x.options.collision_checked, _x.options.grasp_selection, _x.options.arm_selection, _x.options.reset_choice, _x.options.arm_action_choice, _x.options.arm_planner_choice, _x.options.gripper_slider_position,) = _struct_B6i.unpack(str[start:end])
01058 self.options.collision_checked = bool(self.options.collision_checked)
01059 start = end
01060 end += 4
01061 (length,) = _struct_I.unpack(str[start:end])
01062 start = end
01063 end += length
01064 if python3:
01065 self.options.selected_object.reference_frame_id = str[start:end].decode('utf-8')
01066 else:
01067 self.options.selected_object.reference_frame_id = str[start:end]
01068 start = end
01069 end += 4
01070 (length,) = _struct_I.unpack(str[start:end])
01071 self.options.selected_object.potential_models = []
01072 for i in range(0, length):
01073 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01074 start = end
01075 end += 4
01076 (val1.model_id,) = _struct_i.unpack(str[start:end])
01077 _v39 = val1.type
01078 start = end
01079 end += 4
01080 (length,) = _struct_I.unpack(str[start:end])
01081 start = end
01082 end += length
01083 if python3:
01084 _v39.key = str[start:end].decode('utf-8')
01085 else:
01086 _v39.key = str[start:end]
01087 start = end
01088 end += 4
01089 (length,) = _struct_I.unpack(str[start:end])
01090 start = end
01091 end += length
01092 if python3:
01093 _v39.db = str[start:end].decode('utf-8')
01094 else:
01095 _v39.db = str[start:end]
01096 _v40 = val1.pose
01097 _v41 = _v40.header
01098 start = end
01099 end += 4
01100 (_v41.seq,) = _struct_I.unpack(str[start:end])
01101 _v42 = _v41.stamp
01102 _x = _v42
01103 start = end
01104 end += 8
01105 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01106 start = end
01107 end += 4
01108 (length,) = _struct_I.unpack(str[start:end])
01109 start = end
01110 end += length
01111 if python3:
01112 _v41.frame_id = str[start:end].decode('utf-8')
01113 else:
01114 _v41.frame_id = str[start:end]
01115 _v43 = _v40.pose
01116 _v44 = _v43.position
01117 _x = _v44
01118 start = end
01119 end += 24
01120 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01121 _v45 = _v43.orientation
01122 _x = _v45
01123 start = end
01124 end += 32
01125 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01126 start = end
01127 end += 4
01128 (val1.confidence,) = _struct_f.unpack(str[start:end])
01129 start = end
01130 end += 4
01131 (length,) = _struct_I.unpack(str[start:end])
01132 start = end
01133 end += length
01134 if python3:
01135 val1.detector_name = str[start:end].decode('utf-8')
01136 else:
01137 val1.detector_name = str[start:end]
01138 self.options.selected_object.potential_models.append(val1)
01139 _x = self
01140 start = end
01141 end += 12
01142 (_x.options.selected_object.cluster.header.seq, _x.options.selected_object.cluster.header.stamp.secs, _x.options.selected_object.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01143 start = end
01144 end += 4
01145 (length,) = _struct_I.unpack(str[start:end])
01146 start = end
01147 end += length
01148 if python3:
01149 self.options.selected_object.cluster.header.frame_id = str[start:end].decode('utf-8')
01150 else:
01151 self.options.selected_object.cluster.header.frame_id = str[start:end]
01152 start = end
01153 end += 4
01154 (length,) = _struct_I.unpack(str[start:end])
01155 self.options.selected_object.cluster.points = []
01156 for i in range(0, length):
01157 val1 = geometry_msgs.msg.Point32()
01158 _x = val1
01159 start = end
01160 end += 12
01161 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01162 self.options.selected_object.cluster.points.append(val1)
01163 start = end
01164 end += 4
01165 (length,) = _struct_I.unpack(str[start:end])
01166 self.options.selected_object.cluster.channels = []
01167 for i in range(0, length):
01168 val1 = sensor_msgs.msg.ChannelFloat32()
01169 start = end
01170 end += 4
01171 (length,) = _struct_I.unpack(str[start:end])
01172 start = end
01173 end += length
01174 if python3:
01175 val1.name = str[start:end].decode('utf-8')
01176 else:
01177 val1.name = str[start:end]
01178 start = end
01179 end += 4
01180 (length,) = _struct_I.unpack(str[start:end])
01181 pattern = '<%sf'%length
01182 start = end
01183 end += struct.calcsize(pattern)
01184 val1.values = struct.unpack(pattern, str[start:end])
01185 self.options.selected_object.cluster.channels.append(val1)
01186 _x = self
01187 start = end
01188 end += 12
01189 (_x.options.selected_object.region.cloud.header.seq, _x.options.selected_object.region.cloud.header.stamp.secs, _x.options.selected_object.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01190 start = end
01191 end += 4
01192 (length,) = _struct_I.unpack(str[start:end])
01193 start = end
01194 end += length
01195 if python3:
01196 self.options.selected_object.region.cloud.header.frame_id = str[start:end].decode('utf-8')
01197 else:
01198 self.options.selected_object.region.cloud.header.frame_id = str[start:end]
01199 _x = self
01200 start = end
01201 end += 8
01202 (_x.options.selected_object.region.cloud.height, _x.options.selected_object.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01203 start = end
01204 end += 4
01205 (length,) = _struct_I.unpack(str[start:end])
01206 self.options.selected_object.region.cloud.fields = []
01207 for i in range(0, length):
01208 val1 = sensor_msgs.msg.PointField()
01209 start = end
01210 end += 4
01211 (length,) = _struct_I.unpack(str[start:end])
01212 start = end
01213 end += length
01214 if python3:
01215 val1.name = str[start:end].decode('utf-8')
01216 else:
01217 val1.name = str[start:end]
01218 _x = val1
01219 start = end
01220 end += 9
01221 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01222 self.options.selected_object.region.cloud.fields.append(val1)
01223 _x = self
01224 start = end
01225 end += 9
01226 (_x.options.selected_object.region.cloud.is_bigendian, _x.options.selected_object.region.cloud.point_step, _x.options.selected_object.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01227 self.options.selected_object.region.cloud.is_bigendian = bool(self.options.selected_object.region.cloud.is_bigendian)
01228 start = end
01229 end += 4
01230 (length,) = _struct_I.unpack(str[start:end])
01231 start = end
01232 end += length
01233 self.options.selected_object.region.cloud.data = str[start:end]
01234 start = end
01235 end += 1
01236 (self.options.selected_object.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01237 self.options.selected_object.region.cloud.is_dense = bool(self.options.selected_object.region.cloud.is_dense)
01238 start = end
01239 end += 4
01240 (length,) = _struct_I.unpack(str[start:end])
01241 pattern = '<%si'%length
01242 start = end
01243 end += struct.calcsize(pattern)
01244 self.options.selected_object.region.mask = struct.unpack(pattern, str[start:end])
01245 _x = self
01246 start = end
01247 end += 12
01248 (_x.options.selected_object.region.image.header.seq, _x.options.selected_object.region.image.header.stamp.secs, _x.options.selected_object.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01249 start = end
01250 end += 4
01251 (length,) = _struct_I.unpack(str[start:end])
01252 start = end
01253 end += length
01254 if python3:
01255 self.options.selected_object.region.image.header.frame_id = str[start:end].decode('utf-8')
01256 else:
01257 self.options.selected_object.region.image.header.frame_id = str[start:end]
01258 _x = self
01259 start = end
01260 end += 8
01261 (_x.options.selected_object.region.image.height, _x.options.selected_object.region.image.width,) = _struct_2I.unpack(str[start:end])
01262 start = end
01263 end += 4
01264 (length,) = _struct_I.unpack(str[start:end])
01265 start = end
01266 end += length
01267 if python3:
01268 self.options.selected_object.region.image.encoding = str[start:end].decode('utf-8')
01269 else:
01270 self.options.selected_object.region.image.encoding = str[start:end]
01271 _x = self
01272 start = end
01273 end += 5
01274 (_x.options.selected_object.region.image.is_bigendian, _x.options.selected_object.region.image.step,) = _struct_BI.unpack(str[start:end])
01275 start = end
01276 end += 4
01277 (length,) = _struct_I.unpack(str[start:end])
01278 start = end
01279 end += length
01280 self.options.selected_object.region.image.data = str[start:end]
01281 _x = self
01282 start = end
01283 end += 12
01284 (_x.options.selected_object.region.disparity_image.header.seq, _x.options.selected_object.region.disparity_image.header.stamp.secs, _x.options.selected_object.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01285 start = end
01286 end += 4
01287 (length,) = _struct_I.unpack(str[start:end])
01288 start = end
01289 end += length
01290 if python3:
01291 self.options.selected_object.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
01292 else:
01293 self.options.selected_object.region.disparity_image.header.frame_id = str[start:end]
01294 _x = self
01295 start = end
01296 end += 8
01297 (_x.options.selected_object.region.disparity_image.height, _x.options.selected_object.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01298 start = end
01299 end += 4
01300 (length,) = _struct_I.unpack(str[start:end])
01301 start = end
01302 end += length
01303 if python3:
01304 self.options.selected_object.region.disparity_image.encoding = str[start:end].decode('utf-8')
01305 else:
01306 self.options.selected_object.region.disparity_image.encoding = str[start:end]
01307 _x = self
01308 start = end
01309 end += 5
01310 (_x.options.selected_object.region.disparity_image.is_bigendian, _x.options.selected_object.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01311 start = end
01312 end += 4
01313 (length,) = _struct_I.unpack(str[start:end])
01314 start = end
01315 end += length
01316 self.options.selected_object.region.disparity_image.data = str[start:end]
01317 _x = self
01318 start = end
01319 end += 12
01320 (_x.options.selected_object.region.cam_info.header.seq, _x.options.selected_object.region.cam_info.header.stamp.secs, _x.options.selected_object.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01321 start = end
01322 end += 4
01323 (length,) = _struct_I.unpack(str[start:end])
01324 start = end
01325 end += length
01326 if python3:
01327 self.options.selected_object.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
01328 else:
01329 self.options.selected_object.region.cam_info.header.frame_id = str[start:end]
01330 _x = self
01331 start = end
01332 end += 8
01333 (_x.options.selected_object.region.cam_info.height, _x.options.selected_object.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01334 start = end
01335 end += 4
01336 (length,) = _struct_I.unpack(str[start:end])
01337 start = end
01338 end += length
01339 if python3:
01340 self.options.selected_object.region.cam_info.distortion_model = str[start:end].decode('utf-8')
01341 else:
01342 self.options.selected_object.region.cam_info.distortion_model = str[start:end]
01343 start = end
01344 end += 4
01345 (length,) = _struct_I.unpack(str[start:end])
01346 pattern = '<%sd'%length
01347 start = end
01348 end += struct.calcsize(pattern)
01349 self.options.selected_object.region.cam_info.D = struct.unpack(pattern, str[start:end])
01350 start = end
01351 end += 72
01352 self.options.selected_object.region.cam_info.K = _struct_9d.unpack(str[start:end])
01353 start = end
01354 end += 72
01355 self.options.selected_object.region.cam_info.R = _struct_9d.unpack(str[start:end])
01356 start = end
01357 end += 96
01358 self.options.selected_object.region.cam_info.P = _struct_12d.unpack(str[start:end])
01359 _x = self
01360 start = end
01361 end += 37
01362 (_x.options.selected_object.region.cam_info.binning_x, _x.options.selected_object.region.cam_info.binning_y, _x.options.selected_object.region.cam_info.roi.x_offset, _x.options.selected_object.region.cam_info.roi.y_offset, _x.options.selected_object.region.cam_info.roi.height, _x.options.selected_object.region.cam_info.roi.width, _x.options.selected_object.region.cam_info.roi.do_rectify, _x.options.selected_object.region.roi_box_pose.header.seq, _x.options.selected_object.region.roi_box_pose.header.stamp.secs, _x.options.selected_object.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01363 self.options.selected_object.region.cam_info.roi.do_rectify = bool(self.options.selected_object.region.cam_info.roi.do_rectify)
01364 start = end
01365 end += 4
01366 (length,) = _struct_I.unpack(str[start:end])
01367 start = end
01368 end += length
01369 if python3:
01370 self.options.selected_object.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
01371 else:
01372 self.options.selected_object.region.roi_box_pose.header.frame_id = str[start:end]
01373 _x = self
01374 start = end
01375 end += 80
01376 (_x.options.selected_object.region.roi_box_pose.pose.position.x, _x.options.selected_object.region.roi_box_pose.pose.position.y, _x.options.selected_object.region.roi_box_pose.pose.position.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.x, _x.options.selected_object.region.roi_box_pose.pose.orientation.y, _x.options.selected_object.region.roi_box_pose.pose.orientation.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.w, _x.options.selected_object.region.roi_box_dims.x, _x.options.selected_object.region.roi_box_dims.y, _x.options.selected_object.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
01377 start = end
01378 end += 4
01379 (length,) = _struct_I.unpack(str[start:end])
01380 start = end
01381 end += length
01382 if python3:
01383 self.options.selected_object.collision_name = str[start:end].decode('utf-8')
01384 else:
01385 self.options.selected_object.collision_name = str[start:end]
01386 start = end
01387 end += 4
01388 (length,) = _struct_I.unpack(str[start:end])
01389 self.options.movable_obstacles = []
01390 for i in range(0, length):
01391 val1 = manipulation_msgs.msg.GraspableObject()
01392 start = end
01393 end += 4
01394 (length,) = _struct_I.unpack(str[start:end])
01395 start = end
01396 end += length
01397 if python3:
01398 val1.reference_frame_id = str[start:end].decode('utf-8')
01399 else:
01400 val1.reference_frame_id = str[start:end]
01401 start = end
01402 end += 4
01403 (length,) = _struct_I.unpack(str[start:end])
01404 val1.potential_models = []
01405 for i in range(0, length):
01406 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
01407 start = end
01408 end += 4
01409 (val2.model_id,) = _struct_i.unpack(str[start:end])
01410 _v46 = val2.type
01411 start = end
01412 end += 4
01413 (length,) = _struct_I.unpack(str[start:end])
01414 start = end
01415 end += length
01416 if python3:
01417 _v46.key = str[start:end].decode('utf-8')
01418 else:
01419 _v46.key = str[start:end]
01420 start = end
01421 end += 4
01422 (length,) = _struct_I.unpack(str[start:end])
01423 start = end
01424 end += length
01425 if python3:
01426 _v46.db = str[start:end].decode('utf-8')
01427 else:
01428 _v46.db = str[start:end]
01429 _v47 = val2.pose
01430 _v48 = _v47.header
01431 start = end
01432 end += 4
01433 (_v48.seq,) = _struct_I.unpack(str[start:end])
01434 _v49 = _v48.stamp
01435 _x = _v49
01436 start = end
01437 end += 8
01438 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01439 start = end
01440 end += 4
01441 (length,) = _struct_I.unpack(str[start:end])
01442 start = end
01443 end += length
01444 if python3:
01445 _v48.frame_id = str[start:end].decode('utf-8')
01446 else:
01447 _v48.frame_id = str[start:end]
01448 _v50 = _v47.pose
01449 _v51 = _v50.position
01450 _x = _v51
01451 start = end
01452 end += 24
01453 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01454 _v52 = _v50.orientation
01455 _x = _v52
01456 start = end
01457 end += 32
01458 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01459 start = end
01460 end += 4
01461 (val2.confidence,) = _struct_f.unpack(str[start:end])
01462 start = end
01463 end += 4
01464 (length,) = _struct_I.unpack(str[start:end])
01465 start = end
01466 end += length
01467 if python3:
01468 val2.detector_name = str[start:end].decode('utf-8')
01469 else:
01470 val2.detector_name = str[start:end]
01471 val1.potential_models.append(val2)
01472 _v53 = val1.cluster
01473 _v54 = _v53.header
01474 start = end
01475 end += 4
01476 (_v54.seq,) = _struct_I.unpack(str[start:end])
01477 _v55 = _v54.stamp
01478 _x = _v55
01479 start = end
01480 end += 8
01481 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01482 start = end
01483 end += 4
01484 (length,) = _struct_I.unpack(str[start:end])
01485 start = end
01486 end += length
01487 if python3:
01488 _v54.frame_id = str[start:end].decode('utf-8')
01489 else:
01490 _v54.frame_id = str[start:end]
01491 start = end
01492 end += 4
01493 (length,) = _struct_I.unpack(str[start:end])
01494 _v53.points = []
01495 for i in range(0, length):
01496 val3 = geometry_msgs.msg.Point32()
01497 _x = val3
01498 start = end
01499 end += 12
01500 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01501 _v53.points.append(val3)
01502 start = end
01503 end += 4
01504 (length,) = _struct_I.unpack(str[start:end])
01505 _v53.channels = []
01506 for i in range(0, length):
01507 val3 = sensor_msgs.msg.ChannelFloat32()
01508 start = end
01509 end += 4
01510 (length,) = _struct_I.unpack(str[start:end])
01511 start = end
01512 end += length
01513 if python3:
01514 val3.name = str[start:end].decode('utf-8')
01515 else:
01516 val3.name = str[start:end]
01517 start = end
01518 end += 4
01519 (length,) = _struct_I.unpack(str[start:end])
01520 pattern = '<%sf'%length
01521 start = end
01522 end += struct.calcsize(pattern)
01523 val3.values = struct.unpack(pattern, str[start:end])
01524 _v53.channels.append(val3)
01525 _v56 = val1.region
01526 _v57 = _v56.cloud
01527 _v58 = _v57.header
01528 start = end
01529 end += 4
01530 (_v58.seq,) = _struct_I.unpack(str[start:end])
01531 _v59 = _v58.stamp
01532 _x = _v59
01533 start = end
01534 end += 8
01535 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01536 start = end
01537 end += 4
01538 (length,) = _struct_I.unpack(str[start:end])
01539 start = end
01540 end += length
01541 if python3:
01542 _v58.frame_id = str[start:end].decode('utf-8')
01543 else:
01544 _v58.frame_id = str[start:end]
01545 _x = _v57
01546 start = end
01547 end += 8
01548 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01549 start = end
01550 end += 4
01551 (length,) = _struct_I.unpack(str[start:end])
01552 _v57.fields = []
01553 for i in range(0, length):
01554 val4 = sensor_msgs.msg.PointField()
01555 start = end
01556 end += 4
01557 (length,) = _struct_I.unpack(str[start:end])
01558 start = end
01559 end += length
01560 if python3:
01561 val4.name = str[start:end].decode('utf-8')
01562 else:
01563 val4.name = str[start:end]
01564 _x = val4
01565 start = end
01566 end += 9
01567 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01568 _v57.fields.append(val4)
01569 _x = _v57
01570 start = end
01571 end += 9
01572 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
01573 _v57.is_bigendian = bool(_v57.is_bigendian)
01574 start = end
01575 end += 4
01576 (length,) = _struct_I.unpack(str[start:end])
01577 start = end
01578 end += length
01579 _v57.data = str[start:end]
01580 start = end
01581 end += 1
01582 (_v57.is_dense,) = _struct_B.unpack(str[start:end])
01583 _v57.is_dense = bool(_v57.is_dense)
01584 start = end
01585 end += 4
01586 (length,) = _struct_I.unpack(str[start:end])
01587 pattern = '<%si'%length
01588 start = end
01589 end += struct.calcsize(pattern)
01590 _v56.mask = struct.unpack(pattern, str[start:end])
01591 _v60 = _v56.image
01592 _v61 = _v60.header
01593 start = end
01594 end += 4
01595 (_v61.seq,) = _struct_I.unpack(str[start:end])
01596 _v62 = _v61.stamp
01597 _x = _v62
01598 start = end
01599 end += 8
01600 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01601 start = end
01602 end += 4
01603 (length,) = _struct_I.unpack(str[start:end])
01604 start = end
01605 end += length
01606 if python3:
01607 _v61.frame_id = str[start:end].decode('utf-8')
01608 else:
01609 _v61.frame_id = str[start:end]
01610 _x = _v60
01611 start = end
01612 end += 8
01613 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01614 start = end
01615 end += 4
01616 (length,) = _struct_I.unpack(str[start:end])
01617 start = end
01618 end += length
01619 if python3:
01620 _v60.encoding = str[start:end].decode('utf-8')
01621 else:
01622 _v60.encoding = str[start:end]
01623 _x = _v60
01624 start = end
01625 end += 5
01626 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01627 start = end
01628 end += 4
01629 (length,) = _struct_I.unpack(str[start:end])
01630 start = end
01631 end += length
01632 _v60.data = str[start:end]
01633 _v63 = _v56.disparity_image
01634 _v64 = _v63.header
01635 start = end
01636 end += 4
01637 (_v64.seq,) = _struct_I.unpack(str[start:end])
01638 _v65 = _v64.stamp
01639 _x = _v65
01640 start = end
01641 end += 8
01642 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01643 start = end
01644 end += 4
01645 (length,) = _struct_I.unpack(str[start:end])
01646 start = end
01647 end += length
01648 if python3:
01649 _v64.frame_id = str[start:end].decode('utf-8')
01650 else:
01651 _v64.frame_id = str[start:end]
01652 _x = _v63
01653 start = end
01654 end += 8
01655 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01656 start = end
01657 end += 4
01658 (length,) = _struct_I.unpack(str[start:end])
01659 start = end
01660 end += length
01661 if python3:
01662 _v63.encoding = str[start:end].decode('utf-8')
01663 else:
01664 _v63.encoding = str[start:end]
01665 _x = _v63
01666 start = end
01667 end += 5
01668 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01669 start = end
01670 end += 4
01671 (length,) = _struct_I.unpack(str[start:end])
01672 start = end
01673 end += length
01674 _v63.data = str[start:end]
01675 _v66 = _v56.cam_info
01676 _v67 = _v66.header
01677 start = end
01678 end += 4
01679 (_v67.seq,) = _struct_I.unpack(str[start:end])
01680 _v68 = _v67.stamp
01681 _x = _v68
01682 start = end
01683 end += 8
01684 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01685 start = end
01686 end += 4
01687 (length,) = _struct_I.unpack(str[start:end])
01688 start = end
01689 end += length
01690 if python3:
01691 _v67.frame_id = str[start:end].decode('utf-8')
01692 else:
01693 _v67.frame_id = str[start:end]
01694 _x = _v66
01695 start = end
01696 end += 8
01697 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01698 start = end
01699 end += 4
01700 (length,) = _struct_I.unpack(str[start:end])
01701 start = end
01702 end += length
01703 if python3:
01704 _v66.distortion_model = str[start:end].decode('utf-8')
01705 else:
01706 _v66.distortion_model = str[start:end]
01707 start = end
01708 end += 4
01709 (length,) = _struct_I.unpack(str[start:end])
01710 pattern = '<%sd'%length
01711 start = end
01712 end += struct.calcsize(pattern)
01713 _v66.D = struct.unpack(pattern, str[start:end])
01714 start = end
01715 end += 72
01716 _v66.K = _struct_9d.unpack(str[start:end])
01717 start = end
01718 end += 72
01719 _v66.R = _struct_9d.unpack(str[start:end])
01720 start = end
01721 end += 96
01722 _v66.P = _struct_12d.unpack(str[start:end])
01723 _x = _v66
01724 start = end
01725 end += 8
01726 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
01727 _v69 = _v66.roi
01728 _x = _v69
01729 start = end
01730 end += 17
01731 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
01732 _v69.do_rectify = bool(_v69.do_rectify)
01733 _v70 = _v56.roi_box_pose
01734 _v71 = _v70.header
01735 start = end
01736 end += 4
01737 (_v71.seq,) = _struct_I.unpack(str[start:end])
01738 _v72 = _v71.stamp
01739 _x = _v72
01740 start = end
01741 end += 8
01742 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01743 start = end
01744 end += 4
01745 (length,) = _struct_I.unpack(str[start:end])
01746 start = end
01747 end += length
01748 if python3:
01749 _v71.frame_id = str[start:end].decode('utf-8')
01750 else:
01751 _v71.frame_id = str[start:end]
01752 _v73 = _v70.pose
01753 _v74 = _v73.position
01754 _x = _v74
01755 start = end
01756 end += 24
01757 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01758 _v75 = _v73.orientation
01759 _x = _v75
01760 start = end
01761 end += 32
01762 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01763 _v76 = _v56.roi_box_dims
01764 _x = _v76
01765 start = end
01766 end += 24
01767 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01768 start = end
01769 end += 4
01770 (length,) = _struct_I.unpack(str[start:end])
01771 start = end
01772 end += length
01773 if python3:
01774 val1.collision_name = str[start:end].decode('utf-8')
01775 else:
01776 val1.collision_name = str[start:end]
01777 self.options.movable_obstacles.append(val1)
01778 _x = self
01779 start = end
01780 end += 34
01781 (_x.options.adv_options.reactive_grasping, _x.options.adv_options.reactive_force, _x.options.adv_options.reactive_place, _x.options.adv_options.lift_steps, _x.options.adv_options.retreat_steps, _x.options.adv_options.lift_direction_choice, _x.options.adv_options.desired_approach, _x.options.adv_options.min_approach, _x.options.adv_options.max_contact_force, _x.options.adv_options.find_alternatives, _x.options.adv_options.always_plan_grasps, _x.options.adv_options.cycle_gripper_opening, _x.command.command,) = _struct_3B5if3Bi.unpack(str[start:end])
01782 self.options.adv_options.reactive_grasping = bool(self.options.adv_options.reactive_grasping)
01783 self.options.adv_options.reactive_force = bool(self.options.adv_options.reactive_force)
01784 self.options.adv_options.reactive_place = bool(self.options.adv_options.reactive_place)
01785 self.options.adv_options.find_alternatives = bool(self.options.adv_options.find_alternatives)
01786 self.options.adv_options.always_plan_grasps = bool(self.options.adv_options.always_plan_grasps)
01787 self.options.adv_options.cycle_gripper_opening = bool(self.options.adv_options.cycle_gripper_opening)
01788 start = end
01789 end += 4
01790 (length,) = _struct_I.unpack(str[start:end])
01791 start = end
01792 end += length
01793 if python3:
01794 self.command.script_name = str[start:end].decode('utf-8')
01795 else:
01796 self.command.script_name = str[start:end]
01797 start = end
01798 end += 4
01799 (length,) = _struct_I.unpack(str[start:end])
01800 start = end
01801 end += length
01802 if python3:
01803 self.command.script_group_name = str[start:end].decode('utf-8')
01804 else:
01805 self.command.script_group_name = str[start:end]
01806 return self
01807 except struct.error as e:
01808 raise genpy.DeserializationError(e) #most likely buffer underfill
01809
01810
01811 def serialize_numpy(self, buff, numpy):
01812 """
01813 serialize message with numpy array types into buffer
01814 :param buff: buffer, ``StringIO``
01815 :param numpy: numpy python module
01816 """
01817 try:
01818 _x = self
01819 buff.write(_struct_B6i.pack(_x.options.collision_checked, _x.options.grasp_selection, _x.options.arm_selection, _x.options.reset_choice, _x.options.arm_action_choice, _x.options.arm_planner_choice, _x.options.gripper_slider_position))
01820 _x = self.options.selected_object.reference_frame_id
01821 length = len(_x)
01822 if python3 or type(_x) == unicode:
01823 _x = _x.encode('utf-8')
01824 length = len(_x)
01825 buff.write(struct.pack('<I%ss'%length, length, _x))
01826 length = len(self.options.selected_object.potential_models)
01827 buff.write(_struct_I.pack(length))
01828 for val1 in self.options.selected_object.potential_models:
01829 buff.write(_struct_i.pack(val1.model_id))
01830 _v77 = val1.type
01831 _x = _v77.key
01832 length = len(_x)
01833 if python3 or type(_x) == unicode:
01834 _x = _x.encode('utf-8')
01835 length = len(_x)
01836 buff.write(struct.pack('<I%ss'%length, length, _x))
01837 _x = _v77.db
01838 length = len(_x)
01839 if python3 or type(_x) == unicode:
01840 _x = _x.encode('utf-8')
01841 length = len(_x)
01842 buff.write(struct.pack('<I%ss'%length, length, _x))
01843 _v78 = val1.pose
01844 _v79 = _v78.header
01845 buff.write(_struct_I.pack(_v79.seq))
01846 _v80 = _v79.stamp
01847 _x = _v80
01848 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01849 _x = _v79.frame_id
01850 length = len(_x)
01851 if python3 or type(_x) == unicode:
01852 _x = _x.encode('utf-8')
01853 length = len(_x)
01854 buff.write(struct.pack('<I%ss'%length, length, _x))
01855 _v81 = _v78.pose
01856 _v82 = _v81.position
01857 _x = _v82
01858 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01859 _v83 = _v81.orientation
01860 _x = _v83
01861 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01862 buff.write(_struct_f.pack(val1.confidence))
01863 _x = val1.detector_name
01864 length = len(_x)
01865 if python3 or type(_x) == unicode:
01866 _x = _x.encode('utf-8')
01867 length = len(_x)
01868 buff.write(struct.pack('<I%ss'%length, length, _x))
01869 _x = self
01870 buff.write(_struct_3I.pack(_x.options.selected_object.cluster.header.seq, _x.options.selected_object.cluster.header.stamp.secs, _x.options.selected_object.cluster.header.stamp.nsecs))
01871 _x = self.options.selected_object.cluster.header.frame_id
01872 length = len(_x)
01873 if python3 or type(_x) == unicode:
01874 _x = _x.encode('utf-8')
01875 length = len(_x)
01876 buff.write(struct.pack('<I%ss'%length, length, _x))
01877 length = len(self.options.selected_object.cluster.points)
01878 buff.write(_struct_I.pack(length))
01879 for val1 in self.options.selected_object.cluster.points:
01880 _x = val1
01881 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01882 length = len(self.options.selected_object.cluster.channels)
01883 buff.write(_struct_I.pack(length))
01884 for val1 in self.options.selected_object.cluster.channels:
01885 _x = val1.name
01886 length = len(_x)
01887 if python3 or type(_x) == unicode:
01888 _x = _x.encode('utf-8')
01889 length = len(_x)
01890 buff.write(struct.pack('<I%ss'%length, length, _x))
01891 length = len(val1.values)
01892 buff.write(_struct_I.pack(length))
01893 pattern = '<%sf'%length
01894 buff.write(val1.values.tostring())
01895 _x = self
01896 buff.write(_struct_3I.pack(_x.options.selected_object.region.cloud.header.seq, _x.options.selected_object.region.cloud.header.stamp.secs, _x.options.selected_object.region.cloud.header.stamp.nsecs))
01897 _x = self.options.selected_object.region.cloud.header.frame_id
01898 length = len(_x)
01899 if python3 or type(_x) == unicode:
01900 _x = _x.encode('utf-8')
01901 length = len(_x)
01902 buff.write(struct.pack('<I%ss'%length, length, _x))
01903 _x = self
01904 buff.write(_struct_2I.pack(_x.options.selected_object.region.cloud.height, _x.options.selected_object.region.cloud.width))
01905 length = len(self.options.selected_object.region.cloud.fields)
01906 buff.write(_struct_I.pack(length))
01907 for val1 in self.options.selected_object.region.cloud.fields:
01908 _x = val1.name
01909 length = len(_x)
01910 if python3 or type(_x) == unicode:
01911 _x = _x.encode('utf-8')
01912 length = len(_x)
01913 buff.write(struct.pack('<I%ss'%length, length, _x))
01914 _x = val1
01915 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01916 _x = self
01917 buff.write(_struct_B2I.pack(_x.options.selected_object.region.cloud.is_bigendian, _x.options.selected_object.region.cloud.point_step, _x.options.selected_object.region.cloud.row_step))
01918 _x = self.options.selected_object.region.cloud.data
01919 length = len(_x)
01920 # - if encoded as a list instead, serialize as bytes instead of string
01921 if type(_x) in [list, tuple]:
01922 buff.write(struct.pack('<I%sB'%length, length, *_x))
01923 else:
01924 buff.write(struct.pack('<I%ss'%length, length, _x))
01925 buff.write(_struct_B.pack(self.options.selected_object.region.cloud.is_dense))
01926 length = len(self.options.selected_object.region.mask)
01927 buff.write(_struct_I.pack(length))
01928 pattern = '<%si'%length
01929 buff.write(self.options.selected_object.region.mask.tostring())
01930 _x = self
01931 buff.write(_struct_3I.pack(_x.options.selected_object.region.image.header.seq, _x.options.selected_object.region.image.header.stamp.secs, _x.options.selected_object.region.image.header.stamp.nsecs))
01932 _x = self.options.selected_object.region.image.header.frame_id
01933 length = len(_x)
01934 if python3 or type(_x) == unicode:
01935 _x = _x.encode('utf-8')
01936 length = len(_x)
01937 buff.write(struct.pack('<I%ss'%length, length, _x))
01938 _x = self
01939 buff.write(_struct_2I.pack(_x.options.selected_object.region.image.height, _x.options.selected_object.region.image.width))
01940 _x = self.options.selected_object.region.image.encoding
01941 length = len(_x)
01942 if python3 or type(_x) == unicode:
01943 _x = _x.encode('utf-8')
01944 length = len(_x)
01945 buff.write(struct.pack('<I%ss'%length, length, _x))
01946 _x = self
01947 buff.write(_struct_BI.pack(_x.options.selected_object.region.image.is_bigendian, _x.options.selected_object.region.image.step))
01948 _x = self.options.selected_object.region.image.data
01949 length = len(_x)
01950 # - if encoded as a list instead, serialize as bytes instead of string
01951 if type(_x) in [list, tuple]:
01952 buff.write(struct.pack('<I%sB'%length, length, *_x))
01953 else:
01954 buff.write(struct.pack('<I%ss'%length, length, _x))
01955 _x = self
01956 buff.write(_struct_3I.pack(_x.options.selected_object.region.disparity_image.header.seq, _x.options.selected_object.region.disparity_image.header.stamp.secs, _x.options.selected_object.region.disparity_image.header.stamp.nsecs))
01957 _x = self.options.selected_object.region.disparity_image.header.frame_id
01958 length = len(_x)
01959 if python3 or type(_x) == unicode:
01960 _x = _x.encode('utf-8')
01961 length = len(_x)
01962 buff.write(struct.pack('<I%ss'%length, length, _x))
01963 _x = self
01964 buff.write(_struct_2I.pack(_x.options.selected_object.region.disparity_image.height, _x.options.selected_object.region.disparity_image.width))
01965 _x = self.options.selected_object.region.disparity_image.encoding
01966 length = len(_x)
01967 if python3 or type(_x) == unicode:
01968 _x = _x.encode('utf-8')
01969 length = len(_x)
01970 buff.write(struct.pack('<I%ss'%length, length, _x))
01971 _x = self
01972 buff.write(_struct_BI.pack(_x.options.selected_object.region.disparity_image.is_bigendian, _x.options.selected_object.region.disparity_image.step))
01973 _x = self.options.selected_object.region.disparity_image.data
01974 length = len(_x)
01975 # - if encoded as a list instead, serialize as bytes instead of string
01976 if type(_x) in [list, tuple]:
01977 buff.write(struct.pack('<I%sB'%length, length, *_x))
01978 else:
01979 buff.write(struct.pack('<I%ss'%length, length, _x))
01980 _x = self
01981 buff.write(_struct_3I.pack(_x.options.selected_object.region.cam_info.header.seq, _x.options.selected_object.region.cam_info.header.stamp.secs, _x.options.selected_object.region.cam_info.header.stamp.nsecs))
01982 _x = self.options.selected_object.region.cam_info.header.frame_id
01983 length = len(_x)
01984 if python3 or type(_x) == unicode:
01985 _x = _x.encode('utf-8')
01986 length = len(_x)
01987 buff.write(struct.pack('<I%ss'%length, length, _x))
01988 _x = self
01989 buff.write(_struct_2I.pack(_x.options.selected_object.region.cam_info.height, _x.options.selected_object.region.cam_info.width))
01990 _x = self.options.selected_object.region.cam_info.distortion_model
01991 length = len(_x)
01992 if python3 or type(_x) == unicode:
01993 _x = _x.encode('utf-8')
01994 length = len(_x)
01995 buff.write(struct.pack('<I%ss'%length, length, _x))
01996 length = len(self.options.selected_object.region.cam_info.D)
01997 buff.write(_struct_I.pack(length))
01998 pattern = '<%sd'%length
01999 buff.write(self.options.selected_object.region.cam_info.D.tostring())
02000 buff.write(self.options.selected_object.region.cam_info.K.tostring())
02001 buff.write(self.options.selected_object.region.cam_info.R.tostring())
02002 buff.write(self.options.selected_object.region.cam_info.P.tostring())
02003 _x = self
02004 buff.write(_struct_6IB3I.pack(_x.options.selected_object.region.cam_info.binning_x, _x.options.selected_object.region.cam_info.binning_y, _x.options.selected_object.region.cam_info.roi.x_offset, _x.options.selected_object.region.cam_info.roi.y_offset, _x.options.selected_object.region.cam_info.roi.height, _x.options.selected_object.region.cam_info.roi.width, _x.options.selected_object.region.cam_info.roi.do_rectify, _x.options.selected_object.region.roi_box_pose.header.seq, _x.options.selected_object.region.roi_box_pose.header.stamp.secs, _x.options.selected_object.region.roi_box_pose.header.stamp.nsecs))
02005 _x = self.options.selected_object.region.roi_box_pose.header.frame_id
02006 length = len(_x)
02007 if python3 or type(_x) == unicode:
02008 _x = _x.encode('utf-8')
02009 length = len(_x)
02010 buff.write(struct.pack('<I%ss'%length, length, _x))
02011 _x = self
02012 buff.write(_struct_10d.pack(_x.options.selected_object.region.roi_box_pose.pose.position.x, _x.options.selected_object.region.roi_box_pose.pose.position.y, _x.options.selected_object.region.roi_box_pose.pose.position.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.x, _x.options.selected_object.region.roi_box_pose.pose.orientation.y, _x.options.selected_object.region.roi_box_pose.pose.orientation.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.w, _x.options.selected_object.region.roi_box_dims.x, _x.options.selected_object.region.roi_box_dims.y, _x.options.selected_object.region.roi_box_dims.z))
02013 _x = self.options.selected_object.collision_name
02014 length = len(_x)
02015 if python3 or type(_x) == unicode:
02016 _x = _x.encode('utf-8')
02017 length = len(_x)
02018 buff.write(struct.pack('<I%ss'%length, length, _x))
02019 length = len(self.options.movable_obstacles)
02020 buff.write(_struct_I.pack(length))
02021 for val1 in self.options.movable_obstacles:
02022 _x = val1.reference_frame_id
02023 length = len(_x)
02024 if python3 or type(_x) == unicode:
02025 _x = _x.encode('utf-8')
02026 length = len(_x)
02027 buff.write(struct.pack('<I%ss'%length, length, _x))
02028 length = len(val1.potential_models)
02029 buff.write(_struct_I.pack(length))
02030 for val2 in val1.potential_models:
02031 buff.write(_struct_i.pack(val2.model_id))
02032 _v84 = val2.type
02033 _x = _v84.key
02034 length = len(_x)
02035 if python3 or type(_x) == unicode:
02036 _x = _x.encode('utf-8')
02037 length = len(_x)
02038 buff.write(struct.pack('<I%ss'%length, length, _x))
02039 _x = _v84.db
02040 length = len(_x)
02041 if python3 or type(_x) == unicode:
02042 _x = _x.encode('utf-8')
02043 length = len(_x)
02044 buff.write(struct.pack('<I%ss'%length, length, _x))
02045 _v85 = val2.pose
02046 _v86 = _v85.header
02047 buff.write(_struct_I.pack(_v86.seq))
02048 _v87 = _v86.stamp
02049 _x = _v87
02050 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02051 _x = _v86.frame_id
02052 length = len(_x)
02053 if python3 or type(_x) == unicode:
02054 _x = _x.encode('utf-8')
02055 length = len(_x)
02056 buff.write(struct.pack('<I%ss'%length, length, _x))
02057 _v88 = _v85.pose
02058 _v89 = _v88.position
02059 _x = _v89
02060 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02061 _v90 = _v88.orientation
02062 _x = _v90
02063 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02064 buff.write(_struct_f.pack(val2.confidence))
02065 _x = val2.detector_name
02066 length = len(_x)
02067 if python3 or type(_x) == unicode:
02068 _x = _x.encode('utf-8')
02069 length = len(_x)
02070 buff.write(struct.pack('<I%ss'%length, length, _x))
02071 _v91 = val1.cluster
02072 _v92 = _v91.header
02073 buff.write(_struct_I.pack(_v92.seq))
02074 _v93 = _v92.stamp
02075 _x = _v93
02076 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02077 _x = _v92.frame_id
02078 length = len(_x)
02079 if python3 or type(_x) == unicode:
02080 _x = _x.encode('utf-8')
02081 length = len(_x)
02082 buff.write(struct.pack('<I%ss'%length, length, _x))
02083 length = len(_v91.points)
02084 buff.write(_struct_I.pack(length))
02085 for val3 in _v91.points:
02086 _x = val3
02087 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02088 length = len(_v91.channels)
02089 buff.write(_struct_I.pack(length))
02090 for val3 in _v91.channels:
02091 _x = val3.name
02092 length = len(_x)
02093 if python3 or type(_x) == unicode:
02094 _x = _x.encode('utf-8')
02095 length = len(_x)
02096 buff.write(struct.pack('<I%ss'%length, length, _x))
02097 length = len(val3.values)
02098 buff.write(_struct_I.pack(length))
02099 pattern = '<%sf'%length
02100 buff.write(val3.values.tostring())
02101 _v94 = val1.region
02102 _v95 = _v94.cloud
02103 _v96 = _v95.header
02104 buff.write(_struct_I.pack(_v96.seq))
02105 _v97 = _v96.stamp
02106 _x = _v97
02107 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02108 _x = _v96.frame_id
02109 length = len(_x)
02110 if python3 or type(_x) == unicode:
02111 _x = _x.encode('utf-8')
02112 length = len(_x)
02113 buff.write(struct.pack('<I%ss'%length, length, _x))
02114 _x = _v95
02115 buff.write(_struct_2I.pack(_x.height, _x.width))
02116 length = len(_v95.fields)
02117 buff.write(_struct_I.pack(length))
02118 for val4 in _v95.fields:
02119 _x = val4.name
02120 length = len(_x)
02121 if python3 or type(_x) == unicode:
02122 _x = _x.encode('utf-8')
02123 length = len(_x)
02124 buff.write(struct.pack('<I%ss'%length, length, _x))
02125 _x = val4
02126 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
02127 _x = _v95
02128 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
02129 _x = _v95.data
02130 length = len(_x)
02131 # - if encoded as a list instead, serialize as bytes instead of string
02132 if type(_x) in [list, tuple]:
02133 buff.write(struct.pack('<I%sB'%length, length, *_x))
02134 else:
02135 buff.write(struct.pack('<I%ss'%length, length, _x))
02136 buff.write(_struct_B.pack(_v95.is_dense))
02137 length = len(_v94.mask)
02138 buff.write(_struct_I.pack(length))
02139 pattern = '<%si'%length
02140 buff.write(_v94.mask.tostring())
02141 _v98 = _v94.image
02142 _v99 = _v98.header
02143 buff.write(_struct_I.pack(_v99.seq))
02144 _v100 = _v99.stamp
02145 _x = _v100
02146 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02147 _x = _v99.frame_id
02148 length = len(_x)
02149 if python3 or type(_x) == unicode:
02150 _x = _x.encode('utf-8')
02151 length = len(_x)
02152 buff.write(struct.pack('<I%ss'%length, length, _x))
02153 _x = _v98
02154 buff.write(_struct_2I.pack(_x.height, _x.width))
02155 _x = _v98.encoding
02156 length = len(_x)
02157 if python3 or type(_x) == unicode:
02158 _x = _x.encode('utf-8')
02159 length = len(_x)
02160 buff.write(struct.pack('<I%ss'%length, length, _x))
02161 _x = _v98
02162 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02163 _x = _v98.data
02164 length = len(_x)
02165 # - if encoded as a list instead, serialize as bytes instead of string
02166 if type(_x) in [list, tuple]:
02167 buff.write(struct.pack('<I%sB'%length, length, *_x))
02168 else:
02169 buff.write(struct.pack('<I%ss'%length, length, _x))
02170 _v101 = _v94.disparity_image
02171 _v102 = _v101.header
02172 buff.write(_struct_I.pack(_v102.seq))
02173 _v103 = _v102.stamp
02174 _x = _v103
02175 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02176 _x = _v102.frame_id
02177 length = len(_x)
02178 if python3 or type(_x) == unicode:
02179 _x = _x.encode('utf-8')
02180 length = len(_x)
02181 buff.write(struct.pack('<I%ss'%length, length, _x))
02182 _x = _v101
02183 buff.write(_struct_2I.pack(_x.height, _x.width))
02184 _x = _v101.encoding
02185 length = len(_x)
02186 if python3 or type(_x) == unicode:
02187 _x = _x.encode('utf-8')
02188 length = len(_x)
02189 buff.write(struct.pack('<I%ss'%length, length, _x))
02190 _x = _v101
02191 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02192 _x = _v101.data
02193 length = len(_x)
02194 # - if encoded as a list instead, serialize as bytes instead of string
02195 if type(_x) in [list, tuple]:
02196 buff.write(struct.pack('<I%sB'%length, length, *_x))
02197 else:
02198 buff.write(struct.pack('<I%ss'%length, length, _x))
02199 _v104 = _v94.cam_info
02200 _v105 = _v104.header
02201 buff.write(_struct_I.pack(_v105.seq))
02202 _v106 = _v105.stamp
02203 _x = _v106
02204 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02205 _x = _v105.frame_id
02206 length = len(_x)
02207 if python3 or type(_x) == unicode:
02208 _x = _x.encode('utf-8')
02209 length = len(_x)
02210 buff.write(struct.pack('<I%ss'%length, length, _x))
02211 _x = _v104
02212 buff.write(_struct_2I.pack(_x.height, _x.width))
02213 _x = _v104.distortion_model
02214 length = len(_x)
02215 if python3 or type(_x) == unicode:
02216 _x = _x.encode('utf-8')
02217 length = len(_x)
02218 buff.write(struct.pack('<I%ss'%length, length, _x))
02219 length = len(_v104.D)
02220 buff.write(_struct_I.pack(length))
02221 pattern = '<%sd'%length
02222 buff.write(_v104.D.tostring())
02223 buff.write(_v104.K.tostring())
02224 buff.write(_v104.R.tostring())
02225 buff.write(_v104.P.tostring())
02226 _x = _v104
02227 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
02228 _v107 = _v104.roi
02229 _x = _v107
02230 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
02231 _v108 = _v94.roi_box_pose
02232 _v109 = _v108.header
02233 buff.write(_struct_I.pack(_v109.seq))
02234 _v110 = _v109.stamp
02235 _x = _v110
02236 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02237 _x = _v109.frame_id
02238 length = len(_x)
02239 if python3 or type(_x) == unicode:
02240 _x = _x.encode('utf-8')
02241 length = len(_x)
02242 buff.write(struct.pack('<I%ss'%length, length, _x))
02243 _v111 = _v108.pose
02244 _v112 = _v111.position
02245 _x = _v112
02246 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02247 _v113 = _v111.orientation
02248 _x = _v113
02249 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02250 _v114 = _v94.roi_box_dims
02251 _x = _v114
02252 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02253 _x = val1.collision_name
02254 length = len(_x)
02255 if python3 or type(_x) == unicode:
02256 _x = _x.encode('utf-8')
02257 length = len(_x)
02258 buff.write(struct.pack('<I%ss'%length, length, _x))
02259 _x = self
02260 buff.write(_struct_3B5if3Bi.pack(_x.options.adv_options.reactive_grasping, _x.options.adv_options.reactive_force, _x.options.adv_options.reactive_place, _x.options.adv_options.lift_steps, _x.options.adv_options.retreat_steps, _x.options.adv_options.lift_direction_choice, _x.options.adv_options.desired_approach, _x.options.adv_options.min_approach, _x.options.adv_options.max_contact_force, _x.options.adv_options.find_alternatives, _x.options.adv_options.always_plan_grasps, _x.options.adv_options.cycle_gripper_opening, _x.command.command))
02261 _x = self.command.script_name
02262 length = len(_x)
02263 if python3 or type(_x) == unicode:
02264 _x = _x.encode('utf-8')
02265 length = len(_x)
02266 buff.write(struct.pack('<I%ss'%length, length, _x))
02267 _x = self.command.script_group_name
02268 length = len(_x)
02269 if python3 or type(_x) == unicode:
02270 _x = _x.encode('utf-8')
02271 length = len(_x)
02272 buff.write(struct.pack('<I%ss'%length, length, _x))
02273 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
02274 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
02275
02276 def deserialize_numpy(self, str, numpy):
02277 """
02278 unpack serialized message in str into this message instance using numpy for array types
02279 :param str: byte array of serialized message, ``str``
02280 :param numpy: numpy python module
02281 """
02282 try:
02283 if self.options is None:
02284 self.options = pr2_object_manipulation_msgs.msg.IMGUIOptions()
02285 if self.command is None:
02286 self.command = pr2_object_manipulation_msgs.msg.IMGUICommand()
02287 end = 0
02288 _x = self
02289 start = end
02290 end += 25
02291 (_x.options.collision_checked, _x.options.grasp_selection, _x.options.arm_selection, _x.options.reset_choice, _x.options.arm_action_choice, _x.options.arm_planner_choice, _x.options.gripper_slider_position,) = _struct_B6i.unpack(str[start:end])
02292 self.options.collision_checked = bool(self.options.collision_checked)
02293 start = end
02294 end += 4
02295 (length,) = _struct_I.unpack(str[start:end])
02296 start = end
02297 end += length
02298 if python3:
02299 self.options.selected_object.reference_frame_id = str[start:end].decode('utf-8')
02300 else:
02301 self.options.selected_object.reference_frame_id = str[start:end]
02302 start = end
02303 end += 4
02304 (length,) = _struct_I.unpack(str[start:end])
02305 self.options.selected_object.potential_models = []
02306 for i in range(0, length):
02307 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
02308 start = end
02309 end += 4
02310 (val1.model_id,) = _struct_i.unpack(str[start:end])
02311 _v115 = val1.type
02312 start = end
02313 end += 4
02314 (length,) = _struct_I.unpack(str[start:end])
02315 start = end
02316 end += length
02317 if python3:
02318 _v115.key = str[start:end].decode('utf-8')
02319 else:
02320 _v115.key = str[start:end]
02321 start = end
02322 end += 4
02323 (length,) = _struct_I.unpack(str[start:end])
02324 start = end
02325 end += length
02326 if python3:
02327 _v115.db = str[start:end].decode('utf-8')
02328 else:
02329 _v115.db = str[start:end]
02330 _v116 = val1.pose
02331 _v117 = _v116.header
02332 start = end
02333 end += 4
02334 (_v117.seq,) = _struct_I.unpack(str[start:end])
02335 _v118 = _v117.stamp
02336 _x = _v118
02337 start = end
02338 end += 8
02339 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02340 start = end
02341 end += 4
02342 (length,) = _struct_I.unpack(str[start:end])
02343 start = end
02344 end += length
02345 if python3:
02346 _v117.frame_id = str[start:end].decode('utf-8')
02347 else:
02348 _v117.frame_id = str[start:end]
02349 _v119 = _v116.pose
02350 _v120 = _v119.position
02351 _x = _v120
02352 start = end
02353 end += 24
02354 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02355 _v121 = _v119.orientation
02356 _x = _v121
02357 start = end
02358 end += 32
02359 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02360 start = end
02361 end += 4
02362 (val1.confidence,) = _struct_f.unpack(str[start:end])
02363 start = end
02364 end += 4
02365 (length,) = _struct_I.unpack(str[start:end])
02366 start = end
02367 end += length
02368 if python3:
02369 val1.detector_name = str[start:end].decode('utf-8')
02370 else:
02371 val1.detector_name = str[start:end]
02372 self.options.selected_object.potential_models.append(val1)
02373 _x = self
02374 start = end
02375 end += 12
02376 (_x.options.selected_object.cluster.header.seq, _x.options.selected_object.cluster.header.stamp.secs, _x.options.selected_object.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02377 start = end
02378 end += 4
02379 (length,) = _struct_I.unpack(str[start:end])
02380 start = end
02381 end += length
02382 if python3:
02383 self.options.selected_object.cluster.header.frame_id = str[start:end].decode('utf-8')
02384 else:
02385 self.options.selected_object.cluster.header.frame_id = str[start:end]
02386 start = end
02387 end += 4
02388 (length,) = _struct_I.unpack(str[start:end])
02389 self.options.selected_object.cluster.points = []
02390 for i in range(0, length):
02391 val1 = geometry_msgs.msg.Point32()
02392 _x = val1
02393 start = end
02394 end += 12
02395 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02396 self.options.selected_object.cluster.points.append(val1)
02397 start = end
02398 end += 4
02399 (length,) = _struct_I.unpack(str[start:end])
02400 self.options.selected_object.cluster.channels = []
02401 for i in range(0, length):
02402 val1 = sensor_msgs.msg.ChannelFloat32()
02403 start = end
02404 end += 4
02405 (length,) = _struct_I.unpack(str[start:end])
02406 start = end
02407 end += length
02408 if python3:
02409 val1.name = str[start:end].decode('utf-8')
02410 else:
02411 val1.name = str[start:end]
02412 start = end
02413 end += 4
02414 (length,) = _struct_I.unpack(str[start:end])
02415 pattern = '<%sf'%length
02416 start = end
02417 end += struct.calcsize(pattern)
02418 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
02419 self.options.selected_object.cluster.channels.append(val1)
02420 _x = self
02421 start = end
02422 end += 12
02423 (_x.options.selected_object.region.cloud.header.seq, _x.options.selected_object.region.cloud.header.stamp.secs, _x.options.selected_object.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02424 start = end
02425 end += 4
02426 (length,) = _struct_I.unpack(str[start:end])
02427 start = end
02428 end += length
02429 if python3:
02430 self.options.selected_object.region.cloud.header.frame_id = str[start:end].decode('utf-8')
02431 else:
02432 self.options.selected_object.region.cloud.header.frame_id = str[start:end]
02433 _x = self
02434 start = end
02435 end += 8
02436 (_x.options.selected_object.region.cloud.height, _x.options.selected_object.region.cloud.width,) = _struct_2I.unpack(str[start:end])
02437 start = end
02438 end += 4
02439 (length,) = _struct_I.unpack(str[start:end])
02440 self.options.selected_object.region.cloud.fields = []
02441 for i in range(0, length):
02442 val1 = sensor_msgs.msg.PointField()
02443 start = end
02444 end += 4
02445 (length,) = _struct_I.unpack(str[start:end])
02446 start = end
02447 end += length
02448 if python3:
02449 val1.name = str[start:end].decode('utf-8')
02450 else:
02451 val1.name = str[start:end]
02452 _x = val1
02453 start = end
02454 end += 9
02455 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02456 self.options.selected_object.region.cloud.fields.append(val1)
02457 _x = self
02458 start = end
02459 end += 9
02460 (_x.options.selected_object.region.cloud.is_bigendian, _x.options.selected_object.region.cloud.point_step, _x.options.selected_object.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
02461 self.options.selected_object.region.cloud.is_bigendian = bool(self.options.selected_object.region.cloud.is_bigendian)
02462 start = end
02463 end += 4
02464 (length,) = _struct_I.unpack(str[start:end])
02465 start = end
02466 end += length
02467 self.options.selected_object.region.cloud.data = str[start:end]
02468 start = end
02469 end += 1
02470 (self.options.selected_object.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
02471 self.options.selected_object.region.cloud.is_dense = bool(self.options.selected_object.region.cloud.is_dense)
02472 start = end
02473 end += 4
02474 (length,) = _struct_I.unpack(str[start:end])
02475 pattern = '<%si'%length
02476 start = end
02477 end += struct.calcsize(pattern)
02478 self.options.selected_object.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02479 _x = self
02480 start = end
02481 end += 12
02482 (_x.options.selected_object.region.image.header.seq, _x.options.selected_object.region.image.header.stamp.secs, _x.options.selected_object.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02483 start = end
02484 end += 4
02485 (length,) = _struct_I.unpack(str[start:end])
02486 start = end
02487 end += length
02488 if python3:
02489 self.options.selected_object.region.image.header.frame_id = str[start:end].decode('utf-8')
02490 else:
02491 self.options.selected_object.region.image.header.frame_id = str[start:end]
02492 _x = self
02493 start = end
02494 end += 8
02495 (_x.options.selected_object.region.image.height, _x.options.selected_object.region.image.width,) = _struct_2I.unpack(str[start:end])
02496 start = end
02497 end += 4
02498 (length,) = _struct_I.unpack(str[start:end])
02499 start = end
02500 end += length
02501 if python3:
02502 self.options.selected_object.region.image.encoding = str[start:end].decode('utf-8')
02503 else:
02504 self.options.selected_object.region.image.encoding = str[start:end]
02505 _x = self
02506 start = end
02507 end += 5
02508 (_x.options.selected_object.region.image.is_bigendian, _x.options.selected_object.region.image.step,) = _struct_BI.unpack(str[start:end])
02509 start = end
02510 end += 4
02511 (length,) = _struct_I.unpack(str[start:end])
02512 start = end
02513 end += length
02514 self.options.selected_object.region.image.data = str[start:end]
02515 _x = self
02516 start = end
02517 end += 12
02518 (_x.options.selected_object.region.disparity_image.header.seq, _x.options.selected_object.region.disparity_image.header.stamp.secs, _x.options.selected_object.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02519 start = end
02520 end += 4
02521 (length,) = _struct_I.unpack(str[start:end])
02522 start = end
02523 end += length
02524 if python3:
02525 self.options.selected_object.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
02526 else:
02527 self.options.selected_object.region.disparity_image.header.frame_id = str[start:end]
02528 _x = self
02529 start = end
02530 end += 8
02531 (_x.options.selected_object.region.disparity_image.height, _x.options.selected_object.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
02532 start = end
02533 end += 4
02534 (length,) = _struct_I.unpack(str[start:end])
02535 start = end
02536 end += length
02537 if python3:
02538 self.options.selected_object.region.disparity_image.encoding = str[start:end].decode('utf-8')
02539 else:
02540 self.options.selected_object.region.disparity_image.encoding = str[start:end]
02541 _x = self
02542 start = end
02543 end += 5
02544 (_x.options.selected_object.region.disparity_image.is_bigendian, _x.options.selected_object.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
02545 start = end
02546 end += 4
02547 (length,) = _struct_I.unpack(str[start:end])
02548 start = end
02549 end += length
02550 self.options.selected_object.region.disparity_image.data = str[start:end]
02551 _x = self
02552 start = end
02553 end += 12
02554 (_x.options.selected_object.region.cam_info.header.seq, _x.options.selected_object.region.cam_info.header.stamp.secs, _x.options.selected_object.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02555 start = end
02556 end += 4
02557 (length,) = _struct_I.unpack(str[start:end])
02558 start = end
02559 end += length
02560 if python3:
02561 self.options.selected_object.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
02562 else:
02563 self.options.selected_object.region.cam_info.header.frame_id = str[start:end]
02564 _x = self
02565 start = end
02566 end += 8
02567 (_x.options.selected_object.region.cam_info.height, _x.options.selected_object.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
02568 start = end
02569 end += 4
02570 (length,) = _struct_I.unpack(str[start:end])
02571 start = end
02572 end += length
02573 if python3:
02574 self.options.selected_object.region.cam_info.distortion_model = str[start:end].decode('utf-8')
02575 else:
02576 self.options.selected_object.region.cam_info.distortion_model = str[start:end]
02577 start = end
02578 end += 4
02579 (length,) = _struct_I.unpack(str[start:end])
02580 pattern = '<%sd'%length
02581 start = end
02582 end += struct.calcsize(pattern)
02583 self.options.selected_object.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02584 start = end
02585 end += 72
02586 self.options.selected_object.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02587 start = end
02588 end += 72
02589 self.options.selected_object.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02590 start = end
02591 end += 96
02592 self.options.selected_object.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
02593 _x = self
02594 start = end
02595 end += 37
02596 (_x.options.selected_object.region.cam_info.binning_x, _x.options.selected_object.region.cam_info.binning_y, _x.options.selected_object.region.cam_info.roi.x_offset, _x.options.selected_object.region.cam_info.roi.y_offset, _x.options.selected_object.region.cam_info.roi.height, _x.options.selected_object.region.cam_info.roi.width, _x.options.selected_object.region.cam_info.roi.do_rectify, _x.options.selected_object.region.roi_box_pose.header.seq, _x.options.selected_object.region.roi_box_pose.header.stamp.secs, _x.options.selected_object.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
02597 self.options.selected_object.region.cam_info.roi.do_rectify = bool(self.options.selected_object.region.cam_info.roi.do_rectify)
02598 start = end
02599 end += 4
02600 (length,) = _struct_I.unpack(str[start:end])
02601 start = end
02602 end += length
02603 if python3:
02604 self.options.selected_object.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
02605 else:
02606 self.options.selected_object.region.roi_box_pose.header.frame_id = str[start:end]
02607 _x = self
02608 start = end
02609 end += 80
02610 (_x.options.selected_object.region.roi_box_pose.pose.position.x, _x.options.selected_object.region.roi_box_pose.pose.position.y, _x.options.selected_object.region.roi_box_pose.pose.position.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.x, _x.options.selected_object.region.roi_box_pose.pose.orientation.y, _x.options.selected_object.region.roi_box_pose.pose.orientation.z, _x.options.selected_object.region.roi_box_pose.pose.orientation.w, _x.options.selected_object.region.roi_box_dims.x, _x.options.selected_object.region.roi_box_dims.y, _x.options.selected_object.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
02611 start = end
02612 end += 4
02613 (length,) = _struct_I.unpack(str[start:end])
02614 start = end
02615 end += length
02616 if python3:
02617 self.options.selected_object.collision_name = str[start:end].decode('utf-8')
02618 else:
02619 self.options.selected_object.collision_name = str[start:end]
02620 start = end
02621 end += 4
02622 (length,) = _struct_I.unpack(str[start:end])
02623 self.options.movable_obstacles = []
02624 for i in range(0, length):
02625 val1 = manipulation_msgs.msg.GraspableObject()
02626 start = end
02627 end += 4
02628 (length,) = _struct_I.unpack(str[start:end])
02629 start = end
02630 end += length
02631 if python3:
02632 val1.reference_frame_id = str[start:end].decode('utf-8')
02633 else:
02634 val1.reference_frame_id = str[start:end]
02635 start = end
02636 end += 4
02637 (length,) = _struct_I.unpack(str[start:end])
02638 val1.potential_models = []
02639 for i in range(0, length):
02640 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
02641 start = end
02642 end += 4
02643 (val2.model_id,) = _struct_i.unpack(str[start:end])
02644 _v122 = val2.type
02645 start = end
02646 end += 4
02647 (length,) = _struct_I.unpack(str[start:end])
02648 start = end
02649 end += length
02650 if python3:
02651 _v122.key = str[start:end].decode('utf-8')
02652 else:
02653 _v122.key = str[start:end]
02654 start = end
02655 end += 4
02656 (length,) = _struct_I.unpack(str[start:end])
02657 start = end
02658 end += length
02659 if python3:
02660 _v122.db = str[start:end].decode('utf-8')
02661 else:
02662 _v122.db = str[start:end]
02663 _v123 = val2.pose
02664 _v124 = _v123.header
02665 start = end
02666 end += 4
02667 (_v124.seq,) = _struct_I.unpack(str[start:end])
02668 _v125 = _v124.stamp
02669 _x = _v125
02670 start = end
02671 end += 8
02672 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02673 start = end
02674 end += 4
02675 (length,) = _struct_I.unpack(str[start:end])
02676 start = end
02677 end += length
02678 if python3:
02679 _v124.frame_id = str[start:end].decode('utf-8')
02680 else:
02681 _v124.frame_id = str[start:end]
02682 _v126 = _v123.pose
02683 _v127 = _v126.position
02684 _x = _v127
02685 start = end
02686 end += 24
02687 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02688 _v128 = _v126.orientation
02689 _x = _v128
02690 start = end
02691 end += 32
02692 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02693 start = end
02694 end += 4
02695 (val2.confidence,) = _struct_f.unpack(str[start:end])
02696 start = end
02697 end += 4
02698 (length,) = _struct_I.unpack(str[start:end])
02699 start = end
02700 end += length
02701 if python3:
02702 val2.detector_name = str[start:end].decode('utf-8')
02703 else:
02704 val2.detector_name = str[start:end]
02705 val1.potential_models.append(val2)
02706 _v129 = val1.cluster
02707 _v130 = _v129.header
02708 start = end
02709 end += 4
02710 (_v130.seq,) = _struct_I.unpack(str[start:end])
02711 _v131 = _v130.stamp
02712 _x = _v131
02713 start = end
02714 end += 8
02715 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02716 start = end
02717 end += 4
02718 (length,) = _struct_I.unpack(str[start:end])
02719 start = end
02720 end += length
02721 if python3:
02722 _v130.frame_id = str[start:end].decode('utf-8')
02723 else:
02724 _v130.frame_id = str[start:end]
02725 start = end
02726 end += 4
02727 (length,) = _struct_I.unpack(str[start:end])
02728 _v129.points = []
02729 for i in range(0, length):
02730 val3 = geometry_msgs.msg.Point32()
02731 _x = val3
02732 start = end
02733 end += 12
02734 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02735 _v129.points.append(val3)
02736 start = end
02737 end += 4
02738 (length,) = _struct_I.unpack(str[start:end])
02739 _v129.channels = []
02740 for i in range(0, length):
02741 val3 = sensor_msgs.msg.ChannelFloat32()
02742 start = end
02743 end += 4
02744 (length,) = _struct_I.unpack(str[start:end])
02745 start = end
02746 end += length
02747 if python3:
02748 val3.name = str[start:end].decode('utf-8')
02749 else:
02750 val3.name = str[start:end]
02751 start = end
02752 end += 4
02753 (length,) = _struct_I.unpack(str[start:end])
02754 pattern = '<%sf'%length
02755 start = end
02756 end += struct.calcsize(pattern)
02757 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
02758 _v129.channels.append(val3)
02759 _v132 = val1.region
02760 _v133 = _v132.cloud
02761 _v134 = _v133.header
02762 start = end
02763 end += 4
02764 (_v134.seq,) = _struct_I.unpack(str[start:end])
02765 _v135 = _v134.stamp
02766 _x = _v135
02767 start = end
02768 end += 8
02769 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02770 start = end
02771 end += 4
02772 (length,) = _struct_I.unpack(str[start:end])
02773 start = end
02774 end += length
02775 if python3:
02776 _v134.frame_id = str[start:end].decode('utf-8')
02777 else:
02778 _v134.frame_id = str[start:end]
02779 _x = _v133
02780 start = end
02781 end += 8
02782 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02783 start = end
02784 end += 4
02785 (length,) = _struct_I.unpack(str[start:end])
02786 _v133.fields = []
02787 for i in range(0, length):
02788 val4 = sensor_msgs.msg.PointField()
02789 start = end
02790 end += 4
02791 (length,) = _struct_I.unpack(str[start:end])
02792 start = end
02793 end += length
02794 if python3:
02795 val4.name = str[start:end].decode('utf-8')
02796 else:
02797 val4.name = str[start:end]
02798 _x = val4
02799 start = end
02800 end += 9
02801 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02802 _v133.fields.append(val4)
02803 _x = _v133
02804 start = end
02805 end += 9
02806 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
02807 _v133.is_bigendian = bool(_v133.is_bigendian)
02808 start = end
02809 end += 4
02810 (length,) = _struct_I.unpack(str[start:end])
02811 start = end
02812 end += length
02813 _v133.data = str[start:end]
02814 start = end
02815 end += 1
02816 (_v133.is_dense,) = _struct_B.unpack(str[start:end])
02817 _v133.is_dense = bool(_v133.is_dense)
02818 start = end
02819 end += 4
02820 (length,) = _struct_I.unpack(str[start:end])
02821 pattern = '<%si'%length
02822 start = end
02823 end += struct.calcsize(pattern)
02824 _v132.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02825 _v136 = _v132.image
02826 _v137 = _v136.header
02827 start = end
02828 end += 4
02829 (_v137.seq,) = _struct_I.unpack(str[start:end])
02830 _v138 = _v137.stamp
02831 _x = _v138
02832 start = end
02833 end += 8
02834 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02835 start = end
02836 end += 4
02837 (length,) = _struct_I.unpack(str[start:end])
02838 start = end
02839 end += length
02840 if python3:
02841 _v137.frame_id = str[start:end].decode('utf-8')
02842 else:
02843 _v137.frame_id = str[start:end]
02844 _x = _v136
02845 start = end
02846 end += 8
02847 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02848 start = end
02849 end += 4
02850 (length,) = _struct_I.unpack(str[start:end])
02851 start = end
02852 end += length
02853 if python3:
02854 _v136.encoding = str[start:end].decode('utf-8')
02855 else:
02856 _v136.encoding = str[start:end]
02857 _x = _v136
02858 start = end
02859 end += 5
02860 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02861 start = end
02862 end += 4
02863 (length,) = _struct_I.unpack(str[start:end])
02864 start = end
02865 end += length
02866 _v136.data = str[start:end]
02867 _v139 = _v132.disparity_image
02868 _v140 = _v139.header
02869 start = end
02870 end += 4
02871 (_v140.seq,) = _struct_I.unpack(str[start:end])
02872 _v141 = _v140.stamp
02873 _x = _v141
02874 start = end
02875 end += 8
02876 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02877 start = end
02878 end += 4
02879 (length,) = _struct_I.unpack(str[start:end])
02880 start = end
02881 end += length
02882 if python3:
02883 _v140.frame_id = str[start:end].decode('utf-8')
02884 else:
02885 _v140.frame_id = str[start:end]
02886 _x = _v139
02887 start = end
02888 end += 8
02889 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02890 start = end
02891 end += 4
02892 (length,) = _struct_I.unpack(str[start:end])
02893 start = end
02894 end += length
02895 if python3:
02896 _v139.encoding = str[start:end].decode('utf-8')
02897 else:
02898 _v139.encoding = str[start:end]
02899 _x = _v139
02900 start = end
02901 end += 5
02902 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02903 start = end
02904 end += 4
02905 (length,) = _struct_I.unpack(str[start:end])
02906 start = end
02907 end += length
02908 _v139.data = str[start:end]
02909 _v142 = _v132.cam_info
02910 _v143 = _v142.header
02911 start = end
02912 end += 4
02913 (_v143.seq,) = _struct_I.unpack(str[start:end])
02914 _v144 = _v143.stamp
02915 _x = _v144
02916 start = end
02917 end += 8
02918 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02919 start = end
02920 end += 4
02921 (length,) = _struct_I.unpack(str[start:end])
02922 start = end
02923 end += length
02924 if python3:
02925 _v143.frame_id = str[start:end].decode('utf-8')
02926 else:
02927 _v143.frame_id = str[start:end]
02928 _x = _v142
02929 start = end
02930 end += 8
02931 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02932 start = end
02933 end += 4
02934 (length,) = _struct_I.unpack(str[start:end])
02935 start = end
02936 end += length
02937 if python3:
02938 _v142.distortion_model = str[start:end].decode('utf-8')
02939 else:
02940 _v142.distortion_model = str[start:end]
02941 start = end
02942 end += 4
02943 (length,) = _struct_I.unpack(str[start:end])
02944 pattern = '<%sd'%length
02945 start = end
02946 end += struct.calcsize(pattern)
02947 _v142.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02948 start = end
02949 end += 72
02950 _v142.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02951 start = end
02952 end += 72
02953 _v142.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02954 start = end
02955 end += 96
02956 _v142.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
02957 _x = _v142
02958 start = end
02959 end += 8
02960 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
02961 _v145 = _v142.roi
02962 _x = _v145
02963 start = end
02964 end += 17
02965 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
02966 _v145.do_rectify = bool(_v145.do_rectify)
02967 _v146 = _v132.roi_box_pose
02968 _v147 = _v146.header
02969 start = end
02970 end += 4
02971 (_v147.seq,) = _struct_I.unpack(str[start:end])
02972 _v148 = _v147.stamp
02973 _x = _v148
02974 start = end
02975 end += 8
02976 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02977 start = end
02978 end += 4
02979 (length,) = _struct_I.unpack(str[start:end])
02980 start = end
02981 end += length
02982 if python3:
02983 _v147.frame_id = str[start:end].decode('utf-8')
02984 else:
02985 _v147.frame_id = str[start:end]
02986 _v149 = _v146.pose
02987 _v150 = _v149.position
02988 _x = _v150
02989 start = end
02990 end += 24
02991 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02992 _v151 = _v149.orientation
02993 _x = _v151
02994 start = end
02995 end += 32
02996 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02997 _v152 = _v132.roi_box_dims
02998 _x = _v152
02999 start = end
03000 end += 24
03001 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03002 start = end
03003 end += 4
03004 (length,) = _struct_I.unpack(str[start:end])
03005 start = end
03006 end += length
03007 if python3:
03008 val1.collision_name = str[start:end].decode('utf-8')
03009 else:
03010 val1.collision_name = str[start:end]
03011 self.options.movable_obstacles.append(val1)
03012 _x = self
03013 start = end
03014 end += 34
03015 (_x.options.adv_options.reactive_grasping, _x.options.adv_options.reactive_force, _x.options.adv_options.reactive_place, _x.options.adv_options.lift_steps, _x.options.adv_options.retreat_steps, _x.options.adv_options.lift_direction_choice, _x.options.adv_options.desired_approach, _x.options.adv_options.min_approach, _x.options.adv_options.max_contact_force, _x.options.adv_options.find_alternatives, _x.options.adv_options.always_plan_grasps, _x.options.adv_options.cycle_gripper_opening, _x.command.command,) = _struct_3B5if3Bi.unpack(str[start:end])
03016 self.options.adv_options.reactive_grasping = bool(self.options.adv_options.reactive_grasping)
03017 self.options.adv_options.reactive_force = bool(self.options.adv_options.reactive_force)
03018 self.options.adv_options.reactive_place = bool(self.options.adv_options.reactive_place)
03019 self.options.adv_options.find_alternatives = bool(self.options.adv_options.find_alternatives)
03020 self.options.adv_options.always_plan_grasps = bool(self.options.adv_options.always_plan_grasps)
03021 self.options.adv_options.cycle_gripper_opening = bool(self.options.adv_options.cycle_gripper_opening)
03022 start = end
03023 end += 4
03024 (length,) = _struct_I.unpack(str[start:end])
03025 start = end
03026 end += length
03027 if python3:
03028 self.command.script_name = str[start:end].decode('utf-8')
03029 else:
03030 self.command.script_name = str[start:end]
03031 start = end
03032 end += 4
03033 (length,) = _struct_I.unpack(str[start:end])
03034 start = end
03035 end += length
03036 if python3:
03037 self.command.script_group_name = str[start:end].decode('utf-8')
03038 else:
03039 self.command.script_group_name = str[start:end]
03040 return self
03041 except struct.error as e:
03042 raise genpy.DeserializationError(e) #most likely buffer underfill
03043
03044 _struct_I = genpy.struct_I
03045 _struct_IBI = struct.Struct("<IBI")
03046 _struct_6IB3I = struct.Struct("<6IB3I")
03047 _struct_B = struct.Struct("<B")
03048 _struct_12d = struct.Struct("<12d")
03049 _struct_f = struct.Struct("<f")
03050 _struct_i = struct.Struct("<i")
03051 _struct_3f = struct.Struct("<3f")
03052 _struct_BI = struct.Struct("<BI")
03053 _struct_10d = struct.Struct("<10d")
03054 _struct_2I = struct.Struct("<2I")
03055 _struct_3I = struct.Struct("<3I")
03056 _struct_9d = struct.Struct("<9d")
03057 _struct_B2I = struct.Struct("<B2I")
03058 _struct_B6i = struct.Struct("<B6i")
03059 _struct_3B5if3Bi = struct.Struct("<3B5if3Bi")
03060 _struct_4d = struct.Struct("<4d")
03061 _struct_4IB = struct.Struct("<4IB")
03062 _struct_3d = struct.Struct("<3d")