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