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