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