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