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