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