$search
00001 """autogenerated by genmsg_py from GraspPlanningFeedback.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 GraspPlanningFeedback(roslib.message.Message): 00012 _md5sum = "7545a4f0b7d645678d5ba66709912a4b" 00013 _type = "object_manipulation_msgs/GraspPlanningFeedback" 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 # grasps planned so far 00018 Grasp[] grasps 00019 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 __slots__ = ['grasps'] 00495 _slot_types = ['object_manipulation_msgs/Grasp[]'] 00496 00497 def __init__(self, *args, **kwds): 00498 """ 00499 Constructor. Any message fields that are implicitly/explicitly 00500 set to None will be assigned a default value. The recommend 00501 use is keyword arguments as this is more robust to future message 00502 changes. You cannot mix in-order arguments and keyword arguments. 00503 00504 The available fields are: 00505 grasps 00506 00507 @param args: complete set of field values, in .msg order 00508 @param kwds: use keyword arguments corresponding to message field names 00509 to set specific fields. 00510 """ 00511 if args or kwds: 00512 super(GraspPlanningFeedback, self).__init__(*args, **kwds) 00513 #message fields cannot be None, assign default values for those that are 00514 if self.grasps is None: 00515 self.grasps = [] 00516 else: 00517 self.grasps = [] 00518 00519 def _get_types(self): 00520 """ 00521 internal API method 00522 """ 00523 return self._slot_types 00524 00525 def serialize(self, buff): 00526 """ 00527 serialize message into buffer 00528 @param buff: buffer 00529 @type buff: StringIO 00530 """ 00531 try: 00532 length = len(self.grasps) 00533 buff.write(_struct_I.pack(length)) 00534 for val1 in self.grasps: 00535 _v1 = val1.pre_grasp_posture 00536 _v2 = _v1.header 00537 buff.write(_struct_I.pack(_v2.seq)) 00538 _v3 = _v2.stamp 00539 _x = _v3 00540 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00541 _x = _v2.frame_id 00542 length = len(_x) 00543 buff.write(struct.pack('<I%ss'%length, length, _x)) 00544 length = len(_v1.name) 00545 buff.write(_struct_I.pack(length)) 00546 for val3 in _v1.name: 00547 length = len(val3) 00548 buff.write(struct.pack('<I%ss'%length, length, val3)) 00549 length = len(_v1.position) 00550 buff.write(_struct_I.pack(length)) 00551 pattern = '<%sd'%length 00552 buff.write(struct.pack(pattern, *_v1.position)) 00553 length = len(_v1.velocity) 00554 buff.write(_struct_I.pack(length)) 00555 pattern = '<%sd'%length 00556 buff.write(struct.pack(pattern, *_v1.velocity)) 00557 length = len(_v1.effort) 00558 buff.write(_struct_I.pack(length)) 00559 pattern = '<%sd'%length 00560 buff.write(struct.pack(pattern, *_v1.effort)) 00561 _v4 = val1.grasp_posture 00562 _v5 = _v4.header 00563 buff.write(_struct_I.pack(_v5.seq)) 00564 _v6 = _v5.stamp 00565 _x = _v6 00566 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00567 _x = _v5.frame_id 00568 length = len(_x) 00569 buff.write(struct.pack('<I%ss'%length, length, _x)) 00570 length = len(_v4.name) 00571 buff.write(_struct_I.pack(length)) 00572 for val3 in _v4.name: 00573 length = len(val3) 00574 buff.write(struct.pack('<I%ss'%length, length, val3)) 00575 length = len(_v4.position) 00576 buff.write(_struct_I.pack(length)) 00577 pattern = '<%sd'%length 00578 buff.write(struct.pack(pattern, *_v4.position)) 00579 length = len(_v4.velocity) 00580 buff.write(_struct_I.pack(length)) 00581 pattern = '<%sd'%length 00582 buff.write(struct.pack(pattern, *_v4.velocity)) 00583 length = len(_v4.effort) 00584 buff.write(_struct_I.pack(length)) 00585 pattern = '<%sd'%length 00586 buff.write(struct.pack(pattern, *_v4.effort)) 00587 _v7 = val1.grasp_pose 00588 _v8 = _v7.position 00589 _x = _v8 00590 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00591 _v9 = _v7.orientation 00592 _x = _v9 00593 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00594 _x = val1 00595 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 00596 length = len(val1.moved_obstacles) 00597 buff.write(_struct_I.pack(length)) 00598 for val2 in val1.moved_obstacles: 00599 _x = val2.reference_frame_id 00600 length = len(_x) 00601 buff.write(struct.pack('<I%ss'%length, length, _x)) 00602 length = len(val2.potential_models) 00603 buff.write(_struct_I.pack(length)) 00604 for val3 in val2.potential_models: 00605 buff.write(_struct_i.pack(val3.model_id)) 00606 _v10 = val3.pose 00607 _v11 = _v10.header 00608 buff.write(_struct_I.pack(_v11.seq)) 00609 _v12 = _v11.stamp 00610 _x = _v12 00611 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00612 _x = _v11.frame_id 00613 length = len(_x) 00614 buff.write(struct.pack('<I%ss'%length, length, _x)) 00615 _v13 = _v10.pose 00616 _v14 = _v13.position 00617 _x = _v14 00618 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00619 _v15 = _v13.orientation 00620 _x = _v15 00621 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00622 buff.write(_struct_f.pack(val3.confidence)) 00623 _x = val3.detector_name 00624 length = len(_x) 00625 buff.write(struct.pack('<I%ss'%length, length, _x)) 00626 _v16 = val2.cluster 00627 _v17 = _v16.header 00628 buff.write(_struct_I.pack(_v17.seq)) 00629 _v18 = _v17.stamp 00630 _x = _v18 00631 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00632 _x = _v17.frame_id 00633 length = len(_x) 00634 buff.write(struct.pack('<I%ss'%length, length, _x)) 00635 length = len(_v16.points) 00636 buff.write(_struct_I.pack(length)) 00637 for val4 in _v16.points: 00638 _x = val4 00639 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00640 length = len(_v16.channels) 00641 buff.write(_struct_I.pack(length)) 00642 for val4 in _v16.channels: 00643 _x = val4.name 00644 length = len(_x) 00645 buff.write(struct.pack('<I%ss'%length, length, _x)) 00646 length = len(val4.values) 00647 buff.write(_struct_I.pack(length)) 00648 pattern = '<%sf'%length 00649 buff.write(struct.pack(pattern, *val4.values)) 00650 _v19 = val2.region 00651 _v20 = _v19.cloud 00652 _v21 = _v20.header 00653 buff.write(_struct_I.pack(_v21.seq)) 00654 _v22 = _v21.stamp 00655 _x = _v22 00656 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00657 _x = _v21.frame_id 00658 length = len(_x) 00659 buff.write(struct.pack('<I%ss'%length, length, _x)) 00660 _x = _v20 00661 buff.write(_struct_2I.pack(_x.height, _x.width)) 00662 length = len(_v20.fields) 00663 buff.write(_struct_I.pack(length)) 00664 for val5 in _v20.fields: 00665 _x = val5.name 00666 length = len(_x) 00667 buff.write(struct.pack('<I%ss'%length, length, _x)) 00668 _x = val5 00669 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00670 _x = _v20 00671 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 00672 _x = _v20.data 00673 length = len(_x) 00674 # - if encoded as a list instead, serialize as bytes instead of string 00675 if type(_x) in [list, tuple]: 00676 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00677 else: 00678 buff.write(struct.pack('<I%ss'%length, length, _x)) 00679 buff.write(_struct_B.pack(_v20.is_dense)) 00680 length = len(_v19.mask) 00681 buff.write(_struct_I.pack(length)) 00682 pattern = '<%si'%length 00683 buff.write(struct.pack(pattern, *_v19.mask)) 00684 _v23 = _v19.image 00685 _v24 = _v23.header 00686 buff.write(_struct_I.pack(_v24.seq)) 00687 _v25 = _v24.stamp 00688 _x = _v25 00689 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00690 _x = _v24.frame_id 00691 length = len(_x) 00692 buff.write(struct.pack('<I%ss'%length, length, _x)) 00693 _x = _v23 00694 buff.write(_struct_2I.pack(_x.height, _x.width)) 00695 _x = _v23.encoding 00696 length = len(_x) 00697 buff.write(struct.pack('<I%ss'%length, length, _x)) 00698 _x = _v23 00699 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00700 _x = _v23.data 00701 length = len(_x) 00702 # - if encoded as a list instead, serialize as bytes instead of string 00703 if type(_x) in [list, tuple]: 00704 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00705 else: 00706 buff.write(struct.pack('<I%ss'%length, length, _x)) 00707 _v26 = _v19.disparity_image 00708 _v27 = _v26.header 00709 buff.write(_struct_I.pack(_v27.seq)) 00710 _v28 = _v27.stamp 00711 _x = _v28 00712 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00713 _x = _v27.frame_id 00714 length = len(_x) 00715 buff.write(struct.pack('<I%ss'%length, length, _x)) 00716 _x = _v26 00717 buff.write(_struct_2I.pack(_x.height, _x.width)) 00718 _x = _v26.encoding 00719 length = len(_x) 00720 buff.write(struct.pack('<I%ss'%length, length, _x)) 00721 _x = _v26 00722 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00723 _x = _v26.data 00724 length = len(_x) 00725 # - if encoded as a list instead, serialize as bytes instead of string 00726 if type(_x) in [list, tuple]: 00727 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00728 else: 00729 buff.write(struct.pack('<I%ss'%length, length, _x)) 00730 _v29 = _v19.cam_info 00731 _v30 = _v29.header 00732 buff.write(_struct_I.pack(_v30.seq)) 00733 _v31 = _v30.stamp 00734 _x = _v31 00735 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00736 _x = _v30.frame_id 00737 length = len(_x) 00738 buff.write(struct.pack('<I%ss'%length, length, _x)) 00739 _x = _v29 00740 buff.write(_struct_2I.pack(_x.height, _x.width)) 00741 _x = _v29.distortion_model 00742 length = len(_x) 00743 buff.write(struct.pack('<I%ss'%length, length, _x)) 00744 length = len(_v29.D) 00745 buff.write(_struct_I.pack(length)) 00746 pattern = '<%sd'%length 00747 buff.write(struct.pack(pattern, *_v29.D)) 00748 buff.write(_struct_9d.pack(*_v29.K)) 00749 buff.write(_struct_9d.pack(*_v29.R)) 00750 buff.write(_struct_12d.pack(*_v29.P)) 00751 _x = _v29 00752 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00753 _v32 = _v29.roi 00754 _x = _v32 00755 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00756 _v33 = _v19.roi_box_pose 00757 _v34 = _v33.header 00758 buff.write(_struct_I.pack(_v34.seq)) 00759 _v35 = _v34.stamp 00760 _x = _v35 00761 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00762 _x = _v34.frame_id 00763 length = len(_x) 00764 buff.write(struct.pack('<I%ss'%length, length, _x)) 00765 _v36 = _v33.pose 00766 _v37 = _v36.position 00767 _x = _v37 00768 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00769 _v38 = _v36.orientation 00770 _x = _v38 00771 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00772 _v39 = _v19.roi_box_dims 00773 _x = _v39 00774 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00775 _x = val2.collision_name 00776 length = len(_x) 00777 buff.write(struct.pack('<I%ss'%length, length, _x)) 00778 except struct.error as se: self._check_types(se) 00779 except TypeError as te: self._check_types(te) 00780 00781 def deserialize(self, str): 00782 """ 00783 unpack serialized message in str into this message instance 00784 @param str: byte array of serialized message 00785 @type str: str 00786 """ 00787 try: 00788 end = 0 00789 start = end 00790 end += 4 00791 (length,) = _struct_I.unpack(str[start:end]) 00792 self.grasps = [] 00793 for i in range(0, length): 00794 val1 = object_manipulation_msgs.msg.Grasp() 00795 _v40 = val1.pre_grasp_posture 00796 _v41 = _v40.header 00797 start = end 00798 end += 4 00799 (_v41.seq,) = _struct_I.unpack(str[start:end]) 00800 _v42 = _v41.stamp 00801 _x = _v42 00802 start = end 00803 end += 8 00804 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00805 start = end 00806 end += 4 00807 (length,) = _struct_I.unpack(str[start:end]) 00808 start = end 00809 end += length 00810 _v41.frame_id = str[start:end] 00811 start = end 00812 end += 4 00813 (length,) = _struct_I.unpack(str[start:end]) 00814 _v40.name = [] 00815 for i in range(0, length): 00816 start = end 00817 end += 4 00818 (length,) = _struct_I.unpack(str[start:end]) 00819 start = end 00820 end += length 00821 val3 = str[start:end] 00822 _v40.name.append(val3) 00823 start = end 00824 end += 4 00825 (length,) = _struct_I.unpack(str[start:end]) 00826 pattern = '<%sd'%length 00827 start = end 00828 end += struct.calcsize(pattern) 00829 _v40.position = struct.unpack(pattern, str[start:end]) 00830 start = end 00831 end += 4 00832 (length,) = _struct_I.unpack(str[start:end]) 00833 pattern = '<%sd'%length 00834 start = end 00835 end += struct.calcsize(pattern) 00836 _v40.velocity = struct.unpack(pattern, str[start:end]) 00837 start = end 00838 end += 4 00839 (length,) = _struct_I.unpack(str[start:end]) 00840 pattern = '<%sd'%length 00841 start = end 00842 end += struct.calcsize(pattern) 00843 _v40.effort = struct.unpack(pattern, str[start:end]) 00844 _v43 = val1.grasp_posture 00845 _v44 = _v43.header 00846 start = end 00847 end += 4 00848 (_v44.seq,) = _struct_I.unpack(str[start:end]) 00849 _v45 = _v44.stamp 00850 _x = _v45 00851 start = end 00852 end += 8 00853 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00854 start = end 00855 end += 4 00856 (length,) = _struct_I.unpack(str[start:end]) 00857 start = end 00858 end += length 00859 _v44.frame_id = str[start:end] 00860 start = end 00861 end += 4 00862 (length,) = _struct_I.unpack(str[start:end]) 00863 _v43.name = [] 00864 for i in range(0, length): 00865 start = end 00866 end += 4 00867 (length,) = _struct_I.unpack(str[start:end]) 00868 start = end 00869 end += length 00870 val3 = str[start:end] 00871 _v43.name.append(val3) 00872 start = end 00873 end += 4 00874 (length,) = _struct_I.unpack(str[start:end]) 00875 pattern = '<%sd'%length 00876 start = end 00877 end += struct.calcsize(pattern) 00878 _v43.position = struct.unpack(pattern, str[start:end]) 00879 start = end 00880 end += 4 00881 (length,) = _struct_I.unpack(str[start:end]) 00882 pattern = '<%sd'%length 00883 start = end 00884 end += struct.calcsize(pattern) 00885 _v43.velocity = struct.unpack(pattern, str[start:end]) 00886 start = end 00887 end += 4 00888 (length,) = _struct_I.unpack(str[start:end]) 00889 pattern = '<%sd'%length 00890 start = end 00891 end += struct.calcsize(pattern) 00892 _v43.effort = struct.unpack(pattern, str[start:end]) 00893 _v46 = val1.grasp_pose 00894 _v47 = _v46.position 00895 _x = _v47 00896 start = end 00897 end += 24 00898 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00899 _v48 = _v46.orientation 00900 _x = _v48 00901 start = end 00902 end += 32 00903 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00904 _x = val1 00905 start = end 00906 end += 17 00907 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 00908 val1.cluster_rep = bool(val1.cluster_rep) 00909 start = end 00910 end += 4 00911 (length,) = _struct_I.unpack(str[start:end]) 00912 val1.moved_obstacles = [] 00913 for i in range(0, length): 00914 val2 = object_manipulation_msgs.msg.GraspableObject() 00915 start = end 00916 end += 4 00917 (length,) = _struct_I.unpack(str[start:end]) 00918 start = end 00919 end += length 00920 val2.reference_frame_id = str[start:end] 00921 start = end 00922 end += 4 00923 (length,) = _struct_I.unpack(str[start:end]) 00924 val2.potential_models = [] 00925 for i in range(0, length): 00926 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 00927 start = end 00928 end += 4 00929 (val3.model_id,) = _struct_i.unpack(str[start:end]) 00930 _v49 = val3.pose 00931 _v50 = _v49.header 00932 start = end 00933 end += 4 00934 (_v50.seq,) = _struct_I.unpack(str[start:end]) 00935 _v51 = _v50.stamp 00936 _x = _v51 00937 start = end 00938 end += 8 00939 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00940 start = end 00941 end += 4 00942 (length,) = _struct_I.unpack(str[start:end]) 00943 start = end 00944 end += length 00945 _v50.frame_id = str[start:end] 00946 _v52 = _v49.pose 00947 _v53 = _v52.position 00948 _x = _v53 00949 start = end 00950 end += 24 00951 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00952 _v54 = _v52.orientation 00953 _x = _v54 00954 start = end 00955 end += 32 00956 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00957 start = end 00958 end += 4 00959 (val3.confidence,) = _struct_f.unpack(str[start:end]) 00960 start = end 00961 end += 4 00962 (length,) = _struct_I.unpack(str[start:end]) 00963 start = end 00964 end += length 00965 val3.detector_name = str[start:end] 00966 val2.potential_models.append(val3) 00967 _v55 = val2.cluster 00968 _v56 = _v55.header 00969 start = end 00970 end += 4 00971 (_v56.seq,) = _struct_I.unpack(str[start:end]) 00972 _v57 = _v56.stamp 00973 _x = _v57 00974 start = end 00975 end += 8 00976 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00977 start = end 00978 end += 4 00979 (length,) = _struct_I.unpack(str[start:end]) 00980 start = end 00981 end += length 00982 _v56.frame_id = str[start:end] 00983 start = end 00984 end += 4 00985 (length,) = _struct_I.unpack(str[start:end]) 00986 _v55.points = [] 00987 for i in range(0, length): 00988 val4 = geometry_msgs.msg.Point32() 00989 _x = val4 00990 start = end 00991 end += 12 00992 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00993 _v55.points.append(val4) 00994 start = end 00995 end += 4 00996 (length,) = _struct_I.unpack(str[start:end]) 00997 _v55.channels = [] 00998 for i in range(0, length): 00999 val4 = sensor_msgs.msg.ChannelFloat32() 01000 start = end 01001 end += 4 01002 (length,) = _struct_I.unpack(str[start:end]) 01003 start = end 01004 end += length 01005 val4.name = str[start:end] 01006 start = end 01007 end += 4 01008 (length,) = _struct_I.unpack(str[start:end]) 01009 pattern = '<%sf'%length 01010 start = end 01011 end += struct.calcsize(pattern) 01012 val4.values = struct.unpack(pattern, str[start:end]) 01013 _v55.channels.append(val4) 01014 _v58 = val2.region 01015 _v59 = _v58.cloud 01016 _v60 = _v59.header 01017 start = end 01018 end += 4 01019 (_v60.seq,) = _struct_I.unpack(str[start:end]) 01020 _v61 = _v60.stamp 01021 _x = _v61 01022 start = end 01023 end += 8 01024 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01025 start = end 01026 end += 4 01027 (length,) = _struct_I.unpack(str[start:end]) 01028 start = end 01029 end += length 01030 _v60.frame_id = str[start:end] 01031 _x = _v59 01032 start = end 01033 end += 8 01034 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01035 start = end 01036 end += 4 01037 (length,) = _struct_I.unpack(str[start:end]) 01038 _v59.fields = [] 01039 for i in range(0, length): 01040 val5 = sensor_msgs.msg.PointField() 01041 start = end 01042 end += 4 01043 (length,) = _struct_I.unpack(str[start:end]) 01044 start = end 01045 end += length 01046 val5.name = str[start:end] 01047 _x = val5 01048 start = end 01049 end += 9 01050 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01051 _v59.fields.append(val5) 01052 _x = _v59 01053 start = end 01054 end += 9 01055 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01056 _v59.is_bigendian = bool(_v59.is_bigendian) 01057 start = end 01058 end += 4 01059 (length,) = _struct_I.unpack(str[start:end]) 01060 start = end 01061 end += length 01062 _v59.data = str[start:end] 01063 start = end 01064 end += 1 01065 (_v59.is_dense,) = _struct_B.unpack(str[start:end]) 01066 _v59.is_dense = bool(_v59.is_dense) 01067 start = end 01068 end += 4 01069 (length,) = _struct_I.unpack(str[start:end]) 01070 pattern = '<%si'%length 01071 start = end 01072 end += struct.calcsize(pattern) 01073 _v58.mask = struct.unpack(pattern, str[start:end]) 01074 _v62 = _v58.image 01075 _v63 = _v62.header 01076 start = end 01077 end += 4 01078 (_v63.seq,) = _struct_I.unpack(str[start:end]) 01079 _v64 = _v63.stamp 01080 _x = _v64 01081 start = end 01082 end += 8 01083 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01084 start = end 01085 end += 4 01086 (length,) = _struct_I.unpack(str[start:end]) 01087 start = end 01088 end += length 01089 _v63.frame_id = str[start:end] 01090 _x = _v62 01091 start = end 01092 end += 8 01093 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01094 start = end 01095 end += 4 01096 (length,) = _struct_I.unpack(str[start:end]) 01097 start = end 01098 end += length 01099 _v62.encoding = str[start:end] 01100 _x = _v62 01101 start = end 01102 end += 5 01103 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01104 start = end 01105 end += 4 01106 (length,) = _struct_I.unpack(str[start:end]) 01107 start = end 01108 end += length 01109 _v62.data = str[start:end] 01110 _v65 = _v58.disparity_image 01111 _v66 = _v65.header 01112 start = end 01113 end += 4 01114 (_v66.seq,) = _struct_I.unpack(str[start:end]) 01115 _v67 = _v66.stamp 01116 _x = _v67 01117 start = end 01118 end += 8 01119 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01120 start = end 01121 end += 4 01122 (length,) = _struct_I.unpack(str[start:end]) 01123 start = end 01124 end += length 01125 _v66.frame_id = str[start:end] 01126 _x = _v65 01127 start = end 01128 end += 8 01129 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01130 start = end 01131 end += 4 01132 (length,) = _struct_I.unpack(str[start:end]) 01133 start = end 01134 end += length 01135 _v65.encoding = str[start:end] 01136 _x = _v65 01137 start = end 01138 end += 5 01139 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01140 start = end 01141 end += 4 01142 (length,) = _struct_I.unpack(str[start:end]) 01143 start = end 01144 end += length 01145 _v65.data = str[start:end] 01146 _v68 = _v58.cam_info 01147 _v69 = _v68.header 01148 start = end 01149 end += 4 01150 (_v69.seq,) = _struct_I.unpack(str[start:end]) 01151 _v70 = _v69.stamp 01152 _x = _v70 01153 start = end 01154 end += 8 01155 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01156 start = end 01157 end += 4 01158 (length,) = _struct_I.unpack(str[start:end]) 01159 start = end 01160 end += length 01161 _v69.frame_id = str[start:end] 01162 _x = _v68 01163 start = end 01164 end += 8 01165 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01166 start = end 01167 end += 4 01168 (length,) = _struct_I.unpack(str[start:end]) 01169 start = end 01170 end += length 01171 _v68.distortion_model = str[start:end] 01172 start = end 01173 end += 4 01174 (length,) = _struct_I.unpack(str[start:end]) 01175 pattern = '<%sd'%length 01176 start = end 01177 end += struct.calcsize(pattern) 01178 _v68.D = struct.unpack(pattern, str[start:end]) 01179 start = end 01180 end += 72 01181 _v68.K = _struct_9d.unpack(str[start:end]) 01182 start = end 01183 end += 72 01184 _v68.R = _struct_9d.unpack(str[start:end]) 01185 start = end 01186 end += 96 01187 _v68.P = _struct_12d.unpack(str[start:end]) 01188 _x = _v68 01189 start = end 01190 end += 8 01191 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01192 _v71 = _v68.roi 01193 _x = _v71 01194 start = end 01195 end += 17 01196 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01197 _v71.do_rectify = bool(_v71.do_rectify) 01198 _v72 = _v58.roi_box_pose 01199 _v73 = _v72.header 01200 start = end 01201 end += 4 01202 (_v73.seq,) = _struct_I.unpack(str[start:end]) 01203 _v74 = _v73.stamp 01204 _x = _v74 01205 start = end 01206 end += 8 01207 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01208 start = end 01209 end += 4 01210 (length,) = _struct_I.unpack(str[start:end]) 01211 start = end 01212 end += length 01213 _v73.frame_id = str[start:end] 01214 _v75 = _v72.pose 01215 _v76 = _v75.position 01216 _x = _v76 01217 start = end 01218 end += 24 01219 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01220 _v77 = _v75.orientation 01221 _x = _v77 01222 start = end 01223 end += 32 01224 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01225 _v78 = _v58.roi_box_dims 01226 _x = _v78 01227 start = end 01228 end += 24 01229 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01230 start = end 01231 end += 4 01232 (length,) = _struct_I.unpack(str[start:end]) 01233 start = end 01234 end += length 01235 val2.collision_name = str[start:end] 01236 val1.moved_obstacles.append(val2) 01237 self.grasps.append(val1) 01238 return self 01239 except struct.error as e: 01240 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01241 01242 01243 def serialize_numpy(self, buff, numpy): 01244 """ 01245 serialize message with numpy array types into buffer 01246 @param buff: buffer 01247 @type buff: StringIO 01248 @param numpy: numpy python module 01249 @type numpy module 01250 """ 01251 try: 01252 length = len(self.grasps) 01253 buff.write(_struct_I.pack(length)) 01254 for val1 in self.grasps: 01255 _v79 = val1.pre_grasp_posture 01256 _v80 = _v79.header 01257 buff.write(_struct_I.pack(_v80.seq)) 01258 _v81 = _v80.stamp 01259 _x = _v81 01260 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01261 _x = _v80.frame_id 01262 length = len(_x) 01263 buff.write(struct.pack('<I%ss'%length, length, _x)) 01264 length = len(_v79.name) 01265 buff.write(_struct_I.pack(length)) 01266 for val3 in _v79.name: 01267 length = len(val3) 01268 buff.write(struct.pack('<I%ss'%length, length, val3)) 01269 length = len(_v79.position) 01270 buff.write(_struct_I.pack(length)) 01271 pattern = '<%sd'%length 01272 buff.write(_v79.position.tostring()) 01273 length = len(_v79.velocity) 01274 buff.write(_struct_I.pack(length)) 01275 pattern = '<%sd'%length 01276 buff.write(_v79.velocity.tostring()) 01277 length = len(_v79.effort) 01278 buff.write(_struct_I.pack(length)) 01279 pattern = '<%sd'%length 01280 buff.write(_v79.effort.tostring()) 01281 _v82 = val1.grasp_posture 01282 _v83 = _v82.header 01283 buff.write(_struct_I.pack(_v83.seq)) 01284 _v84 = _v83.stamp 01285 _x = _v84 01286 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01287 _x = _v83.frame_id 01288 length = len(_x) 01289 buff.write(struct.pack('<I%ss'%length, length, _x)) 01290 length = len(_v82.name) 01291 buff.write(_struct_I.pack(length)) 01292 for val3 in _v82.name: 01293 length = len(val3) 01294 buff.write(struct.pack('<I%ss'%length, length, val3)) 01295 length = len(_v82.position) 01296 buff.write(_struct_I.pack(length)) 01297 pattern = '<%sd'%length 01298 buff.write(_v82.position.tostring()) 01299 length = len(_v82.velocity) 01300 buff.write(_struct_I.pack(length)) 01301 pattern = '<%sd'%length 01302 buff.write(_v82.velocity.tostring()) 01303 length = len(_v82.effort) 01304 buff.write(_struct_I.pack(length)) 01305 pattern = '<%sd'%length 01306 buff.write(_v82.effort.tostring()) 01307 _v85 = val1.grasp_pose 01308 _v86 = _v85.position 01309 _x = _v86 01310 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01311 _v87 = _v85.orientation 01312 _x = _v87 01313 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01314 _x = val1 01315 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 01316 length = len(val1.moved_obstacles) 01317 buff.write(_struct_I.pack(length)) 01318 for val2 in val1.moved_obstacles: 01319 _x = val2.reference_frame_id 01320 length = len(_x) 01321 buff.write(struct.pack('<I%ss'%length, length, _x)) 01322 length = len(val2.potential_models) 01323 buff.write(_struct_I.pack(length)) 01324 for val3 in val2.potential_models: 01325 buff.write(_struct_i.pack(val3.model_id)) 01326 _v88 = val3.pose 01327 _v89 = _v88.header 01328 buff.write(_struct_I.pack(_v89.seq)) 01329 _v90 = _v89.stamp 01330 _x = _v90 01331 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01332 _x = _v89.frame_id 01333 length = len(_x) 01334 buff.write(struct.pack('<I%ss'%length, length, _x)) 01335 _v91 = _v88.pose 01336 _v92 = _v91.position 01337 _x = _v92 01338 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01339 _v93 = _v91.orientation 01340 _x = _v93 01341 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01342 buff.write(_struct_f.pack(val3.confidence)) 01343 _x = val3.detector_name 01344 length = len(_x) 01345 buff.write(struct.pack('<I%ss'%length, length, _x)) 01346 _v94 = val2.cluster 01347 _v95 = _v94.header 01348 buff.write(_struct_I.pack(_v95.seq)) 01349 _v96 = _v95.stamp 01350 _x = _v96 01351 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01352 _x = _v95.frame_id 01353 length = len(_x) 01354 buff.write(struct.pack('<I%ss'%length, length, _x)) 01355 length = len(_v94.points) 01356 buff.write(_struct_I.pack(length)) 01357 for val4 in _v94.points: 01358 _x = val4 01359 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01360 length = len(_v94.channels) 01361 buff.write(_struct_I.pack(length)) 01362 for val4 in _v94.channels: 01363 _x = val4.name 01364 length = len(_x) 01365 buff.write(struct.pack('<I%ss'%length, length, _x)) 01366 length = len(val4.values) 01367 buff.write(_struct_I.pack(length)) 01368 pattern = '<%sf'%length 01369 buff.write(val4.values.tostring()) 01370 _v97 = val2.region 01371 _v98 = _v97.cloud 01372 _v99 = _v98.header 01373 buff.write(_struct_I.pack(_v99.seq)) 01374 _v100 = _v99.stamp 01375 _x = _v100 01376 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01377 _x = _v99.frame_id 01378 length = len(_x) 01379 buff.write(struct.pack('<I%ss'%length, length, _x)) 01380 _x = _v98 01381 buff.write(_struct_2I.pack(_x.height, _x.width)) 01382 length = len(_v98.fields) 01383 buff.write(_struct_I.pack(length)) 01384 for val5 in _v98.fields: 01385 _x = val5.name 01386 length = len(_x) 01387 buff.write(struct.pack('<I%ss'%length, length, _x)) 01388 _x = val5 01389 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01390 _x = _v98 01391 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01392 _x = _v98.data 01393 length = len(_x) 01394 # - if encoded as a list instead, serialize as bytes instead of string 01395 if type(_x) in [list, tuple]: 01396 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01397 else: 01398 buff.write(struct.pack('<I%ss'%length, length, _x)) 01399 buff.write(_struct_B.pack(_v98.is_dense)) 01400 length = len(_v97.mask) 01401 buff.write(_struct_I.pack(length)) 01402 pattern = '<%si'%length 01403 buff.write(_v97.mask.tostring()) 01404 _v101 = _v97.image 01405 _v102 = _v101.header 01406 buff.write(_struct_I.pack(_v102.seq)) 01407 _v103 = _v102.stamp 01408 _x = _v103 01409 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01410 _x = _v102.frame_id 01411 length = len(_x) 01412 buff.write(struct.pack('<I%ss'%length, length, _x)) 01413 _x = _v101 01414 buff.write(_struct_2I.pack(_x.height, _x.width)) 01415 _x = _v101.encoding 01416 length = len(_x) 01417 buff.write(struct.pack('<I%ss'%length, length, _x)) 01418 _x = _v101 01419 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01420 _x = _v101.data 01421 length = len(_x) 01422 # - if encoded as a list instead, serialize as bytes instead of string 01423 if type(_x) in [list, tuple]: 01424 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01425 else: 01426 buff.write(struct.pack('<I%ss'%length, length, _x)) 01427 _v104 = _v97.disparity_image 01428 _v105 = _v104.header 01429 buff.write(_struct_I.pack(_v105.seq)) 01430 _v106 = _v105.stamp 01431 _x = _v106 01432 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01433 _x = _v105.frame_id 01434 length = len(_x) 01435 buff.write(struct.pack('<I%ss'%length, length, _x)) 01436 _x = _v104 01437 buff.write(_struct_2I.pack(_x.height, _x.width)) 01438 _x = _v104.encoding 01439 length = len(_x) 01440 buff.write(struct.pack('<I%ss'%length, length, _x)) 01441 _x = _v104 01442 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01443 _x = _v104.data 01444 length = len(_x) 01445 # - if encoded as a list instead, serialize as bytes instead of string 01446 if type(_x) in [list, tuple]: 01447 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01448 else: 01449 buff.write(struct.pack('<I%ss'%length, length, _x)) 01450 _v107 = _v97.cam_info 01451 _v108 = _v107.header 01452 buff.write(_struct_I.pack(_v108.seq)) 01453 _v109 = _v108.stamp 01454 _x = _v109 01455 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01456 _x = _v108.frame_id 01457 length = len(_x) 01458 buff.write(struct.pack('<I%ss'%length, length, _x)) 01459 _x = _v107 01460 buff.write(_struct_2I.pack(_x.height, _x.width)) 01461 _x = _v107.distortion_model 01462 length = len(_x) 01463 buff.write(struct.pack('<I%ss'%length, length, _x)) 01464 length = len(_v107.D) 01465 buff.write(_struct_I.pack(length)) 01466 pattern = '<%sd'%length 01467 buff.write(_v107.D.tostring()) 01468 buff.write(_v107.K.tostring()) 01469 buff.write(_v107.R.tostring()) 01470 buff.write(_v107.P.tostring()) 01471 _x = _v107 01472 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01473 _v110 = _v107.roi 01474 _x = _v110 01475 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01476 _v111 = _v97.roi_box_pose 01477 _v112 = _v111.header 01478 buff.write(_struct_I.pack(_v112.seq)) 01479 _v113 = _v112.stamp 01480 _x = _v113 01481 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01482 _x = _v112.frame_id 01483 length = len(_x) 01484 buff.write(struct.pack('<I%ss'%length, length, _x)) 01485 _v114 = _v111.pose 01486 _v115 = _v114.position 01487 _x = _v115 01488 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01489 _v116 = _v114.orientation 01490 _x = _v116 01491 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01492 _v117 = _v97.roi_box_dims 01493 _x = _v117 01494 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01495 _x = val2.collision_name 01496 length = len(_x) 01497 buff.write(struct.pack('<I%ss'%length, length, _x)) 01498 except struct.error as se: self._check_types(se) 01499 except TypeError as te: self._check_types(te) 01500 01501 def deserialize_numpy(self, str, numpy): 01502 """ 01503 unpack serialized message in str into this message instance using numpy for array types 01504 @param str: byte array of serialized message 01505 @type str: str 01506 @param numpy: numpy python module 01507 @type numpy: module 01508 """ 01509 try: 01510 end = 0 01511 start = end 01512 end += 4 01513 (length,) = _struct_I.unpack(str[start:end]) 01514 self.grasps = [] 01515 for i in range(0, length): 01516 val1 = object_manipulation_msgs.msg.Grasp() 01517 _v118 = val1.pre_grasp_posture 01518 _v119 = _v118.header 01519 start = end 01520 end += 4 01521 (_v119.seq,) = _struct_I.unpack(str[start:end]) 01522 _v120 = _v119.stamp 01523 _x = _v120 01524 start = end 01525 end += 8 01526 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01527 start = end 01528 end += 4 01529 (length,) = _struct_I.unpack(str[start:end]) 01530 start = end 01531 end += length 01532 _v119.frame_id = str[start:end] 01533 start = end 01534 end += 4 01535 (length,) = _struct_I.unpack(str[start:end]) 01536 _v118.name = [] 01537 for i in range(0, length): 01538 start = end 01539 end += 4 01540 (length,) = _struct_I.unpack(str[start:end]) 01541 start = end 01542 end += length 01543 val3 = str[start:end] 01544 _v118.name.append(val3) 01545 start = end 01546 end += 4 01547 (length,) = _struct_I.unpack(str[start:end]) 01548 pattern = '<%sd'%length 01549 start = end 01550 end += struct.calcsize(pattern) 01551 _v118.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01552 start = end 01553 end += 4 01554 (length,) = _struct_I.unpack(str[start:end]) 01555 pattern = '<%sd'%length 01556 start = end 01557 end += struct.calcsize(pattern) 01558 _v118.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01559 start = end 01560 end += 4 01561 (length,) = _struct_I.unpack(str[start:end]) 01562 pattern = '<%sd'%length 01563 start = end 01564 end += struct.calcsize(pattern) 01565 _v118.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01566 _v121 = val1.grasp_posture 01567 _v122 = _v121.header 01568 start = end 01569 end += 4 01570 (_v122.seq,) = _struct_I.unpack(str[start:end]) 01571 _v123 = _v122.stamp 01572 _x = _v123 01573 start = end 01574 end += 8 01575 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01576 start = end 01577 end += 4 01578 (length,) = _struct_I.unpack(str[start:end]) 01579 start = end 01580 end += length 01581 _v122.frame_id = str[start:end] 01582 start = end 01583 end += 4 01584 (length,) = _struct_I.unpack(str[start:end]) 01585 _v121.name = [] 01586 for i in range(0, length): 01587 start = end 01588 end += 4 01589 (length,) = _struct_I.unpack(str[start:end]) 01590 start = end 01591 end += length 01592 val3 = str[start:end] 01593 _v121.name.append(val3) 01594 start = end 01595 end += 4 01596 (length,) = _struct_I.unpack(str[start:end]) 01597 pattern = '<%sd'%length 01598 start = end 01599 end += struct.calcsize(pattern) 01600 _v121.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01601 start = end 01602 end += 4 01603 (length,) = _struct_I.unpack(str[start:end]) 01604 pattern = '<%sd'%length 01605 start = end 01606 end += struct.calcsize(pattern) 01607 _v121.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01608 start = end 01609 end += 4 01610 (length,) = _struct_I.unpack(str[start:end]) 01611 pattern = '<%sd'%length 01612 start = end 01613 end += struct.calcsize(pattern) 01614 _v121.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01615 _v124 = val1.grasp_pose 01616 _v125 = _v124.position 01617 _x = _v125 01618 start = end 01619 end += 24 01620 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01621 _v126 = _v124.orientation 01622 _x = _v126 01623 start = end 01624 end += 32 01625 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01626 _x = val1 01627 start = end 01628 end += 17 01629 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 01630 val1.cluster_rep = bool(val1.cluster_rep) 01631 start = end 01632 end += 4 01633 (length,) = _struct_I.unpack(str[start:end]) 01634 val1.moved_obstacles = [] 01635 for i in range(0, length): 01636 val2 = object_manipulation_msgs.msg.GraspableObject() 01637 start = end 01638 end += 4 01639 (length,) = _struct_I.unpack(str[start:end]) 01640 start = end 01641 end += length 01642 val2.reference_frame_id = str[start:end] 01643 start = end 01644 end += 4 01645 (length,) = _struct_I.unpack(str[start:end]) 01646 val2.potential_models = [] 01647 for i in range(0, length): 01648 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 01649 start = end 01650 end += 4 01651 (val3.model_id,) = _struct_i.unpack(str[start:end]) 01652 _v127 = val3.pose 01653 _v128 = _v127.header 01654 start = end 01655 end += 4 01656 (_v128.seq,) = _struct_I.unpack(str[start:end]) 01657 _v129 = _v128.stamp 01658 _x = _v129 01659 start = end 01660 end += 8 01661 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01662 start = end 01663 end += 4 01664 (length,) = _struct_I.unpack(str[start:end]) 01665 start = end 01666 end += length 01667 _v128.frame_id = str[start:end] 01668 _v130 = _v127.pose 01669 _v131 = _v130.position 01670 _x = _v131 01671 start = end 01672 end += 24 01673 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01674 _v132 = _v130.orientation 01675 _x = _v132 01676 start = end 01677 end += 32 01678 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01679 start = end 01680 end += 4 01681 (val3.confidence,) = _struct_f.unpack(str[start:end]) 01682 start = end 01683 end += 4 01684 (length,) = _struct_I.unpack(str[start:end]) 01685 start = end 01686 end += length 01687 val3.detector_name = str[start:end] 01688 val2.potential_models.append(val3) 01689 _v133 = val2.cluster 01690 _v134 = _v133.header 01691 start = end 01692 end += 4 01693 (_v134.seq,) = _struct_I.unpack(str[start:end]) 01694 _v135 = _v134.stamp 01695 _x = _v135 01696 start = end 01697 end += 8 01698 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01699 start = end 01700 end += 4 01701 (length,) = _struct_I.unpack(str[start:end]) 01702 start = end 01703 end += length 01704 _v134.frame_id = str[start:end] 01705 start = end 01706 end += 4 01707 (length,) = _struct_I.unpack(str[start:end]) 01708 _v133.points = [] 01709 for i in range(0, length): 01710 val4 = geometry_msgs.msg.Point32() 01711 _x = val4 01712 start = end 01713 end += 12 01714 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01715 _v133.points.append(val4) 01716 start = end 01717 end += 4 01718 (length,) = _struct_I.unpack(str[start:end]) 01719 _v133.channels = [] 01720 for i in range(0, length): 01721 val4 = sensor_msgs.msg.ChannelFloat32() 01722 start = end 01723 end += 4 01724 (length,) = _struct_I.unpack(str[start:end]) 01725 start = end 01726 end += length 01727 val4.name = str[start:end] 01728 start = end 01729 end += 4 01730 (length,) = _struct_I.unpack(str[start:end]) 01731 pattern = '<%sf'%length 01732 start = end 01733 end += struct.calcsize(pattern) 01734 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 01735 _v133.channels.append(val4) 01736 _v136 = val2.region 01737 _v137 = _v136.cloud 01738 _v138 = _v137.header 01739 start = end 01740 end += 4 01741 (_v138.seq,) = _struct_I.unpack(str[start:end]) 01742 _v139 = _v138.stamp 01743 _x = _v139 01744 start = end 01745 end += 8 01746 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01747 start = end 01748 end += 4 01749 (length,) = _struct_I.unpack(str[start:end]) 01750 start = end 01751 end += length 01752 _v138.frame_id = str[start:end] 01753 _x = _v137 01754 start = end 01755 end += 8 01756 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01757 start = end 01758 end += 4 01759 (length,) = _struct_I.unpack(str[start:end]) 01760 _v137.fields = [] 01761 for i in range(0, length): 01762 val5 = sensor_msgs.msg.PointField() 01763 start = end 01764 end += 4 01765 (length,) = _struct_I.unpack(str[start:end]) 01766 start = end 01767 end += length 01768 val5.name = str[start:end] 01769 _x = val5 01770 start = end 01771 end += 9 01772 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01773 _v137.fields.append(val5) 01774 _x = _v137 01775 start = end 01776 end += 9 01777 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01778 _v137.is_bigendian = bool(_v137.is_bigendian) 01779 start = end 01780 end += 4 01781 (length,) = _struct_I.unpack(str[start:end]) 01782 start = end 01783 end += length 01784 _v137.data = str[start:end] 01785 start = end 01786 end += 1 01787 (_v137.is_dense,) = _struct_B.unpack(str[start:end]) 01788 _v137.is_dense = bool(_v137.is_dense) 01789 start = end 01790 end += 4 01791 (length,) = _struct_I.unpack(str[start:end]) 01792 pattern = '<%si'%length 01793 start = end 01794 end += struct.calcsize(pattern) 01795 _v136.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 01796 _v140 = _v136.image 01797 _v141 = _v140.header 01798 start = end 01799 end += 4 01800 (_v141.seq,) = _struct_I.unpack(str[start:end]) 01801 _v142 = _v141.stamp 01802 _x = _v142 01803 start = end 01804 end += 8 01805 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01806 start = end 01807 end += 4 01808 (length,) = _struct_I.unpack(str[start:end]) 01809 start = end 01810 end += length 01811 _v141.frame_id = str[start:end] 01812 _x = _v140 01813 start = end 01814 end += 8 01815 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01816 start = end 01817 end += 4 01818 (length,) = _struct_I.unpack(str[start:end]) 01819 start = end 01820 end += length 01821 _v140.encoding = str[start:end] 01822 _x = _v140 01823 start = end 01824 end += 5 01825 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01826 start = end 01827 end += 4 01828 (length,) = _struct_I.unpack(str[start:end]) 01829 start = end 01830 end += length 01831 _v140.data = str[start:end] 01832 _v143 = _v136.disparity_image 01833 _v144 = _v143.header 01834 start = end 01835 end += 4 01836 (_v144.seq,) = _struct_I.unpack(str[start:end]) 01837 _v145 = _v144.stamp 01838 _x = _v145 01839 start = end 01840 end += 8 01841 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01842 start = end 01843 end += 4 01844 (length,) = _struct_I.unpack(str[start:end]) 01845 start = end 01846 end += length 01847 _v144.frame_id = str[start:end] 01848 _x = _v143 01849 start = end 01850 end += 8 01851 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01852 start = end 01853 end += 4 01854 (length,) = _struct_I.unpack(str[start:end]) 01855 start = end 01856 end += length 01857 _v143.encoding = str[start:end] 01858 _x = _v143 01859 start = end 01860 end += 5 01861 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01862 start = end 01863 end += 4 01864 (length,) = _struct_I.unpack(str[start:end]) 01865 start = end 01866 end += length 01867 _v143.data = str[start:end] 01868 _v146 = _v136.cam_info 01869 _v147 = _v146.header 01870 start = end 01871 end += 4 01872 (_v147.seq,) = _struct_I.unpack(str[start:end]) 01873 _v148 = _v147.stamp 01874 _x = _v148 01875 start = end 01876 end += 8 01877 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01878 start = end 01879 end += 4 01880 (length,) = _struct_I.unpack(str[start:end]) 01881 start = end 01882 end += length 01883 _v147.frame_id = str[start:end] 01884 _x = _v146 01885 start = end 01886 end += 8 01887 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01888 start = end 01889 end += 4 01890 (length,) = _struct_I.unpack(str[start:end]) 01891 start = end 01892 end += length 01893 _v146.distortion_model = str[start:end] 01894 start = end 01895 end += 4 01896 (length,) = _struct_I.unpack(str[start:end]) 01897 pattern = '<%sd'%length 01898 start = end 01899 end += struct.calcsize(pattern) 01900 _v146.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01901 start = end 01902 end += 72 01903 _v146.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 01904 start = end 01905 end += 72 01906 _v146.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 01907 start = end 01908 end += 96 01909 _v146.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 01910 _x = _v146 01911 start = end 01912 end += 8 01913 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01914 _v149 = _v146.roi 01915 _x = _v149 01916 start = end 01917 end += 17 01918 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01919 _v149.do_rectify = bool(_v149.do_rectify) 01920 _v150 = _v136.roi_box_pose 01921 _v151 = _v150.header 01922 start = end 01923 end += 4 01924 (_v151.seq,) = _struct_I.unpack(str[start:end]) 01925 _v152 = _v151.stamp 01926 _x = _v152 01927 start = end 01928 end += 8 01929 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01930 start = end 01931 end += 4 01932 (length,) = _struct_I.unpack(str[start:end]) 01933 start = end 01934 end += length 01935 _v151.frame_id = str[start:end] 01936 _v153 = _v150.pose 01937 _v154 = _v153.position 01938 _x = _v154 01939 start = end 01940 end += 24 01941 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01942 _v155 = _v153.orientation 01943 _x = _v155 01944 start = end 01945 end += 32 01946 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01947 _v156 = _v136.roi_box_dims 01948 _x = _v156 01949 start = end 01950 end += 24 01951 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01952 start = end 01953 end += 4 01954 (length,) = _struct_I.unpack(str[start:end]) 01955 start = end 01956 end += length 01957 val2.collision_name = str[start:end] 01958 val1.moved_obstacles.append(val2) 01959 self.grasps.append(val1) 01960 return self 01961 except struct.error as e: 01962 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01963 01964 _struct_I = roslib.message.struct_I 01965 _struct_IBI = struct.Struct("<IBI") 01966 _struct_B = struct.Struct("<B") 01967 _struct_12d = struct.Struct("<12d") 01968 _struct_f = struct.Struct("<f") 01969 _struct_dB2f = struct.Struct("<dB2f") 01970 _struct_BI = struct.Struct("<BI") 01971 _struct_3f = struct.Struct("<3f") 01972 _struct_i = struct.Struct("<i") 01973 _struct_9d = struct.Struct("<9d") 01974 _struct_B2I = struct.Struct("<B2I") 01975 _struct_4d = struct.Struct("<4d") 01976 _struct_2I = struct.Struct("<2I") 01977 _struct_4IB = struct.Struct("<4IB") 01978 _struct_3d = struct.Struct("<3d")