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