$search
00001 """autogenerated by genmsg_py from PickupResult.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 PickupResult(roslib.message.Message): 00012 _md5sum = "e37faa4bc17adb31e87f0a21276cde44" 00013 _type = "object_manipulation_msgs/PickupResult" 00014 _has_header = False #flag to mark the presence of a Header object 00015 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00016 00017 # The overall result of the pickup attempt 00018 ManipulationResult manipulation_result 00019 00020 # The performed grasp, if attempt was successful 00021 Grasp grasp 00022 00023 # the complete list of attempted grasp, in the order in which they have been attempted 00024 # the successful one should be the last one in this list 00025 Grasp[] attempted_grasps 00026 00027 # the outcomes of the attempted grasps, in the same order as attempted_grasps 00028 GraspResult[] attempted_grasp_results 00029 00030 00031 ================================================================================ 00032 MSG: object_manipulation_msgs/ManipulationResult 00033 # Result codes for manipulation tasks 00034 00035 # task completed as expected 00036 # generally means you can proceed as planned 00037 int32 SUCCESS = 1 00038 00039 # task not possible (e.g. out of reach or obstacles in the way) 00040 # generally means that the world was not disturbed, so you can try another task 00041 int32 UNFEASIBLE = -1 00042 00043 # task was thought possible, but failed due to unexpected events during execution 00044 # it is likely that the world was disturbed, so you are encouraged to refresh 00045 # your sensed world model before proceeding to another task 00046 int32 FAILED = -2 00047 00048 # a lower level error prevented task completion (e.g. joint controller not responding) 00049 # generally requires human attention 00050 int32 ERROR = -3 00051 00052 # means that at some point during execution we ended up in a state that the collision-aware 00053 # arm navigation module will not move out of. The world was likely not disturbed, but you 00054 # probably need a new collision map to move the arm out of the stuck position 00055 int32 ARM_MOVEMENT_PREVENTED = -4 00056 00057 # specific to grasp actions 00058 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested 00059 # it is likely that the collision environment will see collisions between the hand/object and the support surface 00060 int32 LIFT_FAILED = -5 00061 00062 # specific to place actions 00063 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested 00064 # it is likely that the collision environment will see collisions between the hand and the object 00065 int32 RETREAT_FAILED = -6 00066 00067 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else" 00068 int32 CANCELLED = -7 00069 00070 # the actual value of this error code 00071 int32 value 00072 00073 ================================================================================ 00074 MSG: object_manipulation_msgs/Grasp 00075 00076 # The internal posture of the hand for the pre-grasp 00077 # only positions are used 00078 sensor_msgs/JointState pre_grasp_posture 00079 00080 # The internal posture of the hand for the grasp 00081 # positions and efforts are used 00082 sensor_msgs/JointState grasp_posture 00083 00084 # The position of the end-effector for the grasp relative to a reference frame 00085 # (that is always specified elsewhere, not in this message) 00086 geometry_msgs/Pose grasp_pose 00087 00088 # The estimated probability of success for this grasp 00089 float64 success_probability 00090 00091 # Debug flag to indicate that this grasp would be the best in its cluster 00092 bool cluster_rep 00093 00094 # how far the pre-grasp should ideally be away from the grasp 00095 float32 desired_approach_distance 00096 00097 # how much distance between pre-grasp and grasp must actually be feasible 00098 # for the grasp not to be rejected 00099 float32 min_approach_distance 00100 00101 # an optional list of obstacles that we have semantic information about 00102 # and that we expect might move in the course of executing this grasp 00103 # the grasp planner is expected to make sure they move in an OK way; during 00104 # execution, grasp executors will not check for collisions against these objects 00105 GraspableObject[] moved_obstacles 00106 00107 ================================================================================ 00108 MSG: sensor_msgs/JointState 00109 # This is a message that holds data to describe the state of a set of torque controlled joints. 00110 # 00111 # The state of each joint (revolute or prismatic) is defined by: 00112 # * the position of the joint (rad or m), 00113 # * the velocity of the joint (rad/s or m/s) and 00114 # * the effort that is applied in the joint (Nm or N). 00115 # 00116 # Each joint is uniquely identified by its name 00117 # The header specifies the time at which the joint states were recorded. All the joint states 00118 # in one message have to be recorded at the same time. 00119 # 00120 # This message consists of a multiple arrays, one for each part of the joint state. 00121 # The goal is to make each of the fields optional. When e.g. your joints have no 00122 # effort associated with them, you can leave the effort array empty. 00123 # 00124 # All arrays in this message should have the same size, or be empty. 00125 # This is the only way to uniquely associate the joint name with the correct 00126 # states. 00127 00128 00129 Header header 00130 00131 string[] name 00132 float64[] position 00133 float64[] velocity 00134 float64[] effort 00135 00136 ================================================================================ 00137 MSG: std_msgs/Header 00138 # Standard metadata for higher-level stamped data types. 00139 # This is generally used to communicate timestamped data 00140 # in a particular coordinate frame. 00141 # 00142 # sequence ID: consecutively increasing ID 00143 uint32 seq 00144 #Two-integer timestamp that is expressed as: 00145 # * stamp.secs: seconds (stamp_secs) since epoch 00146 # * stamp.nsecs: nanoseconds since stamp_secs 00147 # time-handling sugar is provided by the client library 00148 time stamp 00149 #Frame this data is associated with 00150 # 0: no frame 00151 # 1: global frame 00152 string frame_id 00153 00154 ================================================================================ 00155 MSG: geometry_msgs/Pose 00156 # A representation of pose in free space, composed of postion and orientation. 00157 Point position 00158 Quaternion orientation 00159 00160 ================================================================================ 00161 MSG: geometry_msgs/Point 00162 # This contains the position of a point in free space 00163 float64 x 00164 float64 y 00165 float64 z 00166 00167 ================================================================================ 00168 MSG: geometry_msgs/Quaternion 00169 # This represents an orientation in free space in quaternion form. 00170 00171 float64 x 00172 float64 y 00173 float64 z 00174 float64 w 00175 00176 ================================================================================ 00177 MSG: object_manipulation_msgs/GraspableObject 00178 # an object that the object_manipulator can work on 00179 00180 # a graspable object can be represented in multiple ways. This message 00181 # can contain all of them. Which one is actually used is up to the receiver 00182 # of this message. When adding new representations, one must be careful that 00183 # they have reasonable lightweight defaults indicating that that particular 00184 # representation is not available. 00185 00186 # the tf frame to be used as a reference frame when combining information from 00187 # the different representations below 00188 string reference_frame_id 00189 00190 # potential recognition results from a database of models 00191 # all poses are relative to the object reference pose 00192 household_objects_database_msgs/DatabaseModelPose[] potential_models 00193 00194 # the point cloud itself 00195 sensor_msgs/PointCloud cluster 00196 00197 # a region of a PointCloud2 of interest 00198 object_manipulation_msgs/SceneRegion region 00199 00200 # the name that this object has in the collision environment 00201 string collision_name 00202 ================================================================================ 00203 MSG: household_objects_database_msgs/DatabaseModelPose 00204 # Informs that a specific model from the Model Database has been 00205 # identified at a certain location 00206 00207 # the database id of the model 00208 int32 model_id 00209 00210 # the pose that it can be found in 00211 geometry_msgs/PoseStamped pose 00212 00213 # a measure of the confidence level in this detection result 00214 float32 confidence 00215 00216 # the name of the object detector that generated this detection result 00217 string detector_name 00218 00219 ================================================================================ 00220 MSG: geometry_msgs/PoseStamped 00221 # A Pose with reference coordinate frame and timestamp 00222 Header header 00223 Pose pose 00224 00225 ================================================================================ 00226 MSG: sensor_msgs/PointCloud 00227 # This message holds a collection of 3d points, plus optional additional 00228 # information about each point. 00229 00230 # Time of sensor data acquisition, coordinate frame ID. 00231 Header header 00232 00233 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00234 # in the frame given in the header. 00235 geometry_msgs/Point32[] points 00236 00237 # Each channel should have the same number of elements as points array, 00238 # and the data in each channel should correspond 1:1 with each point. 00239 # Channel names in common practice are listed in ChannelFloat32.msg. 00240 ChannelFloat32[] channels 00241 00242 ================================================================================ 00243 MSG: geometry_msgs/Point32 00244 # This contains the position of a point in free space(with 32 bits of precision). 00245 # It is recommeded to use Point wherever possible instead of Point32. 00246 # 00247 # This recommendation is to promote interoperability. 00248 # 00249 # This message is designed to take up less space when sending 00250 # lots of points at once, as in the case of a PointCloud. 00251 00252 float32 x 00253 float32 y 00254 float32 z 00255 ================================================================================ 00256 MSG: sensor_msgs/ChannelFloat32 00257 # This message is used by the PointCloud message to hold optional data 00258 # associated with each point in the cloud. The length of the values 00259 # array should be the same as the length of the points array in the 00260 # PointCloud, and each value should be associated with the corresponding 00261 # point. 00262 00263 # Channel names in existing practice include: 00264 # "u", "v" - row and column (respectively) in the left stereo image. 00265 # This is opposite to usual conventions but remains for 00266 # historical reasons. The newer PointCloud2 message has no 00267 # such problem. 00268 # "rgb" - For point clouds produced by color stereo cameras. uint8 00269 # (R,G,B) values packed into the least significant 24 bits, 00270 # in order. 00271 # "intensity" - laser or pixel intensity. 00272 # "distance" 00273 00274 # The channel name should give semantics of the channel (e.g. 00275 # "intensity" instead of "value"). 00276 string name 00277 00278 # The values array should be 1-1 with the elements of the associated 00279 # PointCloud. 00280 float32[] values 00281 00282 ================================================================================ 00283 MSG: object_manipulation_msgs/SceneRegion 00284 # Point cloud 00285 sensor_msgs/PointCloud2 cloud 00286 00287 # Indices for the region of interest 00288 int32[] mask 00289 00290 # One of the corresponding 2D images, if applicable 00291 sensor_msgs/Image image 00292 00293 # The disparity image, if applicable 00294 sensor_msgs/Image disparity_image 00295 00296 # Camera info for the camera that took the image 00297 sensor_msgs/CameraInfo cam_info 00298 00299 # a 3D region of interest for grasp planning 00300 geometry_msgs/PoseStamped roi_box_pose 00301 geometry_msgs/Vector3 roi_box_dims 00302 00303 ================================================================================ 00304 MSG: sensor_msgs/PointCloud2 00305 # This message holds a collection of N-dimensional points, which may 00306 # contain additional information such as normals, intensity, etc. The 00307 # point data is stored as a binary blob, its layout described by the 00308 # contents of the "fields" array. 00309 00310 # The point cloud data may be organized 2d (image-like) or 1d 00311 # (unordered). Point clouds organized as 2d images may be produced by 00312 # camera depth sensors such as stereo or time-of-flight. 00313 00314 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00315 # points). 00316 Header header 00317 00318 # 2D structure of the point cloud. If the cloud is unordered, height is 00319 # 1 and width is the length of the point cloud. 00320 uint32 height 00321 uint32 width 00322 00323 # Describes the channels and their layout in the binary data blob. 00324 PointField[] fields 00325 00326 bool is_bigendian # Is this data bigendian? 00327 uint32 point_step # Length of a point in bytes 00328 uint32 row_step # Length of a row in bytes 00329 uint8[] data # Actual point data, size is (row_step*height) 00330 00331 bool is_dense # True if there are no invalid points 00332 00333 ================================================================================ 00334 MSG: sensor_msgs/PointField 00335 # This message holds the description of one point entry in the 00336 # PointCloud2 message format. 00337 uint8 INT8 = 1 00338 uint8 UINT8 = 2 00339 uint8 INT16 = 3 00340 uint8 UINT16 = 4 00341 uint8 INT32 = 5 00342 uint8 UINT32 = 6 00343 uint8 FLOAT32 = 7 00344 uint8 FLOAT64 = 8 00345 00346 string name # Name of field 00347 uint32 offset # Offset from start of point struct 00348 uint8 datatype # Datatype enumeration, see above 00349 uint32 count # How many elements in the field 00350 00351 ================================================================================ 00352 MSG: sensor_msgs/Image 00353 # This message contains an uncompressed image 00354 # (0, 0) is at top-left corner of image 00355 # 00356 00357 Header header # Header timestamp should be acquisition time of image 00358 # Header frame_id should be optical frame of camera 00359 # origin of frame should be optical center of cameara 00360 # +x should point to the right in the image 00361 # +y should point down in the image 00362 # +z should point into to plane of the image 00363 # If the frame_id here and the frame_id of the CameraInfo 00364 # message associated with the image conflict 00365 # the behavior is undefined 00366 00367 uint32 height # image height, that is, number of rows 00368 uint32 width # image width, that is, number of columns 00369 00370 # The legal values for encoding are in file src/image_encodings.cpp 00371 # If you want to standardize a new string format, join 00372 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00373 00374 string encoding # Encoding of pixels -- channel meaning, ordering, size 00375 # taken from the list of strings in src/image_encodings.cpp 00376 00377 uint8 is_bigendian # is this data bigendian? 00378 uint32 step # Full row length in bytes 00379 uint8[] data # actual matrix data, size is (step * rows) 00380 00381 ================================================================================ 00382 MSG: sensor_msgs/CameraInfo 00383 # This message defines meta information for a camera. It should be in a 00384 # camera namespace on topic "camera_info" and accompanied by up to five 00385 # image topics named: 00386 # 00387 # image_raw - raw data from the camera driver, possibly Bayer encoded 00388 # image - monochrome, distorted 00389 # image_color - color, distorted 00390 # image_rect - monochrome, rectified 00391 # image_rect_color - color, rectified 00392 # 00393 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00394 # for producing the four processed image topics from image_raw and 00395 # camera_info. The meaning of the camera parameters are described in 00396 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00397 # 00398 # The image_geometry package provides a user-friendly interface to 00399 # common operations using this meta information. If you want to, e.g., 00400 # project a 3d point into image coordinates, we strongly recommend 00401 # using image_geometry. 00402 # 00403 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00404 # zeroed out. In particular, clients may assume that K[0] == 0.0 00405 # indicates an uncalibrated camera. 00406 00407 ####################################################################### 00408 # Image acquisition info # 00409 ####################################################################### 00410 00411 # Time of image acquisition, camera coordinate frame ID 00412 Header header # Header timestamp should be acquisition time of image 00413 # Header frame_id should be optical frame of camera 00414 # origin of frame should be optical center of camera 00415 # +x should point to the right in the image 00416 # +y should point down in the image 00417 # +z should point into the plane of the image 00418 00419 00420 ####################################################################### 00421 # Calibration Parameters # 00422 ####################################################################### 00423 # These are fixed during camera calibration. Their values will be the # 00424 # same in all messages until the camera is recalibrated. Note that # 00425 # self-calibrating systems may "recalibrate" frequently. # 00426 # # 00427 # The internal parameters can be used to warp a raw (distorted) image # 00428 # to: # 00429 # 1. An undistorted image (requires D and K) # 00430 # 2. A rectified image (requires D, K, R) # 00431 # The projection matrix P projects 3D points into the rectified image.# 00432 ####################################################################### 00433 00434 # The image dimensions with which the camera was calibrated. Normally 00435 # this will be the full camera resolution in pixels. 00436 uint32 height 00437 uint32 width 00438 00439 # The distortion model used. Supported models are listed in 00440 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00441 # simple model of radial and tangential distortion - is sufficent. 00442 string distortion_model 00443 00444 # The distortion parameters, size depending on the distortion model. 00445 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00446 float64[] D 00447 00448 # Intrinsic camera matrix for the raw (distorted) images. 00449 # [fx 0 cx] 00450 # K = [ 0 fy cy] 00451 # [ 0 0 1] 00452 # Projects 3D points in the camera coordinate frame to 2D pixel 00453 # coordinates using the focal lengths (fx, fy) and principal point 00454 # (cx, cy). 00455 float64[9] K # 3x3 row-major matrix 00456 00457 # Rectification matrix (stereo cameras only) 00458 # A rotation matrix aligning the camera coordinate system to the ideal 00459 # stereo image plane so that epipolar lines in both stereo images are 00460 # parallel. 00461 float64[9] R # 3x3 row-major matrix 00462 00463 # Projection/camera matrix 00464 # [fx' 0 cx' Tx] 00465 # P = [ 0 fy' cy' Ty] 00466 # [ 0 0 1 0] 00467 # By convention, this matrix specifies the intrinsic (camera) matrix 00468 # of the processed (rectified) image. That is, the left 3x3 portion 00469 # is the normal camera intrinsic matrix for the rectified image. 00470 # It projects 3D points in the camera coordinate frame to 2D pixel 00471 # coordinates using the focal lengths (fx', fy') and principal point 00472 # (cx', cy') - these may differ from the values in K. 00473 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00474 # also have R = the identity and P[1:3,1:3] = K. 00475 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00476 # position of the optical center of the second camera in the first 00477 # camera's frame. We assume Tz = 0 so both cameras are in the same 00478 # stereo image plane. The first camera always has Tx = Ty = 0. For 00479 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00480 # Tx = -fx' * B, where B is the baseline between the cameras. 00481 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00482 # the rectified image is given by: 00483 # [u v w]' = P * [X Y Z 1]' 00484 # x = u / w 00485 # y = v / w 00486 # This holds for both images of a stereo pair. 00487 float64[12] P # 3x4 row-major matrix 00488 00489 00490 ####################################################################### 00491 # Operational Parameters # 00492 ####################################################################### 00493 # These define the image region actually captured by the camera # 00494 # driver. Although they affect the geometry of the output image, they # 00495 # may be changed freely without recalibrating the camera. # 00496 ####################################################################### 00497 00498 # Binning refers here to any camera setting which combines rectangular 00499 # neighborhoods of pixels into larger "super-pixels." It reduces the 00500 # resolution of the output image to 00501 # (width / binning_x) x (height / binning_y). 00502 # The default values binning_x = binning_y = 0 is considered the same 00503 # as binning_x = binning_y = 1 (no subsampling). 00504 uint32 binning_x 00505 uint32 binning_y 00506 00507 # Region of interest (subwindow of full camera resolution), given in 00508 # full resolution (unbinned) image coordinates. A particular ROI 00509 # always denotes the same window of pixels on the camera sensor, 00510 # regardless of binning settings. 00511 # The default setting of roi (all values 0) is considered the same as 00512 # full resolution (roi.width = width, roi.height = height). 00513 RegionOfInterest roi 00514 00515 ================================================================================ 00516 MSG: sensor_msgs/RegionOfInterest 00517 # This message is used to specify a region of interest within an image. 00518 # 00519 # When used to specify the ROI setting of the camera when the image was 00520 # taken, the height and width fields should either match the height and 00521 # width fields for the associated image; or height = width = 0 00522 # indicates that the full resolution image was captured. 00523 00524 uint32 x_offset # Leftmost pixel of the ROI 00525 # (0 if the ROI includes the left edge of the image) 00526 uint32 y_offset # Topmost pixel of the ROI 00527 # (0 if the ROI includes the top edge of the image) 00528 uint32 height # Height of ROI 00529 uint32 width # Width of ROI 00530 00531 # True if a distinct rectified ROI should be calculated from the "raw" 00532 # ROI in this message. Typically this should be False if the full image 00533 # is captured (ROI not used), and True if a subwindow is captured (ROI 00534 # used). 00535 bool do_rectify 00536 00537 ================================================================================ 00538 MSG: geometry_msgs/Vector3 00539 # This represents a vector in free space. 00540 00541 float64 x 00542 float64 y 00543 float64 z 00544 ================================================================================ 00545 MSG: object_manipulation_msgs/GraspResult 00546 int32 SUCCESS = 1 00547 int32 GRASP_OUT_OF_REACH = 2 00548 int32 GRASP_IN_COLLISION = 3 00549 int32 GRASP_UNFEASIBLE = 4 00550 int32 PREGRASP_OUT_OF_REACH = 5 00551 int32 PREGRASP_IN_COLLISION = 6 00552 int32 PREGRASP_UNFEASIBLE = 7 00553 int32 LIFT_OUT_OF_REACH = 8 00554 int32 LIFT_IN_COLLISION = 9 00555 int32 LIFT_UNFEASIBLE = 10 00556 int32 MOVE_ARM_FAILED = 11 00557 int32 GRASP_FAILED = 12 00558 int32 LIFT_FAILED = 13 00559 int32 RETREAT_FAILED = 14 00560 int32 result_code 00561 00562 # whether the state of the world was disturbed by this attempt. generally, this flag 00563 # shows if another task can be attempted, or a new sensed world model is recommeded 00564 # before proceeding 00565 bool continuation_possible 00566 00567 """ 00568 __slots__ = ['manipulation_result','grasp','attempted_grasps','attempted_grasp_results'] 00569 _slot_types = ['object_manipulation_msgs/ManipulationResult','object_manipulation_msgs/Grasp','object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspResult[]'] 00570 00571 def __init__(self, *args, **kwds): 00572 """ 00573 Constructor. Any message fields that are implicitly/explicitly 00574 set to None will be assigned a default value. The recommend 00575 use is keyword arguments as this is more robust to future message 00576 changes. You cannot mix in-order arguments and keyword arguments. 00577 00578 The available fields are: 00579 manipulation_result,grasp,attempted_grasps,attempted_grasp_results 00580 00581 @param args: complete set of field values, in .msg order 00582 @param kwds: use keyword arguments corresponding to message field names 00583 to set specific fields. 00584 """ 00585 if args or kwds: 00586 super(PickupResult, self).__init__(*args, **kwds) 00587 #message fields cannot be None, assign default values for those that are 00588 if self.manipulation_result is None: 00589 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult() 00590 if self.grasp is None: 00591 self.grasp = object_manipulation_msgs.msg.Grasp() 00592 if self.attempted_grasps is None: 00593 self.attempted_grasps = [] 00594 if self.attempted_grasp_results is None: 00595 self.attempted_grasp_results = [] 00596 else: 00597 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult() 00598 self.grasp = object_manipulation_msgs.msg.Grasp() 00599 self.attempted_grasps = [] 00600 self.attempted_grasp_results = [] 00601 00602 def _get_types(self): 00603 """ 00604 internal API method 00605 """ 00606 return self._slot_types 00607 00608 def serialize(self, buff): 00609 """ 00610 serialize message into buffer 00611 @param buff: buffer 00612 @type buff: StringIO 00613 """ 00614 try: 00615 _x = self 00616 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs)) 00617 _x = self.grasp.pre_grasp_posture.header.frame_id 00618 length = len(_x) 00619 buff.write(struct.pack('<I%ss'%length, length, _x)) 00620 length = len(self.grasp.pre_grasp_posture.name) 00621 buff.write(_struct_I.pack(length)) 00622 for val1 in self.grasp.pre_grasp_posture.name: 00623 length = len(val1) 00624 buff.write(struct.pack('<I%ss'%length, length, val1)) 00625 length = len(self.grasp.pre_grasp_posture.position) 00626 buff.write(_struct_I.pack(length)) 00627 pattern = '<%sd'%length 00628 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position)) 00629 length = len(self.grasp.pre_grasp_posture.velocity) 00630 buff.write(_struct_I.pack(length)) 00631 pattern = '<%sd'%length 00632 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity)) 00633 length = len(self.grasp.pre_grasp_posture.effort) 00634 buff.write(_struct_I.pack(length)) 00635 pattern = '<%sd'%length 00636 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort)) 00637 _x = self 00638 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)) 00639 _x = self.grasp.grasp_posture.header.frame_id 00640 length = len(_x) 00641 buff.write(struct.pack('<I%ss'%length, length, _x)) 00642 length = len(self.grasp.grasp_posture.name) 00643 buff.write(_struct_I.pack(length)) 00644 for val1 in self.grasp.grasp_posture.name: 00645 length = len(val1) 00646 buff.write(struct.pack('<I%ss'%length, length, val1)) 00647 length = len(self.grasp.grasp_posture.position) 00648 buff.write(_struct_I.pack(length)) 00649 pattern = '<%sd'%length 00650 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position)) 00651 length = len(self.grasp.grasp_posture.velocity) 00652 buff.write(_struct_I.pack(length)) 00653 pattern = '<%sd'%length 00654 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity)) 00655 length = len(self.grasp.grasp_posture.effort) 00656 buff.write(_struct_I.pack(length)) 00657 pattern = '<%sd'%length 00658 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort)) 00659 _x = self 00660 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)) 00661 length = len(self.grasp.moved_obstacles) 00662 buff.write(_struct_I.pack(length)) 00663 for val1 in self.grasp.moved_obstacles: 00664 _x = val1.reference_frame_id 00665 length = len(_x) 00666 buff.write(struct.pack('<I%ss'%length, length, _x)) 00667 length = len(val1.potential_models) 00668 buff.write(_struct_I.pack(length)) 00669 for val2 in val1.potential_models: 00670 buff.write(_struct_i.pack(val2.model_id)) 00671 _v1 = val2.pose 00672 _v2 = _v1.header 00673 buff.write(_struct_I.pack(_v2.seq)) 00674 _v3 = _v2.stamp 00675 _x = _v3 00676 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00677 _x = _v2.frame_id 00678 length = len(_x) 00679 buff.write(struct.pack('<I%ss'%length, length, _x)) 00680 _v4 = _v1.pose 00681 _v5 = _v4.position 00682 _x = _v5 00683 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00684 _v6 = _v4.orientation 00685 _x = _v6 00686 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00687 buff.write(_struct_f.pack(val2.confidence)) 00688 _x = val2.detector_name 00689 length = len(_x) 00690 buff.write(struct.pack('<I%ss'%length, length, _x)) 00691 _v7 = val1.cluster 00692 _v8 = _v7.header 00693 buff.write(_struct_I.pack(_v8.seq)) 00694 _v9 = _v8.stamp 00695 _x = _v9 00696 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00697 _x = _v8.frame_id 00698 length = len(_x) 00699 buff.write(struct.pack('<I%ss'%length, length, _x)) 00700 length = len(_v7.points) 00701 buff.write(_struct_I.pack(length)) 00702 for val3 in _v7.points: 00703 _x = val3 00704 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00705 length = len(_v7.channels) 00706 buff.write(_struct_I.pack(length)) 00707 for val3 in _v7.channels: 00708 _x = val3.name 00709 length = len(_x) 00710 buff.write(struct.pack('<I%ss'%length, length, _x)) 00711 length = len(val3.values) 00712 buff.write(_struct_I.pack(length)) 00713 pattern = '<%sf'%length 00714 buff.write(struct.pack(pattern, *val3.values)) 00715 _v10 = val1.region 00716 _v11 = _v10.cloud 00717 _v12 = _v11.header 00718 buff.write(_struct_I.pack(_v12.seq)) 00719 _v13 = _v12.stamp 00720 _x = _v13 00721 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00722 _x = _v12.frame_id 00723 length = len(_x) 00724 buff.write(struct.pack('<I%ss'%length, length, _x)) 00725 _x = _v11 00726 buff.write(_struct_2I.pack(_x.height, _x.width)) 00727 length = len(_v11.fields) 00728 buff.write(_struct_I.pack(length)) 00729 for val4 in _v11.fields: 00730 _x = val4.name 00731 length = len(_x) 00732 buff.write(struct.pack('<I%ss'%length, length, _x)) 00733 _x = val4 00734 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00735 _x = _v11 00736 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 00737 _x = _v11.data 00738 length = len(_x) 00739 # - if encoded as a list instead, serialize as bytes instead of string 00740 if type(_x) in [list, tuple]: 00741 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00742 else: 00743 buff.write(struct.pack('<I%ss'%length, length, _x)) 00744 buff.write(_struct_B.pack(_v11.is_dense)) 00745 length = len(_v10.mask) 00746 buff.write(_struct_I.pack(length)) 00747 pattern = '<%si'%length 00748 buff.write(struct.pack(pattern, *_v10.mask)) 00749 _v14 = _v10.image 00750 _v15 = _v14.header 00751 buff.write(_struct_I.pack(_v15.seq)) 00752 _v16 = _v15.stamp 00753 _x = _v16 00754 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00755 _x = _v15.frame_id 00756 length = len(_x) 00757 buff.write(struct.pack('<I%ss'%length, length, _x)) 00758 _x = _v14 00759 buff.write(_struct_2I.pack(_x.height, _x.width)) 00760 _x = _v14.encoding 00761 length = len(_x) 00762 buff.write(struct.pack('<I%ss'%length, length, _x)) 00763 _x = _v14 00764 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00765 _x = _v14.data 00766 length = len(_x) 00767 # - if encoded as a list instead, serialize as bytes instead of string 00768 if type(_x) in [list, tuple]: 00769 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00770 else: 00771 buff.write(struct.pack('<I%ss'%length, length, _x)) 00772 _v17 = _v10.disparity_image 00773 _v18 = _v17.header 00774 buff.write(_struct_I.pack(_v18.seq)) 00775 _v19 = _v18.stamp 00776 _x = _v19 00777 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00778 _x = _v18.frame_id 00779 length = len(_x) 00780 buff.write(struct.pack('<I%ss'%length, length, _x)) 00781 _x = _v17 00782 buff.write(_struct_2I.pack(_x.height, _x.width)) 00783 _x = _v17.encoding 00784 length = len(_x) 00785 buff.write(struct.pack('<I%ss'%length, length, _x)) 00786 _x = _v17 00787 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00788 _x = _v17.data 00789 length = len(_x) 00790 # - if encoded as a list instead, serialize as bytes instead of string 00791 if type(_x) in [list, tuple]: 00792 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00793 else: 00794 buff.write(struct.pack('<I%ss'%length, length, _x)) 00795 _v20 = _v10.cam_info 00796 _v21 = _v20.header 00797 buff.write(_struct_I.pack(_v21.seq)) 00798 _v22 = _v21.stamp 00799 _x = _v22 00800 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00801 _x = _v21.frame_id 00802 length = len(_x) 00803 buff.write(struct.pack('<I%ss'%length, length, _x)) 00804 _x = _v20 00805 buff.write(_struct_2I.pack(_x.height, _x.width)) 00806 _x = _v20.distortion_model 00807 length = len(_x) 00808 buff.write(struct.pack('<I%ss'%length, length, _x)) 00809 length = len(_v20.D) 00810 buff.write(_struct_I.pack(length)) 00811 pattern = '<%sd'%length 00812 buff.write(struct.pack(pattern, *_v20.D)) 00813 buff.write(_struct_9d.pack(*_v20.K)) 00814 buff.write(_struct_9d.pack(*_v20.R)) 00815 buff.write(_struct_12d.pack(*_v20.P)) 00816 _x = _v20 00817 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00818 _v23 = _v20.roi 00819 _x = _v23 00820 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00821 _v24 = _v10.roi_box_pose 00822 _v25 = _v24.header 00823 buff.write(_struct_I.pack(_v25.seq)) 00824 _v26 = _v25.stamp 00825 _x = _v26 00826 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00827 _x = _v25.frame_id 00828 length = len(_x) 00829 buff.write(struct.pack('<I%ss'%length, length, _x)) 00830 _v27 = _v24.pose 00831 _v28 = _v27.position 00832 _x = _v28 00833 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00834 _v29 = _v27.orientation 00835 _x = _v29 00836 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00837 _v30 = _v10.roi_box_dims 00838 _x = _v30 00839 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00840 _x = val1.collision_name 00841 length = len(_x) 00842 buff.write(struct.pack('<I%ss'%length, length, _x)) 00843 length = len(self.attempted_grasps) 00844 buff.write(_struct_I.pack(length)) 00845 for val1 in self.attempted_grasps: 00846 _v31 = val1.pre_grasp_posture 00847 _v32 = _v31.header 00848 buff.write(_struct_I.pack(_v32.seq)) 00849 _v33 = _v32.stamp 00850 _x = _v33 00851 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00852 _x = _v32.frame_id 00853 length = len(_x) 00854 buff.write(struct.pack('<I%ss'%length, length, _x)) 00855 length = len(_v31.name) 00856 buff.write(_struct_I.pack(length)) 00857 for val3 in _v31.name: 00858 length = len(val3) 00859 buff.write(struct.pack('<I%ss'%length, length, val3)) 00860 length = len(_v31.position) 00861 buff.write(_struct_I.pack(length)) 00862 pattern = '<%sd'%length 00863 buff.write(struct.pack(pattern, *_v31.position)) 00864 length = len(_v31.velocity) 00865 buff.write(_struct_I.pack(length)) 00866 pattern = '<%sd'%length 00867 buff.write(struct.pack(pattern, *_v31.velocity)) 00868 length = len(_v31.effort) 00869 buff.write(_struct_I.pack(length)) 00870 pattern = '<%sd'%length 00871 buff.write(struct.pack(pattern, *_v31.effort)) 00872 _v34 = val1.grasp_posture 00873 _v35 = _v34.header 00874 buff.write(_struct_I.pack(_v35.seq)) 00875 _v36 = _v35.stamp 00876 _x = _v36 00877 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00878 _x = _v35.frame_id 00879 length = len(_x) 00880 buff.write(struct.pack('<I%ss'%length, length, _x)) 00881 length = len(_v34.name) 00882 buff.write(_struct_I.pack(length)) 00883 for val3 in _v34.name: 00884 length = len(val3) 00885 buff.write(struct.pack('<I%ss'%length, length, val3)) 00886 length = len(_v34.position) 00887 buff.write(_struct_I.pack(length)) 00888 pattern = '<%sd'%length 00889 buff.write(struct.pack(pattern, *_v34.position)) 00890 length = len(_v34.velocity) 00891 buff.write(_struct_I.pack(length)) 00892 pattern = '<%sd'%length 00893 buff.write(struct.pack(pattern, *_v34.velocity)) 00894 length = len(_v34.effort) 00895 buff.write(_struct_I.pack(length)) 00896 pattern = '<%sd'%length 00897 buff.write(struct.pack(pattern, *_v34.effort)) 00898 _v37 = val1.grasp_pose 00899 _v38 = _v37.position 00900 _x = _v38 00901 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00902 _v39 = _v37.orientation 00903 _x = _v39 00904 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00905 _x = val1 00906 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 00907 length = len(val1.moved_obstacles) 00908 buff.write(_struct_I.pack(length)) 00909 for val2 in val1.moved_obstacles: 00910 _x = val2.reference_frame_id 00911 length = len(_x) 00912 buff.write(struct.pack('<I%ss'%length, length, _x)) 00913 length = len(val2.potential_models) 00914 buff.write(_struct_I.pack(length)) 00915 for val3 in val2.potential_models: 00916 buff.write(_struct_i.pack(val3.model_id)) 00917 _v40 = val3.pose 00918 _v41 = _v40.header 00919 buff.write(_struct_I.pack(_v41.seq)) 00920 _v42 = _v41.stamp 00921 _x = _v42 00922 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00923 _x = _v41.frame_id 00924 length = len(_x) 00925 buff.write(struct.pack('<I%ss'%length, length, _x)) 00926 _v43 = _v40.pose 00927 _v44 = _v43.position 00928 _x = _v44 00929 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00930 _v45 = _v43.orientation 00931 _x = _v45 00932 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00933 buff.write(_struct_f.pack(val3.confidence)) 00934 _x = val3.detector_name 00935 length = len(_x) 00936 buff.write(struct.pack('<I%ss'%length, length, _x)) 00937 _v46 = val2.cluster 00938 _v47 = _v46.header 00939 buff.write(_struct_I.pack(_v47.seq)) 00940 _v48 = _v47.stamp 00941 _x = _v48 00942 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00943 _x = _v47.frame_id 00944 length = len(_x) 00945 buff.write(struct.pack('<I%ss'%length, length, _x)) 00946 length = len(_v46.points) 00947 buff.write(_struct_I.pack(length)) 00948 for val4 in _v46.points: 00949 _x = val4 00950 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00951 length = len(_v46.channels) 00952 buff.write(_struct_I.pack(length)) 00953 for val4 in _v46.channels: 00954 _x = val4.name 00955 length = len(_x) 00956 buff.write(struct.pack('<I%ss'%length, length, _x)) 00957 length = len(val4.values) 00958 buff.write(_struct_I.pack(length)) 00959 pattern = '<%sf'%length 00960 buff.write(struct.pack(pattern, *val4.values)) 00961 _v49 = val2.region 00962 _v50 = _v49.cloud 00963 _v51 = _v50.header 00964 buff.write(_struct_I.pack(_v51.seq)) 00965 _v52 = _v51.stamp 00966 _x = _v52 00967 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00968 _x = _v51.frame_id 00969 length = len(_x) 00970 buff.write(struct.pack('<I%ss'%length, length, _x)) 00971 _x = _v50 00972 buff.write(_struct_2I.pack(_x.height, _x.width)) 00973 length = len(_v50.fields) 00974 buff.write(_struct_I.pack(length)) 00975 for val5 in _v50.fields: 00976 _x = val5.name 00977 length = len(_x) 00978 buff.write(struct.pack('<I%ss'%length, length, _x)) 00979 _x = val5 00980 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00981 _x = _v50 00982 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 00983 _x = _v50.data 00984 length = len(_x) 00985 # - if encoded as a list instead, serialize as bytes instead of string 00986 if type(_x) in [list, tuple]: 00987 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00988 else: 00989 buff.write(struct.pack('<I%ss'%length, length, _x)) 00990 buff.write(_struct_B.pack(_v50.is_dense)) 00991 length = len(_v49.mask) 00992 buff.write(_struct_I.pack(length)) 00993 pattern = '<%si'%length 00994 buff.write(struct.pack(pattern, *_v49.mask)) 00995 _v53 = _v49.image 00996 _v54 = _v53.header 00997 buff.write(_struct_I.pack(_v54.seq)) 00998 _v55 = _v54.stamp 00999 _x = _v55 01000 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01001 _x = _v54.frame_id 01002 length = len(_x) 01003 buff.write(struct.pack('<I%ss'%length, length, _x)) 01004 _x = _v53 01005 buff.write(_struct_2I.pack(_x.height, _x.width)) 01006 _x = _v53.encoding 01007 length = len(_x) 01008 buff.write(struct.pack('<I%ss'%length, length, _x)) 01009 _x = _v53 01010 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01011 _x = _v53.data 01012 length = len(_x) 01013 # - if encoded as a list instead, serialize as bytes instead of string 01014 if type(_x) in [list, tuple]: 01015 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01016 else: 01017 buff.write(struct.pack('<I%ss'%length, length, _x)) 01018 _v56 = _v49.disparity_image 01019 _v57 = _v56.header 01020 buff.write(_struct_I.pack(_v57.seq)) 01021 _v58 = _v57.stamp 01022 _x = _v58 01023 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01024 _x = _v57.frame_id 01025 length = len(_x) 01026 buff.write(struct.pack('<I%ss'%length, length, _x)) 01027 _x = _v56 01028 buff.write(_struct_2I.pack(_x.height, _x.width)) 01029 _x = _v56.encoding 01030 length = len(_x) 01031 buff.write(struct.pack('<I%ss'%length, length, _x)) 01032 _x = _v56 01033 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01034 _x = _v56.data 01035 length = len(_x) 01036 # - if encoded as a list instead, serialize as bytes instead of string 01037 if type(_x) in [list, tuple]: 01038 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01039 else: 01040 buff.write(struct.pack('<I%ss'%length, length, _x)) 01041 _v59 = _v49.cam_info 01042 _v60 = _v59.header 01043 buff.write(_struct_I.pack(_v60.seq)) 01044 _v61 = _v60.stamp 01045 _x = _v61 01046 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01047 _x = _v60.frame_id 01048 length = len(_x) 01049 buff.write(struct.pack('<I%ss'%length, length, _x)) 01050 _x = _v59 01051 buff.write(_struct_2I.pack(_x.height, _x.width)) 01052 _x = _v59.distortion_model 01053 length = len(_x) 01054 buff.write(struct.pack('<I%ss'%length, length, _x)) 01055 length = len(_v59.D) 01056 buff.write(_struct_I.pack(length)) 01057 pattern = '<%sd'%length 01058 buff.write(struct.pack(pattern, *_v59.D)) 01059 buff.write(_struct_9d.pack(*_v59.K)) 01060 buff.write(_struct_9d.pack(*_v59.R)) 01061 buff.write(_struct_12d.pack(*_v59.P)) 01062 _x = _v59 01063 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01064 _v62 = _v59.roi 01065 _x = _v62 01066 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01067 _v63 = _v49.roi_box_pose 01068 _v64 = _v63.header 01069 buff.write(_struct_I.pack(_v64.seq)) 01070 _v65 = _v64.stamp 01071 _x = _v65 01072 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01073 _x = _v64.frame_id 01074 length = len(_x) 01075 buff.write(struct.pack('<I%ss'%length, length, _x)) 01076 _v66 = _v63.pose 01077 _v67 = _v66.position 01078 _x = _v67 01079 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01080 _v68 = _v66.orientation 01081 _x = _v68 01082 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01083 _v69 = _v49.roi_box_dims 01084 _x = _v69 01085 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01086 _x = val2.collision_name 01087 length = len(_x) 01088 buff.write(struct.pack('<I%ss'%length, length, _x)) 01089 length = len(self.attempted_grasp_results) 01090 buff.write(_struct_I.pack(length)) 01091 for val1 in self.attempted_grasp_results: 01092 _x = val1 01093 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible)) 01094 except struct.error as se: self._check_types(se) 01095 except TypeError as te: self._check_types(te) 01096 01097 def deserialize(self, str): 01098 """ 01099 unpack serialized message in str into this message instance 01100 @param str: byte array of serialized message 01101 @type str: str 01102 """ 01103 try: 01104 if self.manipulation_result is None: 01105 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult() 01106 if self.grasp is None: 01107 self.grasp = object_manipulation_msgs.msg.Grasp() 01108 end = 0 01109 _x = self 01110 start = end 01111 end += 16 01112 (_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end]) 01113 start = end 01114 end += 4 01115 (length,) = _struct_I.unpack(str[start:end]) 01116 start = end 01117 end += length 01118 self.grasp.pre_grasp_posture.header.frame_id = str[start:end] 01119 start = end 01120 end += 4 01121 (length,) = _struct_I.unpack(str[start:end]) 01122 self.grasp.pre_grasp_posture.name = [] 01123 for i in range(0, length): 01124 start = end 01125 end += 4 01126 (length,) = _struct_I.unpack(str[start:end]) 01127 start = end 01128 end += length 01129 val1 = str[start:end] 01130 self.grasp.pre_grasp_posture.name.append(val1) 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 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end]) 01138 start = end 01139 end += 4 01140 (length,) = _struct_I.unpack(str[start:end]) 01141 pattern = '<%sd'%length 01142 start = end 01143 end += struct.calcsize(pattern) 01144 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 01145 start = end 01146 end += 4 01147 (length,) = _struct_I.unpack(str[start:end]) 01148 pattern = '<%sd'%length 01149 start = end 01150 end += struct.calcsize(pattern) 01151 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end]) 01152 _x = self 01153 start = end 01154 end += 12 01155 (_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]) 01156 start = end 01157 end += 4 01158 (length,) = _struct_I.unpack(str[start:end]) 01159 start = end 01160 end += length 01161 self.grasp.grasp_posture.header.frame_id = str[start:end] 01162 start = end 01163 end += 4 01164 (length,) = _struct_I.unpack(str[start:end]) 01165 self.grasp.grasp_posture.name = [] 01166 for i in range(0, length): 01167 start = end 01168 end += 4 01169 (length,) = _struct_I.unpack(str[start:end]) 01170 start = end 01171 end += length 01172 val1 = str[start:end] 01173 self.grasp.grasp_posture.name.append(val1) 01174 start = end 01175 end += 4 01176 (length,) = _struct_I.unpack(str[start:end]) 01177 pattern = '<%sd'%length 01178 start = end 01179 end += struct.calcsize(pattern) 01180 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end]) 01181 start = end 01182 end += 4 01183 (length,) = _struct_I.unpack(str[start:end]) 01184 pattern = '<%sd'%length 01185 start = end 01186 end += struct.calcsize(pattern) 01187 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 01188 start = end 01189 end += 4 01190 (length,) = _struct_I.unpack(str[start:end]) 01191 pattern = '<%sd'%length 01192 start = end 01193 end += struct.calcsize(pattern) 01194 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end]) 01195 _x = self 01196 start = end 01197 end += 73 01198 (_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]) 01199 self.grasp.cluster_rep = bool(self.grasp.cluster_rep) 01200 start = end 01201 end += 4 01202 (length,) = _struct_I.unpack(str[start:end]) 01203 self.grasp.moved_obstacles = [] 01204 for i in range(0, length): 01205 val1 = object_manipulation_msgs.msg.GraspableObject() 01206 start = end 01207 end += 4 01208 (length,) = _struct_I.unpack(str[start:end]) 01209 start = end 01210 end += length 01211 val1.reference_frame_id = str[start:end] 01212 start = end 01213 end += 4 01214 (length,) = _struct_I.unpack(str[start:end]) 01215 val1.potential_models = [] 01216 for i in range(0, length): 01217 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 01218 start = end 01219 end += 4 01220 (val2.model_id,) = _struct_i.unpack(str[start:end]) 01221 _v70 = val2.pose 01222 _v71 = _v70.header 01223 start = end 01224 end += 4 01225 (_v71.seq,) = _struct_I.unpack(str[start:end]) 01226 _v72 = _v71.stamp 01227 _x = _v72 01228 start = end 01229 end += 8 01230 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01231 start = end 01232 end += 4 01233 (length,) = _struct_I.unpack(str[start:end]) 01234 start = end 01235 end += length 01236 _v71.frame_id = str[start:end] 01237 _v73 = _v70.pose 01238 _v74 = _v73.position 01239 _x = _v74 01240 start = end 01241 end += 24 01242 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01243 _v75 = _v73.orientation 01244 _x = _v75 01245 start = end 01246 end += 32 01247 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01248 start = end 01249 end += 4 01250 (val2.confidence,) = _struct_f.unpack(str[start:end]) 01251 start = end 01252 end += 4 01253 (length,) = _struct_I.unpack(str[start:end]) 01254 start = end 01255 end += length 01256 val2.detector_name = str[start:end] 01257 val1.potential_models.append(val2) 01258 _v76 = val1.cluster 01259 _v77 = _v76.header 01260 start = end 01261 end += 4 01262 (_v77.seq,) = _struct_I.unpack(str[start:end]) 01263 _v78 = _v77.stamp 01264 _x = _v78 01265 start = end 01266 end += 8 01267 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01268 start = end 01269 end += 4 01270 (length,) = _struct_I.unpack(str[start:end]) 01271 start = end 01272 end += length 01273 _v77.frame_id = str[start:end] 01274 start = end 01275 end += 4 01276 (length,) = _struct_I.unpack(str[start:end]) 01277 _v76.points = [] 01278 for i in range(0, length): 01279 val3 = geometry_msgs.msg.Point32() 01280 _x = val3 01281 start = end 01282 end += 12 01283 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01284 _v76.points.append(val3) 01285 start = end 01286 end += 4 01287 (length,) = _struct_I.unpack(str[start:end]) 01288 _v76.channels = [] 01289 for i in range(0, length): 01290 val3 = sensor_msgs.msg.ChannelFloat32() 01291 start = end 01292 end += 4 01293 (length,) = _struct_I.unpack(str[start:end]) 01294 start = end 01295 end += length 01296 val3.name = str[start:end] 01297 start = end 01298 end += 4 01299 (length,) = _struct_I.unpack(str[start:end]) 01300 pattern = '<%sf'%length 01301 start = end 01302 end += struct.calcsize(pattern) 01303 val3.values = struct.unpack(pattern, str[start:end]) 01304 _v76.channels.append(val3) 01305 _v79 = val1.region 01306 _v80 = _v79.cloud 01307 _v81 = _v80.header 01308 start = end 01309 end += 4 01310 (_v81.seq,) = _struct_I.unpack(str[start:end]) 01311 _v82 = _v81.stamp 01312 _x = _v82 01313 start = end 01314 end += 8 01315 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01316 start = end 01317 end += 4 01318 (length,) = _struct_I.unpack(str[start:end]) 01319 start = end 01320 end += length 01321 _v81.frame_id = str[start:end] 01322 _x = _v80 01323 start = end 01324 end += 8 01325 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01326 start = end 01327 end += 4 01328 (length,) = _struct_I.unpack(str[start:end]) 01329 _v80.fields = [] 01330 for i in range(0, length): 01331 val4 = sensor_msgs.msg.PointField() 01332 start = end 01333 end += 4 01334 (length,) = _struct_I.unpack(str[start:end]) 01335 start = end 01336 end += length 01337 val4.name = str[start:end] 01338 _x = val4 01339 start = end 01340 end += 9 01341 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01342 _v80.fields.append(val4) 01343 _x = _v80 01344 start = end 01345 end += 9 01346 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01347 _v80.is_bigendian = bool(_v80.is_bigendian) 01348 start = end 01349 end += 4 01350 (length,) = _struct_I.unpack(str[start:end]) 01351 start = end 01352 end += length 01353 _v80.data = str[start:end] 01354 start = end 01355 end += 1 01356 (_v80.is_dense,) = _struct_B.unpack(str[start:end]) 01357 _v80.is_dense = bool(_v80.is_dense) 01358 start = end 01359 end += 4 01360 (length,) = _struct_I.unpack(str[start:end]) 01361 pattern = '<%si'%length 01362 start = end 01363 end += struct.calcsize(pattern) 01364 _v79.mask = struct.unpack(pattern, str[start:end]) 01365 _v83 = _v79.image 01366 _v84 = _v83.header 01367 start = end 01368 end += 4 01369 (_v84.seq,) = _struct_I.unpack(str[start:end]) 01370 _v85 = _v84.stamp 01371 _x = _v85 01372 start = end 01373 end += 8 01374 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01375 start = end 01376 end += 4 01377 (length,) = _struct_I.unpack(str[start:end]) 01378 start = end 01379 end += length 01380 _v84.frame_id = str[start:end] 01381 _x = _v83 01382 start = end 01383 end += 8 01384 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01385 start = end 01386 end += 4 01387 (length,) = _struct_I.unpack(str[start:end]) 01388 start = end 01389 end += length 01390 _v83.encoding = str[start:end] 01391 _x = _v83 01392 start = end 01393 end += 5 01394 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01395 start = end 01396 end += 4 01397 (length,) = _struct_I.unpack(str[start:end]) 01398 start = end 01399 end += length 01400 _v83.data = str[start:end] 01401 _v86 = _v79.disparity_image 01402 _v87 = _v86.header 01403 start = end 01404 end += 4 01405 (_v87.seq,) = _struct_I.unpack(str[start:end]) 01406 _v88 = _v87.stamp 01407 _x = _v88 01408 start = end 01409 end += 8 01410 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01411 start = end 01412 end += 4 01413 (length,) = _struct_I.unpack(str[start:end]) 01414 start = end 01415 end += length 01416 _v87.frame_id = str[start:end] 01417 _x = _v86 01418 start = end 01419 end += 8 01420 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01421 start = end 01422 end += 4 01423 (length,) = _struct_I.unpack(str[start:end]) 01424 start = end 01425 end += length 01426 _v86.encoding = str[start:end] 01427 _x = _v86 01428 start = end 01429 end += 5 01430 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01431 start = end 01432 end += 4 01433 (length,) = _struct_I.unpack(str[start:end]) 01434 start = end 01435 end += length 01436 _v86.data = str[start:end] 01437 _v89 = _v79.cam_info 01438 _v90 = _v89.header 01439 start = end 01440 end += 4 01441 (_v90.seq,) = _struct_I.unpack(str[start:end]) 01442 _v91 = _v90.stamp 01443 _x = _v91 01444 start = end 01445 end += 8 01446 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01447 start = end 01448 end += 4 01449 (length,) = _struct_I.unpack(str[start:end]) 01450 start = end 01451 end += length 01452 _v90.frame_id = str[start:end] 01453 _x = _v89 01454 start = end 01455 end += 8 01456 (_x.height, _x.width,) = _struct_2I.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 _v89.distortion_model = str[start:end] 01463 start = end 01464 end += 4 01465 (length,) = _struct_I.unpack(str[start:end]) 01466 pattern = '<%sd'%length 01467 start = end 01468 end += struct.calcsize(pattern) 01469 _v89.D = struct.unpack(pattern, str[start:end]) 01470 start = end 01471 end += 72 01472 _v89.K = _struct_9d.unpack(str[start:end]) 01473 start = end 01474 end += 72 01475 _v89.R = _struct_9d.unpack(str[start:end]) 01476 start = end 01477 end += 96 01478 _v89.P = _struct_12d.unpack(str[start:end]) 01479 _x = _v89 01480 start = end 01481 end += 8 01482 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01483 _v92 = _v89.roi 01484 _x = _v92 01485 start = end 01486 end += 17 01487 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01488 _v92.do_rectify = bool(_v92.do_rectify) 01489 _v93 = _v79.roi_box_pose 01490 _v94 = _v93.header 01491 start = end 01492 end += 4 01493 (_v94.seq,) = _struct_I.unpack(str[start:end]) 01494 _v95 = _v94.stamp 01495 _x = _v95 01496 start = end 01497 end += 8 01498 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01499 start = end 01500 end += 4 01501 (length,) = _struct_I.unpack(str[start:end]) 01502 start = end 01503 end += length 01504 _v94.frame_id = str[start:end] 01505 _v96 = _v93.pose 01506 _v97 = _v96.position 01507 _x = _v97 01508 start = end 01509 end += 24 01510 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01511 _v98 = _v96.orientation 01512 _x = _v98 01513 start = end 01514 end += 32 01515 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01516 _v99 = _v79.roi_box_dims 01517 _x = _v99 01518 start = end 01519 end += 24 01520 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01521 start = end 01522 end += 4 01523 (length,) = _struct_I.unpack(str[start:end]) 01524 start = end 01525 end += length 01526 val1.collision_name = str[start:end] 01527 self.grasp.moved_obstacles.append(val1) 01528 start = end 01529 end += 4 01530 (length,) = _struct_I.unpack(str[start:end]) 01531 self.attempted_grasps = [] 01532 for i in range(0, length): 01533 val1 = object_manipulation_msgs.msg.Grasp() 01534 _v100 = val1.pre_grasp_posture 01535 _v101 = _v100.header 01536 start = end 01537 end += 4 01538 (_v101.seq,) = _struct_I.unpack(str[start:end]) 01539 _v102 = _v101.stamp 01540 _x = _v102 01541 start = end 01542 end += 8 01543 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01544 start = end 01545 end += 4 01546 (length,) = _struct_I.unpack(str[start:end]) 01547 start = end 01548 end += length 01549 _v101.frame_id = str[start:end] 01550 start = end 01551 end += 4 01552 (length,) = _struct_I.unpack(str[start:end]) 01553 _v100.name = [] 01554 for i in range(0, length): 01555 start = end 01556 end += 4 01557 (length,) = _struct_I.unpack(str[start:end]) 01558 start = end 01559 end += length 01560 val3 = str[start:end] 01561 _v100.name.append(val3) 01562 start = end 01563 end += 4 01564 (length,) = _struct_I.unpack(str[start:end]) 01565 pattern = '<%sd'%length 01566 start = end 01567 end += struct.calcsize(pattern) 01568 _v100.position = struct.unpack(pattern, str[start:end]) 01569 start = end 01570 end += 4 01571 (length,) = _struct_I.unpack(str[start:end]) 01572 pattern = '<%sd'%length 01573 start = end 01574 end += struct.calcsize(pattern) 01575 _v100.velocity = struct.unpack(pattern, str[start:end]) 01576 start = end 01577 end += 4 01578 (length,) = _struct_I.unpack(str[start:end]) 01579 pattern = '<%sd'%length 01580 start = end 01581 end += struct.calcsize(pattern) 01582 _v100.effort = struct.unpack(pattern, str[start:end]) 01583 _v103 = val1.grasp_posture 01584 _v104 = _v103.header 01585 start = end 01586 end += 4 01587 (_v104.seq,) = _struct_I.unpack(str[start:end]) 01588 _v105 = _v104.stamp 01589 _x = _v105 01590 start = end 01591 end += 8 01592 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01593 start = end 01594 end += 4 01595 (length,) = _struct_I.unpack(str[start:end]) 01596 start = end 01597 end += length 01598 _v104.frame_id = str[start:end] 01599 start = end 01600 end += 4 01601 (length,) = _struct_I.unpack(str[start:end]) 01602 _v103.name = [] 01603 for i in range(0, length): 01604 start = end 01605 end += 4 01606 (length,) = _struct_I.unpack(str[start:end]) 01607 start = end 01608 end += length 01609 val3 = str[start:end] 01610 _v103.name.append(val3) 01611 start = end 01612 end += 4 01613 (length,) = _struct_I.unpack(str[start:end]) 01614 pattern = '<%sd'%length 01615 start = end 01616 end += struct.calcsize(pattern) 01617 _v103.position = struct.unpack(pattern, str[start:end]) 01618 start = end 01619 end += 4 01620 (length,) = _struct_I.unpack(str[start:end]) 01621 pattern = '<%sd'%length 01622 start = end 01623 end += struct.calcsize(pattern) 01624 _v103.velocity = struct.unpack(pattern, str[start:end]) 01625 start = end 01626 end += 4 01627 (length,) = _struct_I.unpack(str[start:end]) 01628 pattern = '<%sd'%length 01629 start = end 01630 end += struct.calcsize(pattern) 01631 _v103.effort = struct.unpack(pattern, str[start:end]) 01632 _v106 = val1.grasp_pose 01633 _v107 = _v106.position 01634 _x = _v107 01635 start = end 01636 end += 24 01637 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01638 _v108 = _v106.orientation 01639 _x = _v108 01640 start = end 01641 end += 32 01642 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01643 _x = val1 01644 start = end 01645 end += 17 01646 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 01647 val1.cluster_rep = bool(val1.cluster_rep) 01648 start = end 01649 end += 4 01650 (length,) = _struct_I.unpack(str[start:end]) 01651 val1.moved_obstacles = [] 01652 for i in range(0, length): 01653 val2 = object_manipulation_msgs.msg.GraspableObject() 01654 start = end 01655 end += 4 01656 (length,) = _struct_I.unpack(str[start:end]) 01657 start = end 01658 end += length 01659 val2.reference_frame_id = str[start:end] 01660 start = end 01661 end += 4 01662 (length,) = _struct_I.unpack(str[start:end]) 01663 val2.potential_models = [] 01664 for i in range(0, length): 01665 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 01666 start = end 01667 end += 4 01668 (val3.model_id,) = _struct_i.unpack(str[start:end]) 01669 _v109 = val3.pose 01670 _v110 = _v109.header 01671 start = end 01672 end += 4 01673 (_v110.seq,) = _struct_I.unpack(str[start:end]) 01674 _v111 = _v110.stamp 01675 _x = _v111 01676 start = end 01677 end += 8 01678 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01679 start = end 01680 end += 4 01681 (length,) = _struct_I.unpack(str[start:end]) 01682 start = end 01683 end += length 01684 _v110.frame_id = str[start:end] 01685 _v112 = _v109.pose 01686 _v113 = _v112.position 01687 _x = _v113 01688 start = end 01689 end += 24 01690 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01691 _v114 = _v112.orientation 01692 _x = _v114 01693 start = end 01694 end += 32 01695 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01696 start = end 01697 end += 4 01698 (val3.confidence,) = _struct_f.unpack(str[start:end]) 01699 start = end 01700 end += 4 01701 (length,) = _struct_I.unpack(str[start:end]) 01702 start = end 01703 end += length 01704 val3.detector_name = str[start:end] 01705 val2.potential_models.append(val3) 01706 _v115 = val2.cluster 01707 _v116 = _v115.header 01708 start = end 01709 end += 4 01710 (_v116.seq,) = _struct_I.unpack(str[start:end]) 01711 _v117 = _v116.stamp 01712 _x = _v117 01713 start = end 01714 end += 8 01715 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01716 start = end 01717 end += 4 01718 (length,) = _struct_I.unpack(str[start:end]) 01719 start = end 01720 end += length 01721 _v116.frame_id = str[start:end] 01722 start = end 01723 end += 4 01724 (length,) = _struct_I.unpack(str[start:end]) 01725 _v115.points = [] 01726 for i in range(0, length): 01727 val4 = geometry_msgs.msg.Point32() 01728 _x = val4 01729 start = end 01730 end += 12 01731 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01732 _v115.points.append(val4) 01733 start = end 01734 end += 4 01735 (length,) = _struct_I.unpack(str[start:end]) 01736 _v115.channels = [] 01737 for i in range(0, length): 01738 val4 = sensor_msgs.msg.ChannelFloat32() 01739 start = end 01740 end += 4 01741 (length,) = _struct_I.unpack(str[start:end]) 01742 start = end 01743 end += length 01744 val4.name = str[start:end] 01745 start = end 01746 end += 4 01747 (length,) = _struct_I.unpack(str[start:end]) 01748 pattern = '<%sf'%length 01749 start = end 01750 end += struct.calcsize(pattern) 01751 val4.values = struct.unpack(pattern, str[start:end]) 01752 _v115.channels.append(val4) 01753 _v118 = val2.region 01754 _v119 = _v118.cloud 01755 _v120 = _v119.header 01756 start = end 01757 end += 4 01758 (_v120.seq,) = _struct_I.unpack(str[start:end]) 01759 _v121 = _v120.stamp 01760 _x = _v121 01761 start = end 01762 end += 8 01763 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01764 start = end 01765 end += 4 01766 (length,) = _struct_I.unpack(str[start:end]) 01767 start = end 01768 end += length 01769 _v120.frame_id = str[start:end] 01770 _x = _v119 01771 start = end 01772 end += 8 01773 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01774 start = end 01775 end += 4 01776 (length,) = _struct_I.unpack(str[start:end]) 01777 _v119.fields = [] 01778 for i in range(0, length): 01779 val5 = sensor_msgs.msg.PointField() 01780 start = end 01781 end += 4 01782 (length,) = _struct_I.unpack(str[start:end]) 01783 start = end 01784 end += length 01785 val5.name = str[start:end] 01786 _x = val5 01787 start = end 01788 end += 9 01789 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01790 _v119.fields.append(val5) 01791 _x = _v119 01792 start = end 01793 end += 9 01794 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01795 _v119.is_bigendian = bool(_v119.is_bigendian) 01796 start = end 01797 end += 4 01798 (length,) = _struct_I.unpack(str[start:end]) 01799 start = end 01800 end += length 01801 _v119.data = str[start:end] 01802 start = end 01803 end += 1 01804 (_v119.is_dense,) = _struct_B.unpack(str[start:end]) 01805 _v119.is_dense = bool(_v119.is_dense) 01806 start = end 01807 end += 4 01808 (length,) = _struct_I.unpack(str[start:end]) 01809 pattern = '<%si'%length 01810 start = end 01811 end += struct.calcsize(pattern) 01812 _v118.mask = struct.unpack(pattern, str[start:end]) 01813 _v122 = _v118.image 01814 _v123 = _v122.header 01815 start = end 01816 end += 4 01817 (_v123.seq,) = _struct_I.unpack(str[start:end]) 01818 _v124 = _v123.stamp 01819 _x = _v124 01820 start = end 01821 end += 8 01822 (_x.secs, _x.nsecs,) = _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 _v123.frame_id = str[start:end] 01829 _x = _v122 01830 start = end 01831 end += 8 01832 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01833 start = end 01834 end += 4 01835 (length,) = _struct_I.unpack(str[start:end]) 01836 start = end 01837 end += length 01838 _v122.encoding = str[start:end] 01839 _x = _v122 01840 start = end 01841 end += 5 01842 (_x.is_bigendian, _x.step,) = _struct_BI.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 _v122.data = str[start:end] 01849 _v125 = _v118.disparity_image 01850 _v126 = _v125.header 01851 start = end 01852 end += 4 01853 (_v126.seq,) = _struct_I.unpack(str[start:end]) 01854 _v127 = _v126.stamp 01855 _x = _v127 01856 start = end 01857 end += 8 01858 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01859 start = end 01860 end += 4 01861 (length,) = _struct_I.unpack(str[start:end]) 01862 start = end 01863 end += length 01864 _v126.frame_id = str[start:end] 01865 _x = _v125 01866 start = end 01867 end += 8 01868 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01869 start = end 01870 end += 4 01871 (length,) = _struct_I.unpack(str[start:end]) 01872 start = end 01873 end += length 01874 _v125.encoding = str[start:end] 01875 _x = _v125 01876 start = end 01877 end += 5 01878 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01879 start = end 01880 end += 4 01881 (length,) = _struct_I.unpack(str[start:end]) 01882 start = end 01883 end += length 01884 _v125.data = str[start:end] 01885 _v128 = _v118.cam_info 01886 _v129 = _v128.header 01887 start = end 01888 end += 4 01889 (_v129.seq,) = _struct_I.unpack(str[start:end]) 01890 _v130 = _v129.stamp 01891 _x = _v130 01892 start = end 01893 end += 8 01894 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01895 start = end 01896 end += 4 01897 (length,) = _struct_I.unpack(str[start:end]) 01898 start = end 01899 end += length 01900 _v129.frame_id = str[start:end] 01901 _x = _v128 01902 start = end 01903 end += 8 01904 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01905 start = end 01906 end += 4 01907 (length,) = _struct_I.unpack(str[start:end]) 01908 start = end 01909 end += length 01910 _v128.distortion_model = str[start:end] 01911 start = end 01912 end += 4 01913 (length,) = _struct_I.unpack(str[start:end]) 01914 pattern = '<%sd'%length 01915 start = end 01916 end += struct.calcsize(pattern) 01917 _v128.D = struct.unpack(pattern, str[start:end]) 01918 start = end 01919 end += 72 01920 _v128.K = _struct_9d.unpack(str[start:end]) 01921 start = end 01922 end += 72 01923 _v128.R = _struct_9d.unpack(str[start:end]) 01924 start = end 01925 end += 96 01926 _v128.P = _struct_12d.unpack(str[start:end]) 01927 _x = _v128 01928 start = end 01929 end += 8 01930 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01931 _v131 = _v128.roi 01932 _x = _v131 01933 start = end 01934 end += 17 01935 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01936 _v131.do_rectify = bool(_v131.do_rectify) 01937 _v132 = _v118.roi_box_pose 01938 _v133 = _v132.header 01939 start = end 01940 end += 4 01941 (_v133.seq,) = _struct_I.unpack(str[start:end]) 01942 _v134 = _v133.stamp 01943 _x = _v134 01944 start = end 01945 end += 8 01946 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01947 start = end 01948 end += 4 01949 (length,) = _struct_I.unpack(str[start:end]) 01950 start = end 01951 end += length 01952 _v133.frame_id = str[start:end] 01953 _v135 = _v132.pose 01954 _v136 = _v135.position 01955 _x = _v136 01956 start = end 01957 end += 24 01958 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01959 _v137 = _v135.orientation 01960 _x = _v137 01961 start = end 01962 end += 32 01963 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01964 _v138 = _v118.roi_box_dims 01965 _x = _v138 01966 start = end 01967 end += 24 01968 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01969 start = end 01970 end += 4 01971 (length,) = _struct_I.unpack(str[start:end]) 01972 start = end 01973 end += length 01974 val2.collision_name = str[start:end] 01975 val1.moved_obstacles.append(val2) 01976 self.attempted_grasps.append(val1) 01977 start = end 01978 end += 4 01979 (length,) = _struct_I.unpack(str[start:end]) 01980 self.attempted_grasp_results = [] 01981 for i in range(0, length): 01982 val1 = object_manipulation_msgs.msg.GraspResult() 01983 _x = val1 01984 start = end 01985 end += 5 01986 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end]) 01987 val1.continuation_possible = bool(val1.continuation_possible) 01988 self.attempted_grasp_results.append(val1) 01989 return self 01990 except struct.error as e: 01991 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01992 01993 01994 def serialize_numpy(self, buff, numpy): 01995 """ 01996 serialize message with numpy array types into buffer 01997 @param buff: buffer 01998 @type buff: StringIO 01999 @param numpy: numpy python module 02000 @type numpy module 02001 """ 02002 try: 02003 _x = self 02004 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs)) 02005 _x = self.grasp.pre_grasp_posture.header.frame_id 02006 length = len(_x) 02007 buff.write(struct.pack('<I%ss'%length, length, _x)) 02008 length = len(self.grasp.pre_grasp_posture.name) 02009 buff.write(_struct_I.pack(length)) 02010 for val1 in self.grasp.pre_grasp_posture.name: 02011 length = len(val1) 02012 buff.write(struct.pack('<I%ss'%length, length, val1)) 02013 length = len(self.grasp.pre_grasp_posture.position) 02014 buff.write(_struct_I.pack(length)) 02015 pattern = '<%sd'%length 02016 buff.write(self.grasp.pre_grasp_posture.position.tostring()) 02017 length = len(self.grasp.pre_grasp_posture.velocity) 02018 buff.write(_struct_I.pack(length)) 02019 pattern = '<%sd'%length 02020 buff.write(self.grasp.pre_grasp_posture.velocity.tostring()) 02021 length = len(self.grasp.pre_grasp_posture.effort) 02022 buff.write(_struct_I.pack(length)) 02023 pattern = '<%sd'%length 02024 buff.write(self.grasp.pre_grasp_posture.effort.tostring()) 02025 _x = self 02026 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)) 02027 _x = self.grasp.grasp_posture.header.frame_id 02028 length = len(_x) 02029 buff.write(struct.pack('<I%ss'%length, length, _x)) 02030 length = len(self.grasp.grasp_posture.name) 02031 buff.write(_struct_I.pack(length)) 02032 for val1 in self.grasp.grasp_posture.name: 02033 length = len(val1) 02034 buff.write(struct.pack('<I%ss'%length, length, val1)) 02035 length = len(self.grasp.grasp_posture.position) 02036 buff.write(_struct_I.pack(length)) 02037 pattern = '<%sd'%length 02038 buff.write(self.grasp.grasp_posture.position.tostring()) 02039 length = len(self.grasp.grasp_posture.velocity) 02040 buff.write(_struct_I.pack(length)) 02041 pattern = '<%sd'%length 02042 buff.write(self.grasp.grasp_posture.velocity.tostring()) 02043 length = len(self.grasp.grasp_posture.effort) 02044 buff.write(_struct_I.pack(length)) 02045 pattern = '<%sd'%length 02046 buff.write(self.grasp.grasp_posture.effort.tostring()) 02047 _x = self 02048 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)) 02049 length = len(self.grasp.moved_obstacles) 02050 buff.write(_struct_I.pack(length)) 02051 for val1 in self.grasp.moved_obstacles: 02052 _x = val1.reference_frame_id 02053 length = len(_x) 02054 buff.write(struct.pack('<I%ss'%length, length, _x)) 02055 length = len(val1.potential_models) 02056 buff.write(_struct_I.pack(length)) 02057 for val2 in val1.potential_models: 02058 buff.write(_struct_i.pack(val2.model_id)) 02059 _v139 = val2.pose 02060 _v140 = _v139.header 02061 buff.write(_struct_I.pack(_v140.seq)) 02062 _v141 = _v140.stamp 02063 _x = _v141 02064 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02065 _x = _v140.frame_id 02066 length = len(_x) 02067 buff.write(struct.pack('<I%ss'%length, length, _x)) 02068 _v142 = _v139.pose 02069 _v143 = _v142.position 02070 _x = _v143 02071 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02072 _v144 = _v142.orientation 02073 _x = _v144 02074 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02075 buff.write(_struct_f.pack(val2.confidence)) 02076 _x = val2.detector_name 02077 length = len(_x) 02078 buff.write(struct.pack('<I%ss'%length, length, _x)) 02079 _v145 = val1.cluster 02080 _v146 = _v145.header 02081 buff.write(_struct_I.pack(_v146.seq)) 02082 _v147 = _v146.stamp 02083 _x = _v147 02084 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02085 _x = _v146.frame_id 02086 length = len(_x) 02087 buff.write(struct.pack('<I%ss'%length, length, _x)) 02088 length = len(_v145.points) 02089 buff.write(_struct_I.pack(length)) 02090 for val3 in _v145.points: 02091 _x = val3 02092 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02093 length = len(_v145.channels) 02094 buff.write(_struct_I.pack(length)) 02095 for val3 in _v145.channels: 02096 _x = val3.name 02097 length = len(_x) 02098 buff.write(struct.pack('<I%ss'%length, length, _x)) 02099 length = len(val3.values) 02100 buff.write(_struct_I.pack(length)) 02101 pattern = '<%sf'%length 02102 buff.write(val3.values.tostring()) 02103 _v148 = val1.region 02104 _v149 = _v148.cloud 02105 _v150 = _v149.header 02106 buff.write(_struct_I.pack(_v150.seq)) 02107 _v151 = _v150.stamp 02108 _x = _v151 02109 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02110 _x = _v150.frame_id 02111 length = len(_x) 02112 buff.write(struct.pack('<I%ss'%length, length, _x)) 02113 _x = _v149 02114 buff.write(_struct_2I.pack(_x.height, _x.width)) 02115 length = len(_v149.fields) 02116 buff.write(_struct_I.pack(length)) 02117 for val4 in _v149.fields: 02118 _x = val4.name 02119 length = len(_x) 02120 buff.write(struct.pack('<I%ss'%length, length, _x)) 02121 _x = val4 02122 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02123 _x = _v149 02124 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 02125 _x = _v149.data 02126 length = len(_x) 02127 # - if encoded as a list instead, serialize as bytes instead of string 02128 if type(_x) in [list, tuple]: 02129 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02130 else: 02131 buff.write(struct.pack('<I%ss'%length, length, _x)) 02132 buff.write(_struct_B.pack(_v149.is_dense)) 02133 length = len(_v148.mask) 02134 buff.write(_struct_I.pack(length)) 02135 pattern = '<%si'%length 02136 buff.write(_v148.mask.tostring()) 02137 _v152 = _v148.image 02138 _v153 = _v152.header 02139 buff.write(_struct_I.pack(_v153.seq)) 02140 _v154 = _v153.stamp 02141 _x = _v154 02142 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02143 _x = _v153.frame_id 02144 length = len(_x) 02145 buff.write(struct.pack('<I%ss'%length, length, _x)) 02146 _x = _v152 02147 buff.write(_struct_2I.pack(_x.height, _x.width)) 02148 _x = _v152.encoding 02149 length = len(_x) 02150 buff.write(struct.pack('<I%ss'%length, length, _x)) 02151 _x = _v152 02152 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02153 _x = _v152.data 02154 length = len(_x) 02155 # - if encoded as a list instead, serialize as bytes instead of string 02156 if type(_x) in [list, tuple]: 02157 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02158 else: 02159 buff.write(struct.pack('<I%ss'%length, length, _x)) 02160 _v155 = _v148.disparity_image 02161 _v156 = _v155.header 02162 buff.write(_struct_I.pack(_v156.seq)) 02163 _v157 = _v156.stamp 02164 _x = _v157 02165 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02166 _x = _v156.frame_id 02167 length = len(_x) 02168 buff.write(struct.pack('<I%ss'%length, length, _x)) 02169 _x = _v155 02170 buff.write(_struct_2I.pack(_x.height, _x.width)) 02171 _x = _v155.encoding 02172 length = len(_x) 02173 buff.write(struct.pack('<I%ss'%length, length, _x)) 02174 _x = _v155 02175 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02176 _x = _v155.data 02177 length = len(_x) 02178 # - if encoded as a list instead, serialize as bytes instead of string 02179 if type(_x) in [list, tuple]: 02180 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02181 else: 02182 buff.write(struct.pack('<I%ss'%length, length, _x)) 02183 _v158 = _v148.cam_info 02184 _v159 = _v158.header 02185 buff.write(_struct_I.pack(_v159.seq)) 02186 _v160 = _v159.stamp 02187 _x = _v160 02188 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02189 _x = _v159.frame_id 02190 length = len(_x) 02191 buff.write(struct.pack('<I%ss'%length, length, _x)) 02192 _x = _v158 02193 buff.write(_struct_2I.pack(_x.height, _x.width)) 02194 _x = _v158.distortion_model 02195 length = len(_x) 02196 buff.write(struct.pack('<I%ss'%length, length, _x)) 02197 length = len(_v158.D) 02198 buff.write(_struct_I.pack(length)) 02199 pattern = '<%sd'%length 02200 buff.write(_v158.D.tostring()) 02201 buff.write(_v158.K.tostring()) 02202 buff.write(_v158.R.tostring()) 02203 buff.write(_v158.P.tostring()) 02204 _x = _v158 02205 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02206 _v161 = _v158.roi 02207 _x = _v161 02208 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02209 _v162 = _v148.roi_box_pose 02210 _v163 = _v162.header 02211 buff.write(_struct_I.pack(_v163.seq)) 02212 _v164 = _v163.stamp 02213 _x = _v164 02214 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02215 _x = _v163.frame_id 02216 length = len(_x) 02217 buff.write(struct.pack('<I%ss'%length, length, _x)) 02218 _v165 = _v162.pose 02219 _v166 = _v165.position 02220 _x = _v166 02221 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02222 _v167 = _v165.orientation 02223 _x = _v167 02224 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02225 _v168 = _v148.roi_box_dims 02226 _x = _v168 02227 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02228 _x = val1.collision_name 02229 length = len(_x) 02230 buff.write(struct.pack('<I%ss'%length, length, _x)) 02231 length = len(self.attempted_grasps) 02232 buff.write(_struct_I.pack(length)) 02233 for val1 in self.attempted_grasps: 02234 _v169 = val1.pre_grasp_posture 02235 _v170 = _v169.header 02236 buff.write(_struct_I.pack(_v170.seq)) 02237 _v171 = _v170.stamp 02238 _x = _v171 02239 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02240 _x = _v170.frame_id 02241 length = len(_x) 02242 buff.write(struct.pack('<I%ss'%length, length, _x)) 02243 length = len(_v169.name) 02244 buff.write(_struct_I.pack(length)) 02245 for val3 in _v169.name: 02246 length = len(val3) 02247 buff.write(struct.pack('<I%ss'%length, length, val3)) 02248 length = len(_v169.position) 02249 buff.write(_struct_I.pack(length)) 02250 pattern = '<%sd'%length 02251 buff.write(_v169.position.tostring()) 02252 length = len(_v169.velocity) 02253 buff.write(_struct_I.pack(length)) 02254 pattern = '<%sd'%length 02255 buff.write(_v169.velocity.tostring()) 02256 length = len(_v169.effort) 02257 buff.write(_struct_I.pack(length)) 02258 pattern = '<%sd'%length 02259 buff.write(_v169.effort.tostring()) 02260 _v172 = val1.grasp_posture 02261 _v173 = _v172.header 02262 buff.write(_struct_I.pack(_v173.seq)) 02263 _v174 = _v173.stamp 02264 _x = _v174 02265 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02266 _x = _v173.frame_id 02267 length = len(_x) 02268 buff.write(struct.pack('<I%ss'%length, length, _x)) 02269 length = len(_v172.name) 02270 buff.write(_struct_I.pack(length)) 02271 for val3 in _v172.name: 02272 length = len(val3) 02273 buff.write(struct.pack('<I%ss'%length, length, val3)) 02274 length = len(_v172.position) 02275 buff.write(_struct_I.pack(length)) 02276 pattern = '<%sd'%length 02277 buff.write(_v172.position.tostring()) 02278 length = len(_v172.velocity) 02279 buff.write(_struct_I.pack(length)) 02280 pattern = '<%sd'%length 02281 buff.write(_v172.velocity.tostring()) 02282 length = len(_v172.effort) 02283 buff.write(_struct_I.pack(length)) 02284 pattern = '<%sd'%length 02285 buff.write(_v172.effort.tostring()) 02286 _v175 = val1.grasp_pose 02287 _v176 = _v175.position 02288 _x = _v176 02289 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02290 _v177 = _v175.orientation 02291 _x = _v177 02292 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02293 _x = val1 02294 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 02295 length = len(val1.moved_obstacles) 02296 buff.write(_struct_I.pack(length)) 02297 for val2 in val1.moved_obstacles: 02298 _x = val2.reference_frame_id 02299 length = len(_x) 02300 buff.write(struct.pack('<I%ss'%length, length, _x)) 02301 length = len(val2.potential_models) 02302 buff.write(_struct_I.pack(length)) 02303 for val3 in val2.potential_models: 02304 buff.write(_struct_i.pack(val3.model_id)) 02305 _v178 = val3.pose 02306 _v179 = _v178.header 02307 buff.write(_struct_I.pack(_v179.seq)) 02308 _v180 = _v179.stamp 02309 _x = _v180 02310 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02311 _x = _v179.frame_id 02312 length = len(_x) 02313 buff.write(struct.pack('<I%ss'%length, length, _x)) 02314 _v181 = _v178.pose 02315 _v182 = _v181.position 02316 _x = _v182 02317 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02318 _v183 = _v181.orientation 02319 _x = _v183 02320 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02321 buff.write(_struct_f.pack(val3.confidence)) 02322 _x = val3.detector_name 02323 length = len(_x) 02324 buff.write(struct.pack('<I%ss'%length, length, _x)) 02325 _v184 = val2.cluster 02326 _v185 = _v184.header 02327 buff.write(_struct_I.pack(_v185.seq)) 02328 _v186 = _v185.stamp 02329 _x = _v186 02330 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02331 _x = _v185.frame_id 02332 length = len(_x) 02333 buff.write(struct.pack('<I%ss'%length, length, _x)) 02334 length = len(_v184.points) 02335 buff.write(_struct_I.pack(length)) 02336 for val4 in _v184.points: 02337 _x = val4 02338 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02339 length = len(_v184.channels) 02340 buff.write(_struct_I.pack(length)) 02341 for val4 in _v184.channels: 02342 _x = val4.name 02343 length = len(_x) 02344 buff.write(struct.pack('<I%ss'%length, length, _x)) 02345 length = len(val4.values) 02346 buff.write(_struct_I.pack(length)) 02347 pattern = '<%sf'%length 02348 buff.write(val4.values.tostring()) 02349 _v187 = val2.region 02350 _v188 = _v187.cloud 02351 _v189 = _v188.header 02352 buff.write(_struct_I.pack(_v189.seq)) 02353 _v190 = _v189.stamp 02354 _x = _v190 02355 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02356 _x = _v189.frame_id 02357 length = len(_x) 02358 buff.write(struct.pack('<I%ss'%length, length, _x)) 02359 _x = _v188 02360 buff.write(_struct_2I.pack(_x.height, _x.width)) 02361 length = len(_v188.fields) 02362 buff.write(_struct_I.pack(length)) 02363 for val5 in _v188.fields: 02364 _x = val5.name 02365 length = len(_x) 02366 buff.write(struct.pack('<I%ss'%length, length, _x)) 02367 _x = val5 02368 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02369 _x = _v188 02370 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 02371 _x = _v188.data 02372 length = len(_x) 02373 # - if encoded as a list instead, serialize as bytes instead of string 02374 if type(_x) in [list, tuple]: 02375 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02376 else: 02377 buff.write(struct.pack('<I%ss'%length, length, _x)) 02378 buff.write(_struct_B.pack(_v188.is_dense)) 02379 length = len(_v187.mask) 02380 buff.write(_struct_I.pack(length)) 02381 pattern = '<%si'%length 02382 buff.write(_v187.mask.tostring()) 02383 _v191 = _v187.image 02384 _v192 = _v191.header 02385 buff.write(_struct_I.pack(_v192.seq)) 02386 _v193 = _v192.stamp 02387 _x = _v193 02388 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02389 _x = _v192.frame_id 02390 length = len(_x) 02391 buff.write(struct.pack('<I%ss'%length, length, _x)) 02392 _x = _v191 02393 buff.write(_struct_2I.pack(_x.height, _x.width)) 02394 _x = _v191.encoding 02395 length = len(_x) 02396 buff.write(struct.pack('<I%ss'%length, length, _x)) 02397 _x = _v191 02398 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02399 _x = _v191.data 02400 length = len(_x) 02401 # - if encoded as a list instead, serialize as bytes instead of string 02402 if type(_x) in [list, tuple]: 02403 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02404 else: 02405 buff.write(struct.pack('<I%ss'%length, length, _x)) 02406 _v194 = _v187.disparity_image 02407 _v195 = _v194.header 02408 buff.write(_struct_I.pack(_v195.seq)) 02409 _v196 = _v195.stamp 02410 _x = _v196 02411 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02412 _x = _v195.frame_id 02413 length = len(_x) 02414 buff.write(struct.pack('<I%ss'%length, length, _x)) 02415 _x = _v194 02416 buff.write(_struct_2I.pack(_x.height, _x.width)) 02417 _x = _v194.encoding 02418 length = len(_x) 02419 buff.write(struct.pack('<I%ss'%length, length, _x)) 02420 _x = _v194 02421 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02422 _x = _v194.data 02423 length = len(_x) 02424 # - if encoded as a list instead, serialize as bytes instead of string 02425 if type(_x) in [list, tuple]: 02426 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02427 else: 02428 buff.write(struct.pack('<I%ss'%length, length, _x)) 02429 _v197 = _v187.cam_info 02430 _v198 = _v197.header 02431 buff.write(_struct_I.pack(_v198.seq)) 02432 _v199 = _v198.stamp 02433 _x = _v199 02434 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02435 _x = _v198.frame_id 02436 length = len(_x) 02437 buff.write(struct.pack('<I%ss'%length, length, _x)) 02438 _x = _v197 02439 buff.write(_struct_2I.pack(_x.height, _x.width)) 02440 _x = _v197.distortion_model 02441 length = len(_x) 02442 buff.write(struct.pack('<I%ss'%length, length, _x)) 02443 length = len(_v197.D) 02444 buff.write(_struct_I.pack(length)) 02445 pattern = '<%sd'%length 02446 buff.write(_v197.D.tostring()) 02447 buff.write(_v197.K.tostring()) 02448 buff.write(_v197.R.tostring()) 02449 buff.write(_v197.P.tostring()) 02450 _x = _v197 02451 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02452 _v200 = _v197.roi 02453 _x = _v200 02454 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02455 _v201 = _v187.roi_box_pose 02456 _v202 = _v201.header 02457 buff.write(_struct_I.pack(_v202.seq)) 02458 _v203 = _v202.stamp 02459 _x = _v203 02460 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02461 _x = _v202.frame_id 02462 length = len(_x) 02463 buff.write(struct.pack('<I%ss'%length, length, _x)) 02464 _v204 = _v201.pose 02465 _v205 = _v204.position 02466 _x = _v205 02467 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02468 _v206 = _v204.orientation 02469 _x = _v206 02470 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02471 _v207 = _v187.roi_box_dims 02472 _x = _v207 02473 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02474 _x = val2.collision_name 02475 length = len(_x) 02476 buff.write(struct.pack('<I%ss'%length, length, _x)) 02477 length = len(self.attempted_grasp_results) 02478 buff.write(_struct_I.pack(length)) 02479 for val1 in self.attempted_grasp_results: 02480 _x = val1 02481 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible)) 02482 except struct.error as se: self._check_types(se) 02483 except TypeError as te: self._check_types(te) 02484 02485 def deserialize_numpy(self, str, numpy): 02486 """ 02487 unpack serialized message in str into this message instance using numpy for array types 02488 @param str: byte array of serialized message 02489 @type str: str 02490 @param numpy: numpy python module 02491 @type numpy: module 02492 """ 02493 try: 02494 if self.manipulation_result is None: 02495 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult() 02496 if self.grasp is None: 02497 self.grasp = object_manipulation_msgs.msg.Grasp() 02498 end = 0 02499 _x = self 02500 start = end 02501 end += 16 02502 (_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end]) 02503 start = end 02504 end += 4 02505 (length,) = _struct_I.unpack(str[start:end]) 02506 start = end 02507 end += length 02508 self.grasp.pre_grasp_posture.header.frame_id = str[start:end] 02509 start = end 02510 end += 4 02511 (length,) = _struct_I.unpack(str[start:end]) 02512 self.grasp.pre_grasp_posture.name = [] 02513 for i in range(0, length): 02514 start = end 02515 end += 4 02516 (length,) = _struct_I.unpack(str[start:end]) 02517 start = end 02518 end += length 02519 val1 = str[start:end] 02520 self.grasp.pre_grasp_posture.name.append(val1) 02521 start = end 02522 end += 4 02523 (length,) = _struct_I.unpack(str[start:end]) 02524 pattern = '<%sd'%length 02525 start = end 02526 end += struct.calcsize(pattern) 02527 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02528 start = end 02529 end += 4 02530 (length,) = _struct_I.unpack(str[start:end]) 02531 pattern = '<%sd'%length 02532 start = end 02533 end += struct.calcsize(pattern) 02534 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02535 start = end 02536 end += 4 02537 (length,) = _struct_I.unpack(str[start:end]) 02538 pattern = '<%sd'%length 02539 start = end 02540 end += struct.calcsize(pattern) 02541 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02542 _x = self 02543 start = end 02544 end += 12 02545 (_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]) 02546 start = end 02547 end += 4 02548 (length,) = _struct_I.unpack(str[start:end]) 02549 start = end 02550 end += length 02551 self.grasp.grasp_posture.header.frame_id = str[start:end] 02552 start = end 02553 end += 4 02554 (length,) = _struct_I.unpack(str[start:end]) 02555 self.grasp.grasp_posture.name = [] 02556 for i in range(0, length): 02557 start = end 02558 end += 4 02559 (length,) = _struct_I.unpack(str[start:end]) 02560 start = end 02561 end += length 02562 val1 = str[start:end] 02563 self.grasp.grasp_posture.name.append(val1) 02564 start = end 02565 end += 4 02566 (length,) = _struct_I.unpack(str[start:end]) 02567 pattern = '<%sd'%length 02568 start = end 02569 end += struct.calcsize(pattern) 02570 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02571 start = end 02572 end += 4 02573 (length,) = _struct_I.unpack(str[start:end]) 02574 pattern = '<%sd'%length 02575 start = end 02576 end += struct.calcsize(pattern) 02577 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02578 start = end 02579 end += 4 02580 (length,) = _struct_I.unpack(str[start:end]) 02581 pattern = '<%sd'%length 02582 start = end 02583 end += struct.calcsize(pattern) 02584 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02585 _x = self 02586 start = end 02587 end += 73 02588 (_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]) 02589 self.grasp.cluster_rep = bool(self.grasp.cluster_rep) 02590 start = end 02591 end += 4 02592 (length,) = _struct_I.unpack(str[start:end]) 02593 self.grasp.moved_obstacles = [] 02594 for i in range(0, length): 02595 val1 = object_manipulation_msgs.msg.GraspableObject() 02596 start = end 02597 end += 4 02598 (length,) = _struct_I.unpack(str[start:end]) 02599 start = end 02600 end += length 02601 val1.reference_frame_id = str[start:end] 02602 start = end 02603 end += 4 02604 (length,) = _struct_I.unpack(str[start:end]) 02605 val1.potential_models = [] 02606 for i in range(0, length): 02607 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 02608 start = end 02609 end += 4 02610 (val2.model_id,) = _struct_i.unpack(str[start:end]) 02611 _v208 = val2.pose 02612 _v209 = _v208.header 02613 start = end 02614 end += 4 02615 (_v209.seq,) = _struct_I.unpack(str[start:end]) 02616 _v210 = _v209.stamp 02617 _x = _v210 02618 start = end 02619 end += 8 02620 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02621 start = end 02622 end += 4 02623 (length,) = _struct_I.unpack(str[start:end]) 02624 start = end 02625 end += length 02626 _v209.frame_id = str[start:end] 02627 _v211 = _v208.pose 02628 _v212 = _v211.position 02629 _x = _v212 02630 start = end 02631 end += 24 02632 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02633 _v213 = _v211.orientation 02634 _x = _v213 02635 start = end 02636 end += 32 02637 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02638 start = end 02639 end += 4 02640 (val2.confidence,) = _struct_f.unpack(str[start:end]) 02641 start = end 02642 end += 4 02643 (length,) = _struct_I.unpack(str[start:end]) 02644 start = end 02645 end += length 02646 val2.detector_name = str[start:end] 02647 val1.potential_models.append(val2) 02648 _v214 = val1.cluster 02649 _v215 = _v214.header 02650 start = end 02651 end += 4 02652 (_v215.seq,) = _struct_I.unpack(str[start:end]) 02653 _v216 = _v215.stamp 02654 _x = _v216 02655 start = end 02656 end += 8 02657 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02658 start = end 02659 end += 4 02660 (length,) = _struct_I.unpack(str[start:end]) 02661 start = end 02662 end += length 02663 _v215.frame_id = str[start:end] 02664 start = end 02665 end += 4 02666 (length,) = _struct_I.unpack(str[start:end]) 02667 _v214.points = [] 02668 for i in range(0, length): 02669 val3 = geometry_msgs.msg.Point32() 02670 _x = val3 02671 start = end 02672 end += 12 02673 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02674 _v214.points.append(val3) 02675 start = end 02676 end += 4 02677 (length,) = _struct_I.unpack(str[start:end]) 02678 _v214.channels = [] 02679 for i in range(0, length): 02680 val3 = sensor_msgs.msg.ChannelFloat32() 02681 start = end 02682 end += 4 02683 (length,) = _struct_I.unpack(str[start:end]) 02684 start = end 02685 end += length 02686 val3.name = str[start:end] 02687 start = end 02688 end += 4 02689 (length,) = _struct_I.unpack(str[start:end]) 02690 pattern = '<%sf'%length 02691 start = end 02692 end += struct.calcsize(pattern) 02693 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 02694 _v214.channels.append(val3) 02695 _v217 = val1.region 02696 _v218 = _v217.cloud 02697 _v219 = _v218.header 02698 start = end 02699 end += 4 02700 (_v219.seq,) = _struct_I.unpack(str[start:end]) 02701 _v220 = _v219.stamp 02702 _x = _v220 02703 start = end 02704 end += 8 02705 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02706 start = end 02707 end += 4 02708 (length,) = _struct_I.unpack(str[start:end]) 02709 start = end 02710 end += length 02711 _v219.frame_id = str[start:end] 02712 _x = _v218 02713 start = end 02714 end += 8 02715 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02716 start = end 02717 end += 4 02718 (length,) = _struct_I.unpack(str[start:end]) 02719 _v218.fields = [] 02720 for i in range(0, length): 02721 val4 = sensor_msgs.msg.PointField() 02722 start = end 02723 end += 4 02724 (length,) = _struct_I.unpack(str[start:end]) 02725 start = end 02726 end += length 02727 val4.name = str[start:end] 02728 _x = val4 02729 start = end 02730 end += 9 02731 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02732 _v218.fields.append(val4) 02733 _x = _v218 02734 start = end 02735 end += 9 02736 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 02737 _v218.is_bigendian = bool(_v218.is_bigendian) 02738 start = end 02739 end += 4 02740 (length,) = _struct_I.unpack(str[start:end]) 02741 start = end 02742 end += length 02743 _v218.data = str[start:end] 02744 start = end 02745 end += 1 02746 (_v218.is_dense,) = _struct_B.unpack(str[start:end]) 02747 _v218.is_dense = bool(_v218.is_dense) 02748 start = end 02749 end += 4 02750 (length,) = _struct_I.unpack(str[start:end]) 02751 pattern = '<%si'%length 02752 start = end 02753 end += struct.calcsize(pattern) 02754 _v217.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 02755 _v221 = _v217.image 02756 _v222 = _v221.header 02757 start = end 02758 end += 4 02759 (_v222.seq,) = _struct_I.unpack(str[start:end]) 02760 _v223 = _v222.stamp 02761 _x = _v223 02762 start = end 02763 end += 8 02764 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02765 start = end 02766 end += 4 02767 (length,) = _struct_I.unpack(str[start:end]) 02768 start = end 02769 end += length 02770 _v222.frame_id = str[start:end] 02771 _x = _v221 02772 start = end 02773 end += 8 02774 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02775 start = end 02776 end += 4 02777 (length,) = _struct_I.unpack(str[start:end]) 02778 start = end 02779 end += length 02780 _v221.encoding = str[start:end] 02781 _x = _v221 02782 start = end 02783 end += 5 02784 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02785 start = end 02786 end += 4 02787 (length,) = _struct_I.unpack(str[start:end]) 02788 start = end 02789 end += length 02790 _v221.data = str[start:end] 02791 _v224 = _v217.disparity_image 02792 _v225 = _v224.header 02793 start = end 02794 end += 4 02795 (_v225.seq,) = _struct_I.unpack(str[start:end]) 02796 _v226 = _v225.stamp 02797 _x = _v226 02798 start = end 02799 end += 8 02800 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02801 start = end 02802 end += 4 02803 (length,) = _struct_I.unpack(str[start:end]) 02804 start = end 02805 end += length 02806 _v225.frame_id = str[start:end] 02807 _x = _v224 02808 start = end 02809 end += 8 02810 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02811 start = end 02812 end += 4 02813 (length,) = _struct_I.unpack(str[start:end]) 02814 start = end 02815 end += length 02816 _v224.encoding = str[start:end] 02817 _x = _v224 02818 start = end 02819 end += 5 02820 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02821 start = end 02822 end += 4 02823 (length,) = _struct_I.unpack(str[start:end]) 02824 start = end 02825 end += length 02826 _v224.data = str[start:end] 02827 _v227 = _v217.cam_info 02828 _v228 = _v227.header 02829 start = end 02830 end += 4 02831 (_v228.seq,) = _struct_I.unpack(str[start:end]) 02832 _v229 = _v228.stamp 02833 _x = _v229 02834 start = end 02835 end += 8 02836 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02837 start = end 02838 end += 4 02839 (length,) = _struct_I.unpack(str[start:end]) 02840 start = end 02841 end += length 02842 _v228.frame_id = str[start:end] 02843 _x = _v227 02844 start = end 02845 end += 8 02846 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02847 start = end 02848 end += 4 02849 (length,) = _struct_I.unpack(str[start:end]) 02850 start = end 02851 end += length 02852 _v227.distortion_model = str[start:end] 02853 start = end 02854 end += 4 02855 (length,) = _struct_I.unpack(str[start:end]) 02856 pattern = '<%sd'%length 02857 start = end 02858 end += struct.calcsize(pattern) 02859 _v227.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02860 start = end 02861 end += 72 02862 _v227.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02863 start = end 02864 end += 72 02865 _v227.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02866 start = end 02867 end += 96 02868 _v227.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 02869 _x = _v227 02870 start = end 02871 end += 8 02872 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02873 _v230 = _v227.roi 02874 _x = _v230 02875 start = end 02876 end += 17 02877 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02878 _v230.do_rectify = bool(_v230.do_rectify) 02879 _v231 = _v217.roi_box_pose 02880 _v232 = _v231.header 02881 start = end 02882 end += 4 02883 (_v232.seq,) = _struct_I.unpack(str[start:end]) 02884 _v233 = _v232.stamp 02885 _x = _v233 02886 start = end 02887 end += 8 02888 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02889 start = end 02890 end += 4 02891 (length,) = _struct_I.unpack(str[start:end]) 02892 start = end 02893 end += length 02894 _v232.frame_id = str[start:end] 02895 _v234 = _v231.pose 02896 _v235 = _v234.position 02897 _x = _v235 02898 start = end 02899 end += 24 02900 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02901 _v236 = _v234.orientation 02902 _x = _v236 02903 start = end 02904 end += 32 02905 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02906 _v237 = _v217.roi_box_dims 02907 _x = _v237 02908 start = end 02909 end += 24 02910 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02911 start = end 02912 end += 4 02913 (length,) = _struct_I.unpack(str[start:end]) 02914 start = end 02915 end += length 02916 val1.collision_name = str[start:end] 02917 self.grasp.moved_obstacles.append(val1) 02918 start = end 02919 end += 4 02920 (length,) = _struct_I.unpack(str[start:end]) 02921 self.attempted_grasps = [] 02922 for i in range(0, length): 02923 val1 = object_manipulation_msgs.msg.Grasp() 02924 _v238 = val1.pre_grasp_posture 02925 _v239 = _v238.header 02926 start = end 02927 end += 4 02928 (_v239.seq,) = _struct_I.unpack(str[start:end]) 02929 _v240 = _v239.stamp 02930 _x = _v240 02931 start = end 02932 end += 8 02933 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02934 start = end 02935 end += 4 02936 (length,) = _struct_I.unpack(str[start:end]) 02937 start = end 02938 end += length 02939 _v239.frame_id = str[start:end] 02940 start = end 02941 end += 4 02942 (length,) = _struct_I.unpack(str[start:end]) 02943 _v238.name = [] 02944 for i in range(0, length): 02945 start = end 02946 end += 4 02947 (length,) = _struct_I.unpack(str[start:end]) 02948 start = end 02949 end += length 02950 val3 = str[start:end] 02951 _v238.name.append(val3) 02952 start = end 02953 end += 4 02954 (length,) = _struct_I.unpack(str[start:end]) 02955 pattern = '<%sd'%length 02956 start = end 02957 end += struct.calcsize(pattern) 02958 _v238.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02959 start = end 02960 end += 4 02961 (length,) = _struct_I.unpack(str[start:end]) 02962 pattern = '<%sd'%length 02963 start = end 02964 end += struct.calcsize(pattern) 02965 _v238.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02966 start = end 02967 end += 4 02968 (length,) = _struct_I.unpack(str[start:end]) 02969 pattern = '<%sd'%length 02970 start = end 02971 end += struct.calcsize(pattern) 02972 _v238.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02973 _v241 = val1.grasp_posture 02974 _v242 = _v241.header 02975 start = end 02976 end += 4 02977 (_v242.seq,) = _struct_I.unpack(str[start:end]) 02978 _v243 = _v242.stamp 02979 _x = _v243 02980 start = end 02981 end += 8 02982 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02983 start = end 02984 end += 4 02985 (length,) = _struct_I.unpack(str[start:end]) 02986 start = end 02987 end += length 02988 _v242.frame_id = str[start:end] 02989 start = end 02990 end += 4 02991 (length,) = _struct_I.unpack(str[start:end]) 02992 _v241.name = [] 02993 for i in range(0, length): 02994 start = end 02995 end += 4 02996 (length,) = _struct_I.unpack(str[start:end]) 02997 start = end 02998 end += length 02999 val3 = str[start:end] 03000 _v241.name.append(val3) 03001 start = end 03002 end += 4 03003 (length,) = _struct_I.unpack(str[start:end]) 03004 pattern = '<%sd'%length 03005 start = end 03006 end += struct.calcsize(pattern) 03007 _v241.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03008 start = end 03009 end += 4 03010 (length,) = _struct_I.unpack(str[start:end]) 03011 pattern = '<%sd'%length 03012 start = end 03013 end += struct.calcsize(pattern) 03014 _v241.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03015 start = end 03016 end += 4 03017 (length,) = _struct_I.unpack(str[start:end]) 03018 pattern = '<%sd'%length 03019 start = end 03020 end += struct.calcsize(pattern) 03021 _v241.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03022 _v244 = val1.grasp_pose 03023 _v245 = _v244.position 03024 _x = _v245 03025 start = end 03026 end += 24 03027 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03028 _v246 = _v244.orientation 03029 _x = _v246 03030 start = end 03031 end += 32 03032 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03033 _x = val1 03034 start = end 03035 end += 17 03036 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 03037 val1.cluster_rep = bool(val1.cluster_rep) 03038 start = end 03039 end += 4 03040 (length,) = _struct_I.unpack(str[start:end]) 03041 val1.moved_obstacles = [] 03042 for i in range(0, length): 03043 val2 = object_manipulation_msgs.msg.GraspableObject() 03044 start = end 03045 end += 4 03046 (length,) = _struct_I.unpack(str[start:end]) 03047 start = end 03048 end += length 03049 val2.reference_frame_id = str[start:end] 03050 start = end 03051 end += 4 03052 (length,) = _struct_I.unpack(str[start:end]) 03053 val2.potential_models = [] 03054 for i in range(0, length): 03055 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 03056 start = end 03057 end += 4 03058 (val3.model_id,) = _struct_i.unpack(str[start:end]) 03059 _v247 = val3.pose 03060 _v248 = _v247.header 03061 start = end 03062 end += 4 03063 (_v248.seq,) = _struct_I.unpack(str[start:end]) 03064 _v249 = _v248.stamp 03065 _x = _v249 03066 start = end 03067 end += 8 03068 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03069 start = end 03070 end += 4 03071 (length,) = _struct_I.unpack(str[start:end]) 03072 start = end 03073 end += length 03074 _v248.frame_id = str[start:end] 03075 _v250 = _v247.pose 03076 _v251 = _v250.position 03077 _x = _v251 03078 start = end 03079 end += 24 03080 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03081 _v252 = _v250.orientation 03082 _x = _v252 03083 start = end 03084 end += 32 03085 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03086 start = end 03087 end += 4 03088 (val3.confidence,) = _struct_f.unpack(str[start:end]) 03089 start = end 03090 end += 4 03091 (length,) = _struct_I.unpack(str[start:end]) 03092 start = end 03093 end += length 03094 val3.detector_name = str[start:end] 03095 val2.potential_models.append(val3) 03096 _v253 = val2.cluster 03097 _v254 = _v253.header 03098 start = end 03099 end += 4 03100 (_v254.seq,) = _struct_I.unpack(str[start:end]) 03101 _v255 = _v254.stamp 03102 _x = _v255 03103 start = end 03104 end += 8 03105 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03106 start = end 03107 end += 4 03108 (length,) = _struct_I.unpack(str[start:end]) 03109 start = end 03110 end += length 03111 _v254.frame_id = str[start:end] 03112 start = end 03113 end += 4 03114 (length,) = _struct_I.unpack(str[start:end]) 03115 _v253.points = [] 03116 for i in range(0, length): 03117 val4 = geometry_msgs.msg.Point32() 03118 _x = val4 03119 start = end 03120 end += 12 03121 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03122 _v253.points.append(val4) 03123 start = end 03124 end += 4 03125 (length,) = _struct_I.unpack(str[start:end]) 03126 _v253.channels = [] 03127 for i in range(0, length): 03128 val4 = sensor_msgs.msg.ChannelFloat32() 03129 start = end 03130 end += 4 03131 (length,) = _struct_I.unpack(str[start:end]) 03132 start = end 03133 end += length 03134 val4.name = str[start:end] 03135 start = end 03136 end += 4 03137 (length,) = _struct_I.unpack(str[start:end]) 03138 pattern = '<%sf'%length 03139 start = end 03140 end += struct.calcsize(pattern) 03141 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 03142 _v253.channels.append(val4) 03143 _v256 = val2.region 03144 _v257 = _v256.cloud 03145 _v258 = _v257.header 03146 start = end 03147 end += 4 03148 (_v258.seq,) = _struct_I.unpack(str[start:end]) 03149 _v259 = _v258.stamp 03150 _x = _v259 03151 start = end 03152 end += 8 03153 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03154 start = end 03155 end += 4 03156 (length,) = _struct_I.unpack(str[start:end]) 03157 start = end 03158 end += length 03159 _v258.frame_id = str[start:end] 03160 _x = _v257 03161 start = end 03162 end += 8 03163 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03164 start = end 03165 end += 4 03166 (length,) = _struct_I.unpack(str[start:end]) 03167 _v257.fields = [] 03168 for i in range(0, length): 03169 val5 = sensor_msgs.msg.PointField() 03170 start = end 03171 end += 4 03172 (length,) = _struct_I.unpack(str[start:end]) 03173 start = end 03174 end += length 03175 val5.name = str[start:end] 03176 _x = val5 03177 start = end 03178 end += 9 03179 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03180 _v257.fields.append(val5) 03181 _x = _v257 03182 start = end 03183 end += 9 03184 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 03185 _v257.is_bigendian = bool(_v257.is_bigendian) 03186 start = end 03187 end += 4 03188 (length,) = _struct_I.unpack(str[start:end]) 03189 start = end 03190 end += length 03191 _v257.data = str[start:end] 03192 start = end 03193 end += 1 03194 (_v257.is_dense,) = _struct_B.unpack(str[start:end]) 03195 _v257.is_dense = bool(_v257.is_dense) 03196 start = end 03197 end += 4 03198 (length,) = _struct_I.unpack(str[start:end]) 03199 pattern = '<%si'%length 03200 start = end 03201 end += struct.calcsize(pattern) 03202 _v256.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03203 _v260 = _v256.image 03204 _v261 = _v260.header 03205 start = end 03206 end += 4 03207 (_v261.seq,) = _struct_I.unpack(str[start:end]) 03208 _v262 = _v261.stamp 03209 _x = _v262 03210 start = end 03211 end += 8 03212 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03213 start = end 03214 end += 4 03215 (length,) = _struct_I.unpack(str[start:end]) 03216 start = end 03217 end += length 03218 _v261.frame_id = str[start:end] 03219 _x = _v260 03220 start = end 03221 end += 8 03222 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03223 start = end 03224 end += 4 03225 (length,) = _struct_I.unpack(str[start:end]) 03226 start = end 03227 end += length 03228 _v260.encoding = str[start:end] 03229 _x = _v260 03230 start = end 03231 end += 5 03232 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03233 start = end 03234 end += 4 03235 (length,) = _struct_I.unpack(str[start:end]) 03236 start = end 03237 end += length 03238 _v260.data = str[start:end] 03239 _v263 = _v256.disparity_image 03240 _v264 = _v263.header 03241 start = end 03242 end += 4 03243 (_v264.seq,) = _struct_I.unpack(str[start:end]) 03244 _v265 = _v264.stamp 03245 _x = _v265 03246 start = end 03247 end += 8 03248 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03249 start = end 03250 end += 4 03251 (length,) = _struct_I.unpack(str[start:end]) 03252 start = end 03253 end += length 03254 _v264.frame_id = str[start:end] 03255 _x = _v263 03256 start = end 03257 end += 8 03258 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03259 start = end 03260 end += 4 03261 (length,) = _struct_I.unpack(str[start:end]) 03262 start = end 03263 end += length 03264 _v263.encoding = str[start:end] 03265 _x = _v263 03266 start = end 03267 end += 5 03268 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03269 start = end 03270 end += 4 03271 (length,) = _struct_I.unpack(str[start:end]) 03272 start = end 03273 end += length 03274 _v263.data = str[start:end] 03275 _v266 = _v256.cam_info 03276 _v267 = _v266.header 03277 start = end 03278 end += 4 03279 (_v267.seq,) = _struct_I.unpack(str[start:end]) 03280 _v268 = _v267.stamp 03281 _x = _v268 03282 start = end 03283 end += 8 03284 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03285 start = end 03286 end += 4 03287 (length,) = _struct_I.unpack(str[start:end]) 03288 start = end 03289 end += length 03290 _v267.frame_id = str[start:end] 03291 _x = _v266 03292 start = end 03293 end += 8 03294 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03295 start = end 03296 end += 4 03297 (length,) = _struct_I.unpack(str[start:end]) 03298 start = end 03299 end += length 03300 _v266.distortion_model = str[start:end] 03301 start = end 03302 end += 4 03303 (length,) = _struct_I.unpack(str[start:end]) 03304 pattern = '<%sd'%length 03305 start = end 03306 end += struct.calcsize(pattern) 03307 _v266.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03308 start = end 03309 end += 72 03310 _v266.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03311 start = end 03312 end += 72 03313 _v266.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03314 start = end 03315 end += 96 03316 _v266.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 03317 _x = _v266 03318 start = end 03319 end += 8 03320 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 03321 _v269 = _v266.roi 03322 _x = _v269 03323 start = end 03324 end += 17 03325 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 03326 _v269.do_rectify = bool(_v269.do_rectify) 03327 _v270 = _v256.roi_box_pose 03328 _v271 = _v270.header 03329 start = end 03330 end += 4 03331 (_v271.seq,) = _struct_I.unpack(str[start:end]) 03332 _v272 = _v271.stamp 03333 _x = _v272 03334 start = end 03335 end += 8 03336 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03337 start = end 03338 end += 4 03339 (length,) = _struct_I.unpack(str[start:end]) 03340 start = end 03341 end += length 03342 _v271.frame_id = str[start:end] 03343 _v273 = _v270.pose 03344 _v274 = _v273.position 03345 _x = _v274 03346 start = end 03347 end += 24 03348 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03349 _v275 = _v273.orientation 03350 _x = _v275 03351 start = end 03352 end += 32 03353 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03354 _v276 = _v256.roi_box_dims 03355 _x = _v276 03356 start = end 03357 end += 24 03358 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03359 start = end 03360 end += 4 03361 (length,) = _struct_I.unpack(str[start:end]) 03362 start = end 03363 end += length 03364 val2.collision_name = str[start:end] 03365 val1.moved_obstacles.append(val2) 03366 self.attempted_grasps.append(val1) 03367 start = end 03368 end += 4 03369 (length,) = _struct_I.unpack(str[start:end]) 03370 self.attempted_grasp_results = [] 03371 for i in range(0, length): 03372 val1 = object_manipulation_msgs.msg.GraspResult() 03373 _x = val1 03374 start = end 03375 end += 5 03376 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end]) 03377 val1.continuation_possible = bool(val1.continuation_possible) 03378 self.attempted_grasp_results.append(val1) 03379 return self 03380 except struct.error as e: 03381 raise roslib.message.DeserializationError(e) #most likely buffer underfill 03382 03383 _struct_I = roslib.message.struct_I 03384 _struct_IBI = struct.Struct("<IBI") 03385 _struct_8dB2f = struct.Struct("<8dB2f") 03386 _struct_12d = struct.Struct("<12d") 03387 _struct_f = struct.Struct("<f") 03388 _struct_i = struct.Struct("<i") 03389 _struct_dB2f = struct.Struct("<dB2f") 03390 _struct_3I = struct.Struct("<3I") 03391 _struct_3f = struct.Struct("<3f") 03392 _struct_i3I = struct.Struct("<i3I") 03393 _struct_B = struct.Struct("<B") 03394 _struct_9d = struct.Struct("<9d") 03395 _struct_B2I = struct.Struct("<B2I") 03396 _struct_4d = struct.Struct("<4d") 03397 _struct_iB = struct.Struct("<iB") 03398 _struct_BI = struct.Struct("<BI") 03399 _struct_2I = struct.Struct("<2I") 03400 _struct_4IB = struct.Struct("<4IB") 03401 _struct_3d = struct.Struct("<3d")