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