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