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