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