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