$search
00001 """autogenerated by genmsg_py from GraspPlanningActionResult.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 household_objects_database_msgs.msg 00011 import std_msgs.msg 00012 00013 class GraspPlanningActionResult(roslib.message.Message): 00014 _md5sum = "079c7691212d2a05ab136adedb1d64d4" 00015 _type = "object_manipulation_msgs/GraspPlanningActionResult" 00016 _has_header = True #flag to mark the presence of a Header object 00017 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00018 00019 Header header 00020 actionlib_msgs/GoalStatus status 00021 GraspPlanningResult result 00022 00023 ================================================================================ 00024 MSG: std_msgs/Header 00025 # Standard metadata for higher-level stamped data types. 00026 # This is generally used to communicate timestamped data 00027 # in a particular coordinate frame. 00028 # 00029 # sequence ID: consecutively increasing ID 00030 uint32 seq 00031 #Two-integer timestamp that is expressed as: 00032 # * stamp.secs: seconds (stamp_secs) since epoch 00033 # * stamp.nsecs: nanoseconds since stamp_secs 00034 # time-handling sugar is provided by the client library 00035 time stamp 00036 #Frame this data is associated with 00037 # 0: no frame 00038 # 1: global frame 00039 string frame_id 00040 00041 ================================================================================ 00042 MSG: actionlib_msgs/GoalStatus 00043 GoalID goal_id 00044 uint8 status 00045 uint8 PENDING = 0 # The goal has yet to be processed by the action server 00046 uint8 ACTIVE = 1 # The goal is currently being processed by the action server 00047 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing 00048 # and has since completed its execution (Terminal State) 00049 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State) 00050 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due 00051 # to some failure (Terminal State) 00052 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, 00053 # because the goal was unattainable or invalid (Terminal State) 00054 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing 00055 # and has not yet completed execution 00056 uint8 RECALLING = 7 # The goal received a cancel request before it started executing, 00057 # but the action server has not yet confirmed that the goal is canceled 00058 uint8 RECALLED = 8 # The goal received a cancel request before it started executing 00059 # and was successfully cancelled (Terminal State) 00060 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be 00061 # sent over the wire by an action server 00062 00063 #Allow for the user to associate a string with GoalStatus for debugging 00064 string text 00065 00066 00067 ================================================================================ 00068 MSG: actionlib_msgs/GoalID 00069 # The stamp should store the time at which this goal was requested. 00070 # It is used by an action server when it tries to preempt all 00071 # goals that were requested before a certain time 00072 time stamp 00073 00074 # The id provides a way to associate feedback and 00075 # result message with specific goal requests. The id 00076 # specified must be unique. 00077 string id 00078 00079 00080 ================================================================================ 00081 MSG: object_manipulation_msgs/GraspPlanningResult 00082 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00083 00084 # the list of planned grasps 00085 Grasp[] grasps 00086 00087 # whether an error occurred 00088 GraspPlanningErrorCode error_code 00089 00090 00091 ================================================================================ 00092 MSG: object_manipulation_msgs/Grasp 00093 00094 # The internal posture of the hand for the pre-grasp 00095 # only positions are used 00096 sensor_msgs/JointState pre_grasp_posture 00097 00098 # The internal posture of the hand for the grasp 00099 # positions and efforts are used 00100 sensor_msgs/JointState grasp_posture 00101 00102 # The position of the end-effector for the grasp relative to a reference frame 00103 # (that is always specified elsewhere, not in this message) 00104 geometry_msgs/Pose grasp_pose 00105 00106 # The estimated probability of success for this grasp 00107 float64 success_probability 00108 00109 # Debug flag to indicate that this grasp would be the best in its cluster 00110 bool cluster_rep 00111 00112 # how far the pre-grasp should ideally be away from the grasp 00113 float32 desired_approach_distance 00114 00115 # how much distance between pre-grasp and grasp must actually be feasible 00116 # for the grasp not to be rejected 00117 float32 min_approach_distance 00118 00119 # an optional list of obstacles that we have semantic information about 00120 # and that we expect might move in the course of executing this grasp 00121 # the grasp planner is expected to make sure they move in an OK way; during 00122 # execution, grasp executors will not check for collisions against these objects 00123 GraspableObject[] moved_obstacles 00124 00125 ================================================================================ 00126 MSG: sensor_msgs/JointState 00127 # This is a message that holds data to describe the state of a set of torque controlled joints. 00128 # 00129 # The state of each joint (revolute or prismatic) is defined by: 00130 # * the position of the joint (rad or m), 00131 # * the velocity of the joint (rad/s or m/s) and 00132 # * the effort that is applied in the joint (Nm or N). 00133 # 00134 # Each joint is uniquely identified by its name 00135 # The header specifies the time at which the joint states were recorded. All the joint states 00136 # in one message have to be recorded at the same time. 00137 # 00138 # This message consists of a multiple arrays, one for each part of the joint state. 00139 # The goal is to make each of the fields optional. When e.g. your joints have no 00140 # effort associated with them, you can leave the effort array empty. 00141 # 00142 # All arrays in this message should have the same size, or be empty. 00143 # This is the only way to uniquely associate the joint name with the correct 00144 # states. 00145 00146 00147 Header header 00148 00149 string[] name 00150 float64[] position 00151 float64[] velocity 00152 float64[] effort 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/GraspPlanningErrorCode 00546 # Error codes for grasp and place planning 00547 00548 # plan completed as expected 00549 int32 SUCCESS = 0 00550 00551 # tf error encountered while transforming 00552 int32 TF_ERROR = 1 00553 00554 # some other error 00555 int32 OTHER_ERROR = 2 00556 00557 # the actual value of this error code 00558 int32 value 00559 """ 00560 __slots__ = ['header','status','result'] 00561 _slot_types = ['Header','actionlib_msgs/GoalStatus','object_manipulation_msgs/GraspPlanningResult'] 00562 00563 def __init__(self, *args, **kwds): 00564 """ 00565 Constructor. Any message fields that are implicitly/explicitly 00566 set to None will be assigned a default value. The recommend 00567 use is keyword arguments as this is more robust to future message 00568 changes. You cannot mix in-order arguments and keyword arguments. 00569 00570 The available fields are: 00571 header,status,result 00572 00573 @param args: complete set of field values, in .msg order 00574 @param kwds: use keyword arguments corresponding to message field names 00575 to set specific fields. 00576 """ 00577 if args or kwds: 00578 super(GraspPlanningActionResult, self).__init__(*args, **kwds) 00579 #message fields cannot be None, assign default values for those that are 00580 if self.header is None: 00581 self.header = std_msgs.msg._Header.Header() 00582 if self.status is None: 00583 self.status = actionlib_msgs.msg.GoalStatus() 00584 if self.result is None: 00585 self.result = object_manipulation_msgs.msg.GraspPlanningResult() 00586 else: 00587 self.header = std_msgs.msg._Header.Header() 00588 self.status = actionlib_msgs.msg.GoalStatus() 00589 self.result = object_manipulation_msgs.msg.GraspPlanningResult() 00590 00591 def _get_types(self): 00592 """ 00593 internal API method 00594 """ 00595 return self._slot_types 00596 00597 def serialize(self, buff): 00598 """ 00599 serialize message into buffer 00600 @param buff: buffer 00601 @type buff: StringIO 00602 """ 00603 try: 00604 _x = self 00605 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00606 _x = self.header.frame_id 00607 length = len(_x) 00608 buff.write(struct.pack('<I%ss'%length, length, _x)) 00609 _x = self 00610 buff.write(_struct_2I.pack(_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs)) 00611 _x = self.status.goal_id.id 00612 length = len(_x) 00613 buff.write(struct.pack('<I%ss'%length, length, _x)) 00614 buff.write(_struct_B.pack(self.status.status)) 00615 _x = self.status.text 00616 length = len(_x) 00617 buff.write(struct.pack('<I%ss'%length, length, _x)) 00618 length = len(self.result.grasps) 00619 buff.write(_struct_I.pack(length)) 00620 for val1 in self.result.grasps: 00621 _v1 = val1.pre_grasp_posture 00622 _v2 = _v1.header 00623 buff.write(_struct_I.pack(_v2.seq)) 00624 _v3 = _v2.stamp 00625 _x = _v3 00626 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00627 _x = _v2.frame_id 00628 length = len(_x) 00629 buff.write(struct.pack('<I%ss'%length, length, _x)) 00630 length = len(_v1.name) 00631 buff.write(_struct_I.pack(length)) 00632 for val3 in _v1.name: 00633 length = len(val3) 00634 buff.write(struct.pack('<I%ss'%length, length, val3)) 00635 length = len(_v1.position) 00636 buff.write(_struct_I.pack(length)) 00637 pattern = '<%sd'%length 00638 buff.write(struct.pack(pattern, *_v1.position)) 00639 length = len(_v1.velocity) 00640 buff.write(_struct_I.pack(length)) 00641 pattern = '<%sd'%length 00642 buff.write(struct.pack(pattern, *_v1.velocity)) 00643 length = len(_v1.effort) 00644 buff.write(_struct_I.pack(length)) 00645 pattern = '<%sd'%length 00646 buff.write(struct.pack(pattern, *_v1.effort)) 00647 _v4 = val1.grasp_posture 00648 _v5 = _v4.header 00649 buff.write(_struct_I.pack(_v5.seq)) 00650 _v6 = _v5.stamp 00651 _x = _v6 00652 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00653 _x = _v5.frame_id 00654 length = len(_x) 00655 buff.write(struct.pack('<I%ss'%length, length, _x)) 00656 length = len(_v4.name) 00657 buff.write(_struct_I.pack(length)) 00658 for val3 in _v4.name: 00659 length = len(val3) 00660 buff.write(struct.pack('<I%ss'%length, length, val3)) 00661 length = len(_v4.position) 00662 buff.write(_struct_I.pack(length)) 00663 pattern = '<%sd'%length 00664 buff.write(struct.pack(pattern, *_v4.position)) 00665 length = len(_v4.velocity) 00666 buff.write(_struct_I.pack(length)) 00667 pattern = '<%sd'%length 00668 buff.write(struct.pack(pattern, *_v4.velocity)) 00669 length = len(_v4.effort) 00670 buff.write(_struct_I.pack(length)) 00671 pattern = '<%sd'%length 00672 buff.write(struct.pack(pattern, *_v4.effort)) 00673 _v7 = val1.grasp_pose 00674 _v8 = _v7.position 00675 _x = _v8 00676 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00677 _v9 = _v7.orientation 00678 _x = _v9 00679 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00680 _x = val1 00681 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 00682 length = len(val1.moved_obstacles) 00683 buff.write(_struct_I.pack(length)) 00684 for val2 in val1.moved_obstacles: 00685 _x = val2.reference_frame_id 00686 length = len(_x) 00687 buff.write(struct.pack('<I%ss'%length, length, _x)) 00688 length = len(val2.potential_models) 00689 buff.write(_struct_I.pack(length)) 00690 for val3 in val2.potential_models: 00691 buff.write(_struct_i.pack(val3.model_id)) 00692 _v10 = val3.pose 00693 _v11 = _v10.header 00694 buff.write(_struct_I.pack(_v11.seq)) 00695 _v12 = _v11.stamp 00696 _x = _v12 00697 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00698 _x = _v11.frame_id 00699 length = len(_x) 00700 buff.write(struct.pack('<I%ss'%length, length, _x)) 00701 _v13 = _v10.pose 00702 _v14 = _v13.position 00703 _x = _v14 00704 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00705 _v15 = _v13.orientation 00706 _x = _v15 00707 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00708 buff.write(_struct_f.pack(val3.confidence)) 00709 _x = val3.detector_name 00710 length = len(_x) 00711 buff.write(struct.pack('<I%ss'%length, length, _x)) 00712 _v16 = val2.cluster 00713 _v17 = _v16.header 00714 buff.write(_struct_I.pack(_v17.seq)) 00715 _v18 = _v17.stamp 00716 _x = _v18 00717 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00718 _x = _v17.frame_id 00719 length = len(_x) 00720 buff.write(struct.pack('<I%ss'%length, length, _x)) 00721 length = len(_v16.points) 00722 buff.write(_struct_I.pack(length)) 00723 for val4 in _v16.points: 00724 _x = val4 00725 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00726 length = len(_v16.channels) 00727 buff.write(_struct_I.pack(length)) 00728 for val4 in _v16.channels: 00729 _x = val4.name 00730 length = len(_x) 00731 buff.write(struct.pack('<I%ss'%length, length, _x)) 00732 length = len(val4.values) 00733 buff.write(_struct_I.pack(length)) 00734 pattern = '<%sf'%length 00735 buff.write(struct.pack(pattern, *val4.values)) 00736 _v19 = val2.region 00737 _v20 = _v19.cloud 00738 _v21 = _v20.header 00739 buff.write(_struct_I.pack(_v21.seq)) 00740 _v22 = _v21.stamp 00741 _x = _v22 00742 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00743 _x = _v21.frame_id 00744 length = len(_x) 00745 buff.write(struct.pack('<I%ss'%length, length, _x)) 00746 _x = _v20 00747 buff.write(_struct_2I.pack(_x.height, _x.width)) 00748 length = len(_v20.fields) 00749 buff.write(_struct_I.pack(length)) 00750 for val5 in _v20.fields: 00751 _x = val5.name 00752 length = len(_x) 00753 buff.write(struct.pack('<I%ss'%length, length, _x)) 00754 _x = val5 00755 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00756 _x = _v20 00757 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 00758 _x = _v20.data 00759 length = len(_x) 00760 # - if encoded as a list instead, serialize as bytes instead of string 00761 if type(_x) in [list, tuple]: 00762 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00763 else: 00764 buff.write(struct.pack('<I%ss'%length, length, _x)) 00765 buff.write(_struct_B.pack(_v20.is_dense)) 00766 length = len(_v19.mask) 00767 buff.write(_struct_I.pack(length)) 00768 pattern = '<%si'%length 00769 buff.write(struct.pack(pattern, *_v19.mask)) 00770 _v23 = _v19.image 00771 _v24 = _v23.header 00772 buff.write(_struct_I.pack(_v24.seq)) 00773 _v25 = _v24.stamp 00774 _x = _v25 00775 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00776 _x = _v24.frame_id 00777 length = len(_x) 00778 buff.write(struct.pack('<I%ss'%length, length, _x)) 00779 _x = _v23 00780 buff.write(_struct_2I.pack(_x.height, _x.width)) 00781 _x = _v23.encoding 00782 length = len(_x) 00783 buff.write(struct.pack('<I%ss'%length, length, _x)) 00784 _x = _v23 00785 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00786 _x = _v23.data 00787 length = len(_x) 00788 # - if encoded as a list instead, serialize as bytes instead of string 00789 if type(_x) in [list, tuple]: 00790 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00791 else: 00792 buff.write(struct.pack('<I%ss'%length, length, _x)) 00793 _v26 = _v19.disparity_image 00794 _v27 = _v26.header 00795 buff.write(_struct_I.pack(_v27.seq)) 00796 _v28 = _v27.stamp 00797 _x = _v28 00798 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00799 _x = _v27.frame_id 00800 length = len(_x) 00801 buff.write(struct.pack('<I%ss'%length, length, _x)) 00802 _x = _v26 00803 buff.write(_struct_2I.pack(_x.height, _x.width)) 00804 _x = _v26.encoding 00805 length = len(_x) 00806 buff.write(struct.pack('<I%ss'%length, length, _x)) 00807 _x = _v26 00808 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00809 _x = _v26.data 00810 length = len(_x) 00811 # - if encoded as a list instead, serialize as bytes instead of string 00812 if type(_x) in [list, tuple]: 00813 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00814 else: 00815 buff.write(struct.pack('<I%ss'%length, length, _x)) 00816 _v29 = _v19.cam_info 00817 _v30 = _v29.header 00818 buff.write(_struct_I.pack(_v30.seq)) 00819 _v31 = _v30.stamp 00820 _x = _v31 00821 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00822 _x = _v30.frame_id 00823 length = len(_x) 00824 buff.write(struct.pack('<I%ss'%length, length, _x)) 00825 _x = _v29 00826 buff.write(_struct_2I.pack(_x.height, _x.width)) 00827 _x = _v29.distortion_model 00828 length = len(_x) 00829 buff.write(struct.pack('<I%ss'%length, length, _x)) 00830 length = len(_v29.D) 00831 buff.write(_struct_I.pack(length)) 00832 pattern = '<%sd'%length 00833 buff.write(struct.pack(pattern, *_v29.D)) 00834 buff.write(_struct_9d.pack(*_v29.K)) 00835 buff.write(_struct_9d.pack(*_v29.R)) 00836 buff.write(_struct_12d.pack(*_v29.P)) 00837 _x = _v29 00838 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00839 _v32 = _v29.roi 00840 _x = _v32 00841 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00842 _v33 = _v19.roi_box_pose 00843 _v34 = _v33.header 00844 buff.write(_struct_I.pack(_v34.seq)) 00845 _v35 = _v34.stamp 00846 _x = _v35 00847 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00848 _x = _v34.frame_id 00849 length = len(_x) 00850 buff.write(struct.pack('<I%ss'%length, length, _x)) 00851 _v36 = _v33.pose 00852 _v37 = _v36.position 00853 _x = _v37 00854 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00855 _v38 = _v36.orientation 00856 _x = _v38 00857 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00858 _v39 = _v19.roi_box_dims 00859 _x = _v39 00860 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00861 _x = val2.collision_name 00862 length = len(_x) 00863 buff.write(struct.pack('<I%ss'%length, length, _x)) 00864 buff.write(_struct_i.pack(self.result.error_code.value)) 00865 except struct.error as se: self._check_types(se) 00866 except TypeError as te: self._check_types(te) 00867 00868 def deserialize(self, str): 00869 """ 00870 unpack serialized message in str into this message instance 00871 @param str: byte array of serialized message 00872 @type str: str 00873 """ 00874 try: 00875 if self.header is None: 00876 self.header = std_msgs.msg._Header.Header() 00877 if self.status is None: 00878 self.status = actionlib_msgs.msg.GoalStatus() 00879 if self.result is None: 00880 self.result = object_manipulation_msgs.msg.GraspPlanningResult() 00881 end = 0 00882 _x = self 00883 start = end 00884 end += 12 00885 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00886 start = end 00887 end += 4 00888 (length,) = _struct_I.unpack(str[start:end]) 00889 start = end 00890 end += length 00891 self.header.frame_id = str[start:end] 00892 _x = self 00893 start = end 00894 end += 8 00895 (_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00896 start = end 00897 end += 4 00898 (length,) = _struct_I.unpack(str[start:end]) 00899 start = end 00900 end += length 00901 self.status.goal_id.id = str[start:end] 00902 start = end 00903 end += 1 00904 (self.status.status,) = _struct_B.unpack(str[start:end]) 00905 start = end 00906 end += 4 00907 (length,) = _struct_I.unpack(str[start:end]) 00908 start = end 00909 end += length 00910 self.status.text = str[start:end] 00911 start = end 00912 end += 4 00913 (length,) = _struct_I.unpack(str[start:end]) 00914 self.result.grasps = [] 00915 for i in range(0, length): 00916 val1 = object_manipulation_msgs.msg.Grasp() 00917 _v40 = val1.pre_grasp_posture 00918 _v41 = _v40.header 00919 start = end 00920 end += 4 00921 (_v41.seq,) = _struct_I.unpack(str[start:end]) 00922 _v42 = _v41.stamp 00923 _x = _v42 00924 start = end 00925 end += 8 00926 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00927 start = end 00928 end += 4 00929 (length,) = _struct_I.unpack(str[start:end]) 00930 start = end 00931 end += length 00932 _v41.frame_id = str[start:end] 00933 start = end 00934 end += 4 00935 (length,) = _struct_I.unpack(str[start:end]) 00936 _v40.name = [] 00937 for i in range(0, length): 00938 start = end 00939 end += 4 00940 (length,) = _struct_I.unpack(str[start:end]) 00941 start = end 00942 end += length 00943 val3 = str[start:end] 00944 _v40.name.append(val3) 00945 start = end 00946 end += 4 00947 (length,) = _struct_I.unpack(str[start:end]) 00948 pattern = '<%sd'%length 00949 start = end 00950 end += struct.calcsize(pattern) 00951 _v40.position = struct.unpack(pattern, str[start:end]) 00952 start = end 00953 end += 4 00954 (length,) = _struct_I.unpack(str[start:end]) 00955 pattern = '<%sd'%length 00956 start = end 00957 end += struct.calcsize(pattern) 00958 _v40.velocity = struct.unpack(pattern, str[start:end]) 00959 start = end 00960 end += 4 00961 (length,) = _struct_I.unpack(str[start:end]) 00962 pattern = '<%sd'%length 00963 start = end 00964 end += struct.calcsize(pattern) 00965 _v40.effort = struct.unpack(pattern, str[start:end]) 00966 _v43 = val1.grasp_posture 00967 _v44 = _v43.header 00968 start = end 00969 end += 4 00970 (_v44.seq,) = _struct_I.unpack(str[start:end]) 00971 _v45 = _v44.stamp 00972 _x = _v45 00973 start = end 00974 end += 8 00975 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00976 start = end 00977 end += 4 00978 (length,) = _struct_I.unpack(str[start:end]) 00979 start = end 00980 end += length 00981 _v44.frame_id = str[start:end] 00982 start = end 00983 end += 4 00984 (length,) = _struct_I.unpack(str[start:end]) 00985 _v43.name = [] 00986 for i in range(0, length): 00987 start = end 00988 end += 4 00989 (length,) = _struct_I.unpack(str[start:end]) 00990 start = end 00991 end += length 00992 val3 = str[start:end] 00993 _v43.name.append(val3) 00994 start = end 00995 end += 4 00996 (length,) = _struct_I.unpack(str[start:end]) 00997 pattern = '<%sd'%length 00998 start = end 00999 end += struct.calcsize(pattern) 01000 _v43.position = struct.unpack(pattern, str[start:end]) 01001 start = end 01002 end += 4 01003 (length,) = _struct_I.unpack(str[start:end]) 01004 pattern = '<%sd'%length 01005 start = end 01006 end += struct.calcsize(pattern) 01007 _v43.velocity = struct.unpack(pattern, str[start:end]) 01008 start = end 01009 end += 4 01010 (length,) = _struct_I.unpack(str[start:end]) 01011 pattern = '<%sd'%length 01012 start = end 01013 end += struct.calcsize(pattern) 01014 _v43.effort = struct.unpack(pattern, str[start:end]) 01015 _v46 = val1.grasp_pose 01016 _v47 = _v46.position 01017 _x = _v47 01018 start = end 01019 end += 24 01020 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01021 _v48 = _v46.orientation 01022 _x = _v48 01023 start = end 01024 end += 32 01025 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01026 _x = val1 01027 start = end 01028 end += 17 01029 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 01030 val1.cluster_rep = bool(val1.cluster_rep) 01031 start = end 01032 end += 4 01033 (length,) = _struct_I.unpack(str[start:end]) 01034 val1.moved_obstacles = [] 01035 for i in range(0, length): 01036 val2 = object_manipulation_msgs.msg.GraspableObject() 01037 start = end 01038 end += 4 01039 (length,) = _struct_I.unpack(str[start:end]) 01040 start = end 01041 end += length 01042 val2.reference_frame_id = str[start:end] 01043 start = end 01044 end += 4 01045 (length,) = _struct_I.unpack(str[start:end]) 01046 val2.potential_models = [] 01047 for i in range(0, length): 01048 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 01049 start = end 01050 end += 4 01051 (val3.model_id,) = _struct_i.unpack(str[start:end]) 01052 _v49 = val3.pose 01053 _v50 = _v49.header 01054 start = end 01055 end += 4 01056 (_v50.seq,) = _struct_I.unpack(str[start:end]) 01057 _v51 = _v50.stamp 01058 _x = _v51 01059 start = end 01060 end += 8 01061 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01062 start = end 01063 end += 4 01064 (length,) = _struct_I.unpack(str[start:end]) 01065 start = end 01066 end += length 01067 _v50.frame_id = str[start:end] 01068 _v52 = _v49.pose 01069 _v53 = _v52.position 01070 _x = _v53 01071 start = end 01072 end += 24 01073 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01074 _v54 = _v52.orientation 01075 _x = _v54 01076 start = end 01077 end += 32 01078 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01079 start = end 01080 end += 4 01081 (val3.confidence,) = _struct_f.unpack(str[start:end]) 01082 start = end 01083 end += 4 01084 (length,) = _struct_I.unpack(str[start:end]) 01085 start = end 01086 end += length 01087 val3.detector_name = str[start:end] 01088 val2.potential_models.append(val3) 01089 _v55 = val2.cluster 01090 _v56 = _v55.header 01091 start = end 01092 end += 4 01093 (_v56.seq,) = _struct_I.unpack(str[start:end]) 01094 _v57 = _v56.stamp 01095 _x = _v57 01096 start = end 01097 end += 8 01098 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01099 start = end 01100 end += 4 01101 (length,) = _struct_I.unpack(str[start:end]) 01102 start = end 01103 end += length 01104 _v56.frame_id = str[start:end] 01105 start = end 01106 end += 4 01107 (length,) = _struct_I.unpack(str[start:end]) 01108 _v55.points = [] 01109 for i in range(0, length): 01110 val4 = geometry_msgs.msg.Point32() 01111 _x = val4 01112 start = end 01113 end += 12 01114 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01115 _v55.points.append(val4) 01116 start = end 01117 end += 4 01118 (length,) = _struct_I.unpack(str[start:end]) 01119 _v55.channels = [] 01120 for i in range(0, length): 01121 val4 = sensor_msgs.msg.ChannelFloat32() 01122 start = end 01123 end += 4 01124 (length,) = _struct_I.unpack(str[start:end]) 01125 start = end 01126 end += length 01127 val4.name = str[start:end] 01128 start = end 01129 end += 4 01130 (length,) = _struct_I.unpack(str[start:end]) 01131 pattern = '<%sf'%length 01132 start = end 01133 end += struct.calcsize(pattern) 01134 val4.values = struct.unpack(pattern, str[start:end]) 01135 _v55.channels.append(val4) 01136 _v58 = val2.region 01137 _v59 = _v58.cloud 01138 _v60 = _v59.header 01139 start = end 01140 end += 4 01141 (_v60.seq,) = _struct_I.unpack(str[start:end]) 01142 _v61 = _v60.stamp 01143 _x = _v61 01144 start = end 01145 end += 8 01146 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01147 start = end 01148 end += 4 01149 (length,) = _struct_I.unpack(str[start:end]) 01150 start = end 01151 end += length 01152 _v60.frame_id = str[start:end] 01153 _x = _v59 01154 start = end 01155 end += 8 01156 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01157 start = end 01158 end += 4 01159 (length,) = _struct_I.unpack(str[start:end]) 01160 _v59.fields = [] 01161 for i in range(0, length): 01162 val5 = sensor_msgs.msg.PointField() 01163 start = end 01164 end += 4 01165 (length,) = _struct_I.unpack(str[start:end]) 01166 start = end 01167 end += length 01168 val5.name = str[start:end] 01169 _x = val5 01170 start = end 01171 end += 9 01172 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01173 _v59.fields.append(val5) 01174 _x = _v59 01175 start = end 01176 end += 9 01177 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01178 _v59.is_bigendian = bool(_v59.is_bigendian) 01179 start = end 01180 end += 4 01181 (length,) = _struct_I.unpack(str[start:end]) 01182 start = end 01183 end += length 01184 _v59.data = str[start:end] 01185 start = end 01186 end += 1 01187 (_v59.is_dense,) = _struct_B.unpack(str[start:end]) 01188 _v59.is_dense = bool(_v59.is_dense) 01189 start = end 01190 end += 4 01191 (length,) = _struct_I.unpack(str[start:end]) 01192 pattern = '<%si'%length 01193 start = end 01194 end += struct.calcsize(pattern) 01195 _v58.mask = struct.unpack(pattern, str[start:end]) 01196 _v62 = _v58.image 01197 _v63 = _v62.header 01198 start = end 01199 end += 4 01200 (_v63.seq,) = _struct_I.unpack(str[start:end]) 01201 _v64 = _v63.stamp 01202 _x = _v64 01203 start = end 01204 end += 8 01205 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01206 start = end 01207 end += 4 01208 (length,) = _struct_I.unpack(str[start:end]) 01209 start = end 01210 end += length 01211 _v63.frame_id = str[start:end] 01212 _x = _v62 01213 start = end 01214 end += 8 01215 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01216 start = end 01217 end += 4 01218 (length,) = _struct_I.unpack(str[start:end]) 01219 start = end 01220 end += length 01221 _v62.encoding = str[start:end] 01222 _x = _v62 01223 start = end 01224 end += 5 01225 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01226 start = end 01227 end += 4 01228 (length,) = _struct_I.unpack(str[start:end]) 01229 start = end 01230 end += length 01231 _v62.data = str[start:end] 01232 _v65 = _v58.disparity_image 01233 _v66 = _v65.header 01234 start = end 01235 end += 4 01236 (_v66.seq,) = _struct_I.unpack(str[start:end]) 01237 _v67 = _v66.stamp 01238 _x = _v67 01239 start = end 01240 end += 8 01241 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01242 start = end 01243 end += 4 01244 (length,) = _struct_I.unpack(str[start:end]) 01245 start = end 01246 end += length 01247 _v66.frame_id = str[start:end] 01248 _x = _v65 01249 start = end 01250 end += 8 01251 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01252 start = end 01253 end += 4 01254 (length,) = _struct_I.unpack(str[start:end]) 01255 start = end 01256 end += length 01257 _v65.encoding = str[start:end] 01258 _x = _v65 01259 start = end 01260 end += 5 01261 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01262 start = end 01263 end += 4 01264 (length,) = _struct_I.unpack(str[start:end]) 01265 start = end 01266 end += length 01267 _v65.data = str[start:end] 01268 _v68 = _v58.cam_info 01269 _v69 = _v68.header 01270 start = end 01271 end += 4 01272 (_v69.seq,) = _struct_I.unpack(str[start:end]) 01273 _v70 = _v69.stamp 01274 _x = _v70 01275 start = end 01276 end += 8 01277 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01278 start = end 01279 end += 4 01280 (length,) = _struct_I.unpack(str[start:end]) 01281 start = end 01282 end += length 01283 _v69.frame_id = str[start:end] 01284 _x = _v68 01285 start = end 01286 end += 8 01287 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01288 start = end 01289 end += 4 01290 (length,) = _struct_I.unpack(str[start:end]) 01291 start = end 01292 end += length 01293 _v68.distortion_model = str[start:end] 01294 start = end 01295 end += 4 01296 (length,) = _struct_I.unpack(str[start:end]) 01297 pattern = '<%sd'%length 01298 start = end 01299 end += struct.calcsize(pattern) 01300 _v68.D = struct.unpack(pattern, str[start:end]) 01301 start = end 01302 end += 72 01303 _v68.K = _struct_9d.unpack(str[start:end]) 01304 start = end 01305 end += 72 01306 _v68.R = _struct_9d.unpack(str[start:end]) 01307 start = end 01308 end += 96 01309 _v68.P = _struct_12d.unpack(str[start:end]) 01310 _x = _v68 01311 start = end 01312 end += 8 01313 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01314 _v71 = _v68.roi 01315 _x = _v71 01316 start = end 01317 end += 17 01318 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01319 _v71.do_rectify = bool(_v71.do_rectify) 01320 _v72 = _v58.roi_box_pose 01321 _v73 = _v72.header 01322 start = end 01323 end += 4 01324 (_v73.seq,) = _struct_I.unpack(str[start:end]) 01325 _v74 = _v73.stamp 01326 _x = _v74 01327 start = end 01328 end += 8 01329 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01330 start = end 01331 end += 4 01332 (length,) = _struct_I.unpack(str[start:end]) 01333 start = end 01334 end += length 01335 _v73.frame_id = str[start:end] 01336 _v75 = _v72.pose 01337 _v76 = _v75.position 01338 _x = _v76 01339 start = end 01340 end += 24 01341 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01342 _v77 = _v75.orientation 01343 _x = _v77 01344 start = end 01345 end += 32 01346 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01347 _v78 = _v58.roi_box_dims 01348 _x = _v78 01349 start = end 01350 end += 24 01351 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01352 start = end 01353 end += 4 01354 (length,) = _struct_I.unpack(str[start:end]) 01355 start = end 01356 end += length 01357 val2.collision_name = str[start:end] 01358 val1.moved_obstacles.append(val2) 01359 self.result.grasps.append(val1) 01360 start = end 01361 end += 4 01362 (self.result.error_code.value,) = _struct_i.unpack(str[start:end]) 01363 return self 01364 except struct.error as e: 01365 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01366 01367 01368 def serialize_numpy(self, buff, numpy): 01369 """ 01370 serialize message with numpy array types into buffer 01371 @param buff: buffer 01372 @type buff: StringIO 01373 @param numpy: numpy python module 01374 @type numpy module 01375 """ 01376 try: 01377 _x = self 01378 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 01379 _x = self.header.frame_id 01380 length = len(_x) 01381 buff.write(struct.pack('<I%ss'%length, length, _x)) 01382 _x = self 01383 buff.write(_struct_2I.pack(_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs)) 01384 _x = self.status.goal_id.id 01385 length = len(_x) 01386 buff.write(struct.pack('<I%ss'%length, length, _x)) 01387 buff.write(_struct_B.pack(self.status.status)) 01388 _x = self.status.text 01389 length = len(_x) 01390 buff.write(struct.pack('<I%ss'%length, length, _x)) 01391 length = len(self.result.grasps) 01392 buff.write(_struct_I.pack(length)) 01393 for val1 in self.result.grasps: 01394 _v79 = val1.pre_grasp_posture 01395 _v80 = _v79.header 01396 buff.write(_struct_I.pack(_v80.seq)) 01397 _v81 = _v80.stamp 01398 _x = _v81 01399 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01400 _x = _v80.frame_id 01401 length = len(_x) 01402 buff.write(struct.pack('<I%ss'%length, length, _x)) 01403 length = len(_v79.name) 01404 buff.write(_struct_I.pack(length)) 01405 for val3 in _v79.name: 01406 length = len(val3) 01407 buff.write(struct.pack('<I%ss'%length, length, val3)) 01408 length = len(_v79.position) 01409 buff.write(_struct_I.pack(length)) 01410 pattern = '<%sd'%length 01411 buff.write(_v79.position.tostring()) 01412 length = len(_v79.velocity) 01413 buff.write(_struct_I.pack(length)) 01414 pattern = '<%sd'%length 01415 buff.write(_v79.velocity.tostring()) 01416 length = len(_v79.effort) 01417 buff.write(_struct_I.pack(length)) 01418 pattern = '<%sd'%length 01419 buff.write(_v79.effort.tostring()) 01420 _v82 = val1.grasp_posture 01421 _v83 = _v82.header 01422 buff.write(_struct_I.pack(_v83.seq)) 01423 _v84 = _v83.stamp 01424 _x = _v84 01425 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01426 _x = _v83.frame_id 01427 length = len(_x) 01428 buff.write(struct.pack('<I%ss'%length, length, _x)) 01429 length = len(_v82.name) 01430 buff.write(_struct_I.pack(length)) 01431 for val3 in _v82.name: 01432 length = len(val3) 01433 buff.write(struct.pack('<I%ss'%length, length, val3)) 01434 length = len(_v82.position) 01435 buff.write(_struct_I.pack(length)) 01436 pattern = '<%sd'%length 01437 buff.write(_v82.position.tostring()) 01438 length = len(_v82.velocity) 01439 buff.write(_struct_I.pack(length)) 01440 pattern = '<%sd'%length 01441 buff.write(_v82.velocity.tostring()) 01442 length = len(_v82.effort) 01443 buff.write(_struct_I.pack(length)) 01444 pattern = '<%sd'%length 01445 buff.write(_v82.effort.tostring()) 01446 _v85 = val1.grasp_pose 01447 _v86 = _v85.position 01448 _x = _v86 01449 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01450 _v87 = _v85.orientation 01451 _x = _v87 01452 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01453 _x = val1 01454 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 01455 length = len(val1.moved_obstacles) 01456 buff.write(_struct_I.pack(length)) 01457 for val2 in val1.moved_obstacles: 01458 _x = val2.reference_frame_id 01459 length = len(_x) 01460 buff.write(struct.pack('<I%ss'%length, length, _x)) 01461 length = len(val2.potential_models) 01462 buff.write(_struct_I.pack(length)) 01463 for val3 in val2.potential_models: 01464 buff.write(_struct_i.pack(val3.model_id)) 01465 _v88 = val3.pose 01466 _v89 = _v88.header 01467 buff.write(_struct_I.pack(_v89.seq)) 01468 _v90 = _v89.stamp 01469 _x = _v90 01470 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01471 _x = _v89.frame_id 01472 length = len(_x) 01473 buff.write(struct.pack('<I%ss'%length, length, _x)) 01474 _v91 = _v88.pose 01475 _v92 = _v91.position 01476 _x = _v92 01477 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01478 _v93 = _v91.orientation 01479 _x = _v93 01480 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01481 buff.write(_struct_f.pack(val3.confidence)) 01482 _x = val3.detector_name 01483 length = len(_x) 01484 buff.write(struct.pack('<I%ss'%length, length, _x)) 01485 _v94 = val2.cluster 01486 _v95 = _v94.header 01487 buff.write(_struct_I.pack(_v95.seq)) 01488 _v96 = _v95.stamp 01489 _x = _v96 01490 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01491 _x = _v95.frame_id 01492 length = len(_x) 01493 buff.write(struct.pack('<I%ss'%length, length, _x)) 01494 length = len(_v94.points) 01495 buff.write(_struct_I.pack(length)) 01496 for val4 in _v94.points: 01497 _x = val4 01498 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01499 length = len(_v94.channels) 01500 buff.write(_struct_I.pack(length)) 01501 for val4 in _v94.channels: 01502 _x = val4.name 01503 length = len(_x) 01504 buff.write(struct.pack('<I%ss'%length, length, _x)) 01505 length = len(val4.values) 01506 buff.write(_struct_I.pack(length)) 01507 pattern = '<%sf'%length 01508 buff.write(val4.values.tostring()) 01509 _v97 = val2.region 01510 _v98 = _v97.cloud 01511 _v99 = _v98.header 01512 buff.write(_struct_I.pack(_v99.seq)) 01513 _v100 = _v99.stamp 01514 _x = _v100 01515 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01516 _x = _v99.frame_id 01517 length = len(_x) 01518 buff.write(struct.pack('<I%ss'%length, length, _x)) 01519 _x = _v98 01520 buff.write(_struct_2I.pack(_x.height, _x.width)) 01521 length = len(_v98.fields) 01522 buff.write(_struct_I.pack(length)) 01523 for val5 in _v98.fields: 01524 _x = val5.name 01525 length = len(_x) 01526 buff.write(struct.pack('<I%ss'%length, length, _x)) 01527 _x = val5 01528 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01529 _x = _v98 01530 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01531 _x = _v98.data 01532 length = len(_x) 01533 # - if encoded as a list instead, serialize as bytes instead of string 01534 if type(_x) in [list, tuple]: 01535 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01536 else: 01537 buff.write(struct.pack('<I%ss'%length, length, _x)) 01538 buff.write(_struct_B.pack(_v98.is_dense)) 01539 length = len(_v97.mask) 01540 buff.write(_struct_I.pack(length)) 01541 pattern = '<%si'%length 01542 buff.write(_v97.mask.tostring()) 01543 _v101 = _v97.image 01544 _v102 = _v101.header 01545 buff.write(_struct_I.pack(_v102.seq)) 01546 _v103 = _v102.stamp 01547 _x = _v103 01548 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01549 _x = _v102.frame_id 01550 length = len(_x) 01551 buff.write(struct.pack('<I%ss'%length, length, _x)) 01552 _x = _v101 01553 buff.write(_struct_2I.pack(_x.height, _x.width)) 01554 _x = _v101.encoding 01555 length = len(_x) 01556 buff.write(struct.pack('<I%ss'%length, length, _x)) 01557 _x = _v101 01558 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01559 _x = _v101.data 01560 length = len(_x) 01561 # - if encoded as a list instead, serialize as bytes instead of string 01562 if type(_x) in [list, tuple]: 01563 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01564 else: 01565 buff.write(struct.pack('<I%ss'%length, length, _x)) 01566 _v104 = _v97.disparity_image 01567 _v105 = _v104.header 01568 buff.write(_struct_I.pack(_v105.seq)) 01569 _v106 = _v105.stamp 01570 _x = _v106 01571 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01572 _x = _v105.frame_id 01573 length = len(_x) 01574 buff.write(struct.pack('<I%ss'%length, length, _x)) 01575 _x = _v104 01576 buff.write(_struct_2I.pack(_x.height, _x.width)) 01577 _x = _v104.encoding 01578 length = len(_x) 01579 buff.write(struct.pack('<I%ss'%length, length, _x)) 01580 _x = _v104 01581 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01582 _x = _v104.data 01583 length = len(_x) 01584 # - if encoded as a list instead, serialize as bytes instead of string 01585 if type(_x) in [list, tuple]: 01586 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01587 else: 01588 buff.write(struct.pack('<I%ss'%length, length, _x)) 01589 _v107 = _v97.cam_info 01590 _v108 = _v107.header 01591 buff.write(_struct_I.pack(_v108.seq)) 01592 _v109 = _v108.stamp 01593 _x = _v109 01594 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01595 _x = _v108.frame_id 01596 length = len(_x) 01597 buff.write(struct.pack('<I%ss'%length, length, _x)) 01598 _x = _v107 01599 buff.write(_struct_2I.pack(_x.height, _x.width)) 01600 _x = _v107.distortion_model 01601 length = len(_x) 01602 buff.write(struct.pack('<I%ss'%length, length, _x)) 01603 length = len(_v107.D) 01604 buff.write(_struct_I.pack(length)) 01605 pattern = '<%sd'%length 01606 buff.write(_v107.D.tostring()) 01607 buff.write(_v107.K.tostring()) 01608 buff.write(_v107.R.tostring()) 01609 buff.write(_v107.P.tostring()) 01610 _x = _v107 01611 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01612 _v110 = _v107.roi 01613 _x = _v110 01614 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01615 _v111 = _v97.roi_box_pose 01616 _v112 = _v111.header 01617 buff.write(_struct_I.pack(_v112.seq)) 01618 _v113 = _v112.stamp 01619 _x = _v113 01620 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01621 _x = _v112.frame_id 01622 length = len(_x) 01623 buff.write(struct.pack('<I%ss'%length, length, _x)) 01624 _v114 = _v111.pose 01625 _v115 = _v114.position 01626 _x = _v115 01627 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01628 _v116 = _v114.orientation 01629 _x = _v116 01630 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01631 _v117 = _v97.roi_box_dims 01632 _x = _v117 01633 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01634 _x = val2.collision_name 01635 length = len(_x) 01636 buff.write(struct.pack('<I%ss'%length, length, _x)) 01637 buff.write(_struct_i.pack(self.result.error_code.value)) 01638 except struct.error as se: self._check_types(se) 01639 except TypeError as te: self._check_types(te) 01640 01641 def deserialize_numpy(self, str, numpy): 01642 """ 01643 unpack serialized message in str into this message instance using numpy for array types 01644 @param str: byte array of serialized message 01645 @type str: str 01646 @param numpy: numpy python module 01647 @type numpy: module 01648 """ 01649 try: 01650 if self.header is None: 01651 self.header = std_msgs.msg._Header.Header() 01652 if self.status is None: 01653 self.status = actionlib_msgs.msg.GoalStatus() 01654 if self.result is None: 01655 self.result = object_manipulation_msgs.msg.GraspPlanningResult() 01656 end = 0 01657 _x = self 01658 start = end 01659 end += 12 01660 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01661 start = end 01662 end += 4 01663 (length,) = _struct_I.unpack(str[start:end]) 01664 start = end 01665 end += length 01666 self.header.frame_id = str[start:end] 01667 _x = self 01668 start = end 01669 end += 8 01670 (_x.status.goal_id.stamp.secs, _x.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 01671 start = end 01672 end += 4 01673 (length,) = _struct_I.unpack(str[start:end]) 01674 start = end 01675 end += length 01676 self.status.goal_id.id = str[start:end] 01677 start = end 01678 end += 1 01679 (self.status.status,) = _struct_B.unpack(str[start:end]) 01680 start = end 01681 end += 4 01682 (length,) = _struct_I.unpack(str[start:end]) 01683 start = end 01684 end += length 01685 self.status.text = str[start:end] 01686 start = end 01687 end += 4 01688 (length,) = _struct_I.unpack(str[start:end]) 01689 self.result.grasps = [] 01690 for i in range(0, length): 01691 val1 = object_manipulation_msgs.msg.Grasp() 01692 _v118 = val1.pre_grasp_posture 01693 _v119 = _v118.header 01694 start = end 01695 end += 4 01696 (_v119.seq,) = _struct_I.unpack(str[start:end]) 01697 _v120 = _v119.stamp 01698 _x = _v120 01699 start = end 01700 end += 8 01701 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01702 start = end 01703 end += 4 01704 (length,) = _struct_I.unpack(str[start:end]) 01705 start = end 01706 end += length 01707 _v119.frame_id = str[start:end] 01708 start = end 01709 end += 4 01710 (length,) = _struct_I.unpack(str[start:end]) 01711 _v118.name = [] 01712 for i in range(0, length): 01713 start = end 01714 end += 4 01715 (length,) = _struct_I.unpack(str[start:end]) 01716 start = end 01717 end += length 01718 val3 = str[start:end] 01719 _v118.name.append(val3) 01720 start = end 01721 end += 4 01722 (length,) = _struct_I.unpack(str[start:end]) 01723 pattern = '<%sd'%length 01724 start = end 01725 end += struct.calcsize(pattern) 01726 _v118.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01727 start = end 01728 end += 4 01729 (length,) = _struct_I.unpack(str[start:end]) 01730 pattern = '<%sd'%length 01731 start = end 01732 end += struct.calcsize(pattern) 01733 _v118.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01734 start = end 01735 end += 4 01736 (length,) = _struct_I.unpack(str[start:end]) 01737 pattern = '<%sd'%length 01738 start = end 01739 end += struct.calcsize(pattern) 01740 _v118.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01741 _v121 = val1.grasp_posture 01742 _v122 = _v121.header 01743 start = end 01744 end += 4 01745 (_v122.seq,) = _struct_I.unpack(str[start:end]) 01746 _v123 = _v122.stamp 01747 _x = _v123 01748 start = end 01749 end += 8 01750 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01751 start = end 01752 end += 4 01753 (length,) = _struct_I.unpack(str[start:end]) 01754 start = end 01755 end += length 01756 _v122.frame_id = str[start:end] 01757 start = end 01758 end += 4 01759 (length,) = _struct_I.unpack(str[start:end]) 01760 _v121.name = [] 01761 for i in range(0, length): 01762 start = end 01763 end += 4 01764 (length,) = _struct_I.unpack(str[start:end]) 01765 start = end 01766 end += length 01767 val3 = str[start:end] 01768 _v121.name.append(val3) 01769 start = end 01770 end += 4 01771 (length,) = _struct_I.unpack(str[start:end]) 01772 pattern = '<%sd'%length 01773 start = end 01774 end += struct.calcsize(pattern) 01775 _v121.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01776 start = end 01777 end += 4 01778 (length,) = _struct_I.unpack(str[start:end]) 01779 pattern = '<%sd'%length 01780 start = end 01781 end += struct.calcsize(pattern) 01782 _v121.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01783 start = end 01784 end += 4 01785 (length,) = _struct_I.unpack(str[start:end]) 01786 pattern = '<%sd'%length 01787 start = end 01788 end += struct.calcsize(pattern) 01789 _v121.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01790 _v124 = val1.grasp_pose 01791 _v125 = _v124.position 01792 _x = _v125 01793 start = end 01794 end += 24 01795 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01796 _v126 = _v124.orientation 01797 _x = _v126 01798 start = end 01799 end += 32 01800 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01801 _x = val1 01802 start = end 01803 end += 17 01804 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 01805 val1.cluster_rep = bool(val1.cluster_rep) 01806 start = end 01807 end += 4 01808 (length,) = _struct_I.unpack(str[start:end]) 01809 val1.moved_obstacles = [] 01810 for i in range(0, length): 01811 val2 = object_manipulation_msgs.msg.GraspableObject() 01812 start = end 01813 end += 4 01814 (length,) = _struct_I.unpack(str[start:end]) 01815 start = end 01816 end += length 01817 val2.reference_frame_id = str[start:end] 01818 start = end 01819 end += 4 01820 (length,) = _struct_I.unpack(str[start:end]) 01821 val2.potential_models = [] 01822 for i in range(0, length): 01823 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 01824 start = end 01825 end += 4 01826 (val3.model_id,) = _struct_i.unpack(str[start:end]) 01827 _v127 = val3.pose 01828 _v128 = _v127.header 01829 start = end 01830 end += 4 01831 (_v128.seq,) = _struct_I.unpack(str[start:end]) 01832 _v129 = _v128.stamp 01833 _x = _v129 01834 start = end 01835 end += 8 01836 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01837 start = end 01838 end += 4 01839 (length,) = _struct_I.unpack(str[start:end]) 01840 start = end 01841 end += length 01842 _v128.frame_id = str[start:end] 01843 _v130 = _v127.pose 01844 _v131 = _v130.position 01845 _x = _v131 01846 start = end 01847 end += 24 01848 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01849 _v132 = _v130.orientation 01850 _x = _v132 01851 start = end 01852 end += 32 01853 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01854 start = end 01855 end += 4 01856 (val3.confidence,) = _struct_f.unpack(str[start:end]) 01857 start = end 01858 end += 4 01859 (length,) = _struct_I.unpack(str[start:end]) 01860 start = end 01861 end += length 01862 val3.detector_name = str[start:end] 01863 val2.potential_models.append(val3) 01864 _v133 = val2.cluster 01865 _v134 = _v133.header 01866 start = end 01867 end += 4 01868 (_v134.seq,) = _struct_I.unpack(str[start:end]) 01869 _v135 = _v134.stamp 01870 _x = _v135 01871 start = end 01872 end += 8 01873 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01874 start = end 01875 end += 4 01876 (length,) = _struct_I.unpack(str[start:end]) 01877 start = end 01878 end += length 01879 _v134.frame_id = str[start:end] 01880 start = end 01881 end += 4 01882 (length,) = _struct_I.unpack(str[start:end]) 01883 _v133.points = [] 01884 for i in range(0, length): 01885 val4 = geometry_msgs.msg.Point32() 01886 _x = val4 01887 start = end 01888 end += 12 01889 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01890 _v133.points.append(val4) 01891 start = end 01892 end += 4 01893 (length,) = _struct_I.unpack(str[start:end]) 01894 _v133.channels = [] 01895 for i in range(0, length): 01896 val4 = sensor_msgs.msg.ChannelFloat32() 01897 start = end 01898 end += 4 01899 (length,) = _struct_I.unpack(str[start:end]) 01900 start = end 01901 end += length 01902 val4.name = str[start:end] 01903 start = end 01904 end += 4 01905 (length,) = _struct_I.unpack(str[start:end]) 01906 pattern = '<%sf'%length 01907 start = end 01908 end += struct.calcsize(pattern) 01909 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 01910 _v133.channels.append(val4) 01911 _v136 = val2.region 01912 _v137 = _v136.cloud 01913 _v138 = _v137.header 01914 start = end 01915 end += 4 01916 (_v138.seq,) = _struct_I.unpack(str[start:end]) 01917 _v139 = _v138.stamp 01918 _x = _v139 01919 start = end 01920 end += 8 01921 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01922 start = end 01923 end += 4 01924 (length,) = _struct_I.unpack(str[start:end]) 01925 start = end 01926 end += length 01927 _v138.frame_id = str[start:end] 01928 _x = _v137 01929 start = end 01930 end += 8 01931 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01932 start = end 01933 end += 4 01934 (length,) = _struct_I.unpack(str[start:end]) 01935 _v137.fields = [] 01936 for i in range(0, length): 01937 val5 = sensor_msgs.msg.PointField() 01938 start = end 01939 end += 4 01940 (length,) = _struct_I.unpack(str[start:end]) 01941 start = end 01942 end += length 01943 val5.name = str[start:end] 01944 _x = val5 01945 start = end 01946 end += 9 01947 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01948 _v137.fields.append(val5) 01949 _x = _v137 01950 start = end 01951 end += 9 01952 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01953 _v137.is_bigendian = bool(_v137.is_bigendian) 01954 start = end 01955 end += 4 01956 (length,) = _struct_I.unpack(str[start:end]) 01957 start = end 01958 end += length 01959 _v137.data = str[start:end] 01960 start = end 01961 end += 1 01962 (_v137.is_dense,) = _struct_B.unpack(str[start:end]) 01963 _v137.is_dense = bool(_v137.is_dense) 01964 start = end 01965 end += 4 01966 (length,) = _struct_I.unpack(str[start:end]) 01967 pattern = '<%si'%length 01968 start = end 01969 end += struct.calcsize(pattern) 01970 _v136.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 01971 _v140 = _v136.image 01972 _v141 = _v140.header 01973 start = end 01974 end += 4 01975 (_v141.seq,) = _struct_I.unpack(str[start:end]) 01976 _v142 = _v141.stamp 01977 _x = _v142 01978 start = end 01979 end += 8 01980 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01981 start = end 01982 end += 4 01983 (length,) = _struct_I.unpack(str[start:end]) 01984 start = end 01985 end += length 01986 _v141.frame_id = str[start:end] 01987 _x = _v140 01988 start = end 01989 end += 8 01990 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01991 start = end 01992 end += 4 01993 (length,) = _struct_I.unpack(str[start:end]) 01994 start = end 01995 end += length 01996 _v140.encoding = str[start:end] 01997 _x = _v140 01998 start = end 01999 end += 5 02000 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02001 start = end 02002 end += 4 02003 (length,) = _struct_I.unpack(str[start:end]) 02004 start = end 02005 end += length 02006 _v140.data = str[start:end] 02007 _v143 = _v136.disparity_image 02008 _v144 = _v143.header 02009 start = end 02010 end += 4 02011 (_v144.seq,) = _struct_I.unpack(str[start:end]) 02012 _v145 = _v144.stamp 02013 _x = _v145 02014 start = end 02015 end += 8 02016 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02017 start = end 02018 end += 4 02019 (length,) = _struct_I.unpack(str[start:end]) 02020 start = end 02021 end += length 02022 _v144.frame_id = str[start:end] 02023 _x = _v143 02024 start = end 02025 end += 8 02026 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02027 start = end 02028 end += 4 02029 (length,) = _struct_I.unpack(str[start:end]) 02030 start = end 02031 end += length 02032 _v143.encoding = str[start:end] 02033 _x = _v143 02034 start = end 02035 end += 5 02036 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02037 start = end 02038 end += 4 02039 (length,) = _struct_I.unpack(str[start:end]) 02040 start = end 02041 end += length 02042 _v143.data = str[start:end] 02043 _v146 = _v136.cam_info 02044 _v147 = _v146.header 02045 start = end 02046 end += 4 02047 (_v147.seq,) = _struct_I.unpack(str[start:end]) 02048 _v148 = _v147.stamp 02049 _x = _v148 02050 start = end 02051 end += 8 02052 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02053 start = end 02054 end += 4 02055 (length,) = _struct_I.unpack(str[start:end]) 02056 start = end 02057 end += length 02058 _v147.frame_id = str[start:end] 02059 _x = _v146 02060 start = end 02061 end += 8 02062 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02063 start = end 02064 end += 4 02065 (length,) = _struct_I.unpack(str[start:end]) 02066 start = end 02067 end += length 02068 _v146.distortion_model = str[start:end] 02069 start = end 02070 end += 4 02071 (length,) = _struct_I.unpack(str[start:end]) 02072 pattern = '<%sd'%length 02073 start = end 02074 end += struct.calcsize(pattern) 02075 _v146.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02076 start = end 02077 end += 72 02078 _v146.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02079 start = end 02080 end += 72 02081 _v146.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02082 start = end 02083 end += 96 02084 _v146.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 02085 _x = _v146 02086 start = end 02087 end += 8 02088 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02089 _v149 = _v146.roi 02090 _x = _v149 02091 start = end 02092 end += 17 02093 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02094 _v149.do_rectify = bool(_v149.do_rectify) 02095 _v150 = _v136.roi_box_pose 02096 _v151 = _v150.header 02097 start = end 02098 end += 4 02099 (_v151.seq,) = _struct_I.unpack(str[start:end]) 02100 _v152 = _v151.stamp 02101 _x = _v152 02102 start = end 02103 end += 8 02104 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02105 start = end 02106 end += 4 02107 (length,) = _struct_I.unpack(str[start:end]) 02108 start = end 02109 end += length 02110 _v151.frame_id = str[start:end] 02111 _v153 = _v150.pose 02112 _v154 = _v153.position 02113 _x = _v154 02114 start = end 02115 end += 24 02116 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02117 _v155 = _v153.orientation 02118 _x = _v155 02119 start = end 02120 end += 32 02121 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02122 _v156 = _v136.roi_box_dims 02123 _x = _v156 02124 start = end 02125 end += 24 02126 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02127 start = end 02128 end += 4 02129 (length,) = _struct_I.unpack(str[start:end]) 02130 start = end 02131 end += length 02132 val2.collision_name = str[start:end] 02133 val1.moved_obstacles.append(val2) 02134 self.result.grasps.append(val1) 02135 start = end 02136 end += 4 02137 (self.result.error_code.value,) = _struct_i.unpack(str[start:end]) 02138 return self 02139 except struct.error as e: 02140 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02141 02142 _struct_I = roslib.message.struct_I 02143 _struct_IBI = struct.Struct("<IBI") 02144 _struct_B = struct.Struct("<B") 02145 _struct_12d = struct.Struct("<12d") 02146 _struct_f = struct.Struct("<f") 02147 _struct_dB2f = struct.Struct("<dB2f") 02148 _struct_BI = struct.Struct("<BI") 02149 _struct_3f = struct.Struct("<3f") 02150 _struct_i = struct.Struct("<i") 02151 _struct_3I = struct.Struct("<3I") 02152 _struct_9d = struct.Struct("<9d") 02153 _struct_B2I = struct.Struct("<B2I") 02154 _struct_4d = struct.Struct("<4d") 02155 _struct_2I = struct.Struct("<2I") 02156 _struct_4IB = struct.Struct("<4IB") 02157 _struct_3d = struct.Struct("<3d")