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