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