$search
00001 """autogenerated by genmsg_py from GraspPlanningRequest.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 GraspPlanningRequest(roslib.message.Message): 00012 _md5sum = "5003fdf2225280c9d81c2bce4e645c20" 00013 _type = "object_manipulation_msgs/GraspPlanningRequest" 00014 _has_header = False #flag to mark the presence of a Header object 00015 _full_text = """ 00016 00017 00018 00019 string arm_name 00020 00021 00022 GraspableObject target 00023 00024 00025 00026 string collision_object_name 00027 00028 00029 00030 string collision_support_surface_name 00031 00032 00033 Grasp[] grasps_to_evaluate 00034 00035 00036 00037 GraspableObject[] movable_obstacles 00038 00039 00040 ================================================================================ 00041 MSG: object_manipulation_msgs/GraspableObject 00042 # an object that the object_manipulator can work on 00043 00044 # a graspable object can be represented in multiple ways. This message 00045 # can contain all of them. Which one is actually used is up to the receiver 00046 # of this message. When adding new representations, one must be careful that 00047 # they have reasonable lightweight defaults indicating that that particular 00048 # representation is not available. 00049 00050 # the tf frame to be used as a reference frame when combining information from 00051 # the different representations below 00052 string reference_frame_id 00053 00054 # potential recognition results from a database of models 00055 # all poses are relative to the object reference pose 00056 household_objects_database_msgs/DatabaseModelPose[] potential_models 00057 00058 # the point cloud itself 00059 sensor_msgs/PointCloud cluster 00060 00061 # a region of a PointCloud2 of interest 00062 object_manipulation_msgs/SceneRegion region 00063 00064 # the name that this object has in the collision environment 00065 string collision_name 00066 ================================================================================ 00067 MSG: household_objects_database_msgs/DatabaseModelPose 00068 # Informs that a specific model from the Model Database has been 00069 # identified at a certain location 00070 00071 # the database id of the model 00072 int32 model_id 00073 00074 # the pose that it can be found in 00075 geometry_msgs/PoseStamped pose 00076 00077 # a measure of the confidence level in this detection result 00078 float32 confidence 00079 00080 # the name of the object detector that generated this detection result 00081 string detector_name 00082 00083 ================================================================================ 00084 MSG: geometry_msgs/PoseStamped 00085 # A Pose with reference coordinate frame and timestamp 00086 Header header 00087 Pose pose 00088 00089 ================================================================================ 00090 MSG: std_msgs/Header 00091 # Standard metadata for higher-level stamped data types. 00092 # This is generally used to communicate timestamped data 00093 # in a particular coordinate frame. 00094 # 00095 # sequence ID: consecutively increasing ID 00096 uint32 seq 00097 #Two-integer timestamp that is expressed as: 00098 # * stamp.secs: seconds (stamp_secs) since epoch 00099 # * stamp.nsecs: nanoseconds since stamp_secs 00100 # time-handling sugar is provided by the client library 00101 time stamp 00102 #Frame this data is associated with 00103 # 0: no frame 00104 # 1: global frame 00105 string frame_id 00106 00107 ================================================================================ 00108 MSG: geometry_msgs/Pose 00109 # A representation of pose in free space, composed of postion and orientation. 00110 Point position 00111 Quaternion orientation 00112 00113 ================================================================================ 00114 MSG: geometry_msgs/Point 00115 # This contains the position of a point in free space 00116 float64 x 00117 float64 y 00118 float64 z 00119 00120 ================================================================================ 00121 MSG: geometry_msgs/Quaternion 00122 # This represents an orientation in free space in quaternion form. 00123 00124 float64 x 00125 float64 y 00126 float64 z 00127 float64 w 00128 00129 ================================================================================ 00130 MSG: sensor_msgs/PointCloud 00131 # This message holds a collection of 3d points, plus optional additional 00132 # information about each point. 00133 00134 # Time of sensor data acquisition, coordinate frame ID. 00135 Header header 00136 00137 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00138 # in the frame given in the header. 00139 geometry_msgs/Point32[] points 00140 00141 # Each channel should have the same number of elements as points array, 00142 # and the data in each channel should correspond 1:1 with each point. 00143 # Channel names in common practice are listed in ChannelFloat32.msg. 00144 ChannelFloat32[] channels 00145 00146 ================================================================================ 00147 MSG: geometry_msgs/Point32 00148 # This contains the position of a point in free space(with 32 bits of precision). 00149 # It is recommeded to use Point wherever possible instead of Point32. 00150 # 00151 # This recommendation is to promote interoperability. 00152 # 00153 # This message is designed to take up less space when sending 00154 # lots of points at once, as in the case of a PointCloud. 00155 00156 float32 x 00157 float32 y 00158 float32 z 00159 ================================================================================ 00160 MSG: sensor_msgs/ChannelFloat32 00161 # This message is used by the PointCloud message to hold optional data 00162 # associated with each point in the cloud. The length of the values 00163 # array should be the same as the length of the points array in the 00164 # PointCloud, and each value should be associated with the corresponding 00165 # point. 00166 00167 # Channel names in existing practice include: 00168 # "u", "v" - row and column (respectively) in the left stereo image. 00169 # This is opposite to usual conventions but remains for 00170 # historical reasons. The newer PointCloud2 message has no 00171 # such problem. 00172 # "rgb" - For point clouds produced by color stereo cameras. uint8 00173 # (R,G,B) values packed into the least significant 24 bits, 00174 # in order. 00175 # "intensity" - laser or pixel intensity. 00176 # "distance" 00177 00178 # The channel name should give semantics of the channel (e.g. 00179 # "intensity" instead of "value"). 00180 string name 00181 00182 # The values array should be 1-1 with the elements of the associated 00183 # PointCloud. 00184 float32[] values 00185 00186 ================================================================================ 00187 MSG: object_manipulation_msgs/SceneRegion 00188 # Point cloud 00189 sensor_msgs/PointCloud2 cloud 00190 00191 # Indices for the region of interest 00192 int32[] mask 00193 00194 # One of the corresponding 2D images, if applicable 00195 sensor_msgs/Image image 00196 00197 # The disparity image, if applicable 00198 sensor_msgs/Image disparity_image 00199 00200 # Camera info for the camera that took the image 00201 sensor_msgs/CameraInfo cam_info 00202 00203 # a 3D region of interest for grasp planning 00204 geometry_msgs/PoseStamped roi_box_pose 00205 geometry_msgs/Vector3 roi_box_dims 00206 00207 ================================================================================ 00208 MSG: sensor_msgs/PointCloud2 00209 # This message holds a collection of N-dimensional points, which may 00210 # contain additional information such as normals, intensity, etc. The 00211 # point data is stored as a binary blob, its layout described by the 00212 # contents of the "fields" array. 00213 00214 # The point cloud data may be organized 2d (image-like) or 1d 00215 # (unordered). Point clouds organized as 2d images may be produced by 00216 # camera depth sensors such as stereo or time-of-flight. 00217 00218 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00219 # points). 00220 Header header 00221 00222 # 2D structure of the point cloud. If the cloud is unordered, height is 00223 # 1 and width is the length of the point cloud. 00224 uint32 height 00225 uint32 width 00226 00227 # Describes the channels and their layout in the binary data blob. 00228 PointField[] fields 00229 00230 bool is_bigendian # Is this data bigendian? 00231 uint32 point_step # Length of a point in bytes 00232 uint32 row_step # Length of a row in bytes 00233 uint8[] data # Actual point data, size is (row_step*height) 00234 00235 bool is_dense # True if there are no invalid points 00236 00237 ================================================================================ 00238 MSG: sensor_msgs/PointField 00239 # This message holds the description of one point entry in the 00240 # PointCloud2 message format. 00241 uint8 INT8 = 1 00242 uint8 UINT8 = 2 00243 uint8 INT16 = 3 00244 uint8 UINT16 = 4 00245 uint8 INT32 = 5 00246 uint8 UINT32 = 6 00247 uint8 FLOAT32 = 7 00248 uint8 FLOAT64 = 8 00249 00250 string name # Name of field 00251 uint32 offset # Offset from start of point struct 00252 uint8 datatype # Datatype enumeration, see above 00253 uint32 count # How many elements in the field 00254 00255 ================================================================================ 00256 MSG: sensor_msgs/Image 00257 # This message contains an uncompressed image 00258 # (0, 0) is at top-left corner of image 00259 # 00260 00261 Header header # Header timestamp should be acquisition time of image 00262 # Header frame_id should be optical frame of camera 00263 # origin of frame should be optical center of cameara 00264 # +x should point to the right in the image 00265 # +y should point down in the image 00266 # +z should point into to plane of the image 00267 # If the frame_id here and the frame_id of the CameraInfo 00268 # message associated with the image conflict 00269 # the behavior is undefined 00270 00271 uint32 height # image height, that is, number of rows 00272 uint32 width # image width, that is, number of columns 00273 00274 # The legal values for encoding are in file src/image_encodings.cpp 00275 # If you want to standardize a new string format, join 00276 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00277 00278 string encoding # Encoding of pixels -- channel meaning, ordering, size 00279 # taken from the list of strings in src/image_encodings.cpp 00280 00281 uint8 is_bigendian # is this data bigendian? 00282 uint32 step # Full row length in bytes 00283 uint8[] data # actual matrix data, size is (step * rows) 00284 00285 ================================================================================ 00286 MSG: sensor_msgs/CameraInfo 00287 # This message defines meta information for a camera. It should be in a 00288 # camera namespace on topic "camera_info" and accompanied by up to five 00289 # image topics named: 00290 # 00291 # image_raw - raw data from the camera driver, possibly Bayer encoded 00292 # image - monochrome, distorted 00293 # image_color - color, distorted 00294 # image_rect - monochrome, rectified 00295 # image_rect_color - color, rectified 00296 # 00297 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00298 # for producing the four processed image topics from image_raw and 00299 # camera_info. The meaning of the camera parameters are described in 00300 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00301 # 00302 # The image_geometry package provides a user-friendly interface to 00303 # common operations using this meta information. If you want to, e.g., 00304 # project a 3d point into image coordinates, we strongly recommend 00305 # using image_geometry. 00306 # 00307 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00308 # zeroed out. In particular, clients may assume that K[0] == 0.0 00309 # indicates an uncalibrated camera. 00310 00311 ####################################################################### 00312 # Image acquisition info # 00313 ####################################################################### 00314 00315 # Time of image acquisition, camera coordinate frame ID 00316 Header header # Header timestamp should be acquisition time of image 00317 # Header frame_id should be optical frame of camera 00318 # origin of frame should be optical center of camera 00319 # +x should point to the right in the image 00320 # +y should point down in the image 00321 # +z should point into the plane of the image 00322 00323 00324 ####################################################################### 00325 # Calibration Parameters # 00326 ####################################################################### 00327 # These are fixed during camera calibration. Their values will be the # 00328 # same in all messages until the camera is recalibrated. Note that # 00329 # self-calibrating systems may "recalibrate" frequently. # 00330 # # 00331 # The internal parameters can be used to warp a raw (distorted) image # 00332 # to: # 00333 # 1. An undistorted image (requires D and K) # 00334 # 2. A rectified image (requires D, K, R) # 00335 # The projection matrix P projects 3D points into the rectified image.# 00336 ####################################################################### 00337 00338 # The image dimensions with which the camera was calibrated. Normally 00339 # this will be the full camera resolution in pixels. 00340 uint32 height 00341 uint32 width 00342 00343 # The distortion model used. Supported models are listed in 00344 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00345 # simple model of radial and tangential distortion - is sufficent. 00346 string distortion_model 00347 00348 # The distortion parameters, size depending on the distortion model. 00349 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00350 float64[] D 00351 00352 # Intrinsic camera matrix for the raw (distorted) images. 00353 # [fx 0 cx] 00354 # K = [ 0 fy cy] 00355 # [ 0 0 1] 00356 # Projects 3D points in the camera coordinate frame to 2D pixel 00357 # coordinates using the focal lengths (fx, fy) and principal point 00358 # (cx, cy). 00359 float64[9] K # 3x3 row-major matrix 00360 00361 # Rectification matrix (stereo cameras only) 00362 # A rotation matrix aligning the camera coordinate system to the ideal 00363 # stereo image plane so that epipolar lines in both stereo images are 00364 # parallel. 00365 float64[9] R # 3x3 row-major matrix 00366 00367 # Projection/camera matrix 00368 # [fx' 0 cx' Tx] 00369 # P = [ 0 fy' cy' Ty] 00370 # [ 0 0 1 0] 00371 # By convention, this matrix specifies the intrinsic (camera) matrix 00372 # of the processed (rectified) image. That is, the left 3x3 portion 00373 # is the normal camera intrinsic matrix for the rectified image. 00374 # It projects 3D points in the camera coordinate frame to 2D pixel 00375 # coordinates using the focal lengths (fx', fy') and principal point 00376 # (cx', cy') - these may differ from the values in K. 00377 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00378 # also have R = the identity and P[1:3,1:3] = K. 00379 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00380 # position of the optical center of the second camera in the first 00381 # camera's frame. We assume Tz = 0 so both cameras are in the same 00382 # stereo image plane. The first camera always has Tx = Ty = 0. For 00383 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00384 # Tx = -fx' * B, where B is the baseline between the cameras. 00385 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00386 # the rectified image is given by: 00387 # [u v w]' = P * [X Y Z 1]' 00388 # x = u / w 00389 # y = v / w 00390 # This holds for both images of a stereo pair. 00391 float64[12] P # 3x4 row-major matrix 00392 00393 00394 ####################################################################### 00395 # Operational Parameters # 00396 ####################################################################### 00397 # These define the image region actually captured by the camera # 00398 # driver. Although they affect the geometry of the output image, they # 00399 # may be changed freely without recalibrating the camera. # 00400 ####################################################################### 00401 00402 # Binning refers here to any camera setting which combines rectangular 00403 # neighborhoods of pixels into larger "super-pixels." It reduces the 00404 # resolution of the output image to 00405 # (width / binning_x) x (height / binning_y). 00406 # The default values binning_x = binning_y = 0 is considered the same 00407 # as binning_x = binning_y = 1 (no subsampling). 00408 uint32 binning_x 00409 uint32 binning_y 00410 00411 # Region of interest (subwindow of full camera resolution), given in 00412 # full resolution (unbinned) image coordinates. A particular ROI 00413 # always denotes the same window of pixels on the camera sensor, 00414 # regardless of binning settings. 00415 # The default setting of roi (all values 0) is considered the same as 00416 # full resolution (roi.width = width, roi.height = height). 00417 RegionOfInterest roi 00418 00419 ================================================================================ 00420 MSG: sensor_msgs/RegionOfInterest 00421 # This message is used to specify a region of interest within an image. 00422 # 00423 # When used to specify the ROI setting of the camera when the image was 00424 # taken, the height and width fields should either match the height and 00425 # width fields for the associated image; or height = width = 0 00426 # indicates that the full resolution image was captured. 00427 00428 uint32 x_offset # Leftmost pixel of the ROI 00429 # (0 if the ROI includes the left edge of the image) 00430 uint32 y_offset # Topmost pixel of the ROI 00431 # (0 if the ROI includes the top edge of the image) 00432 uint32 height # Height of ROI 00433 uint32 width # Width of ROI 00434 00435 # True if a distinct rectified ROI should be calculated from the "raw" 00436 # ROI in this message. Typically this should be False if the full image 00437 # is captured (ROI not used), and True if a subwindow is captured (ROI 00438 # used). 00439 bool do_rectify 00440 00441 ================================================================================ 00442 MSG: geometry_msgs/Vector3 00443 # This represents a vector in free space. 00444 00445 float64 x 00446 float64 y 00447 float64 z 00448 ================================================================================ 00449 MSG: object_manipulation_msgs/Grasp 00450 00451 # The internal posture of the hand for the pre-grasp 00452 # only positions are used 00453 sensor_msgs/JointState pre_grasp_posture 00454 00455 # The internal posture of the hand for the grasp 00456 # positions and efforts are used 00457 sensor_msgs/JointState grasp_posture 00458 00459 # The position of the end-effector for the grasp relative to a reference frame 00460 # (that is always specified elsewhere, not in this message) 00461 geometry_msgs/Pose grasp_pose 00462 00463 # The estimated probability of success for this grasp 00464 float64 success_probability 00465 00466 # Debug flag to indicate that this grasp would be the best in its cluster 00467 bool cluster_rep 00468 00469 # how far the pre-grasp should ideally be away from the grasp 00470 float32 desired_approach_distance 00471 00472 # how much distance between pre-grasp and grasp must actually be feasible 00473 # for the grasp not to be rejected 00474 float32 min_approach_distance 00475 00476 # an optional list of obstacles that we have semantic information about 00477 # and that we expect might move in the course of executing this grasp 00478 # the grasp planner is expected to make sure they move in an OK way; during 00479 # execution, grasp executors will not check for collisions against these objects 00480 GraspableObject[] moved_obstacles 00481 00482 ================================================================================ 00483 MSG: sensor_msgs/JointState 00484 # This is a message that holds data to describe the state of a set of torque controlled joints. 00485 # 00486 # The state of each joint (revolute or prismatic) is defined by: 00487 # * the position of the joint (rad or m), 00488 # * the velocity of the joint (rad/s or m/s) and 00489 # * the effort that is applied in the joint (Nm or N). 00490 # 00491 # Each joint is uniquely identified by its name 00492 # The header specifies the time at which the joint states were recorded. All the joint states 00493 # in one message have to be recorded at the same time. 00494 # 00495 # This message consists of a multiple arrays, one for each part of the joint state. 00496 # The goal is to make each of the fields optional. When e.g. your joints have no 00497 # effort associated with them, you can leave the effort array empty. 00498 # 00499 # All arrays in this message should have the same size, or be empty. 00500 # This is the only way to uniquely associate the joint name with the correct 00501 # states. 00502 00503 00504 Header header 00505 00506 string[] name 00507 float64[] position 00508 float64[] velocity 00509 float64[] effort 00510 00511 """ 00512 __slots__ = ['arm_name','target','collision_object_name','collision_support_surface_name','grasps_to_evaluate','movable_obstacles'] 00513 _slot_types = ['string','object_manipulation_msgs/GraspableObject','string','string','object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspableObject[]'] 00514 00515 def __init__(self, *args, **kwds): 00516 """ 00517 Constructor. Any message fields that are implicitly/explicitly 00518 set to None will be assigned a default value. The recommend 00519 use is keyword arguments as this is more robust to future message 00520 changes. You cannot mix in-order arguments and keyword arguments. 00521 00522 The available fields are: 00523 arm_name,target,collision_object_name,collision_support_surface_name,grasps_to_evaluate,movable_obstacles 00524 00525 @param args: complete set of field values, in .msg order 00526 @param kwds: use keyword arguments corresponding to message field names 00527 to set specific fields. 00528 """ 00529 if args or kwds: 00530 super(GraspPlanningRequest, self).__init__(*args, **kwds) 00531 #message fields cannot be None, assign default values for those that are 00532 if self.arm_name is None: 00533 self.arm_name = '' 00534 if self.target is None: 00535 self.target = object_manipulation_msgs.msg.GraspableObject() 00536 if self.collision_object_name is None: 00537 self.collision_object_name = '' 00538 if self.collision_support_surface_name is None: 00539 self.collision_support_surface_name = '' 00540 if self.grasps_to_evaluate is None: 00541 self.grasps_to_evaluate = [] 00542 if self.movable_obstacles is None: 00543 self.movable_obstacles = [] 00544 else: 00545 self.arm_name = '' 00546 self.target = object_manipulation_msgs.msg.GraspableObject() 00547 self.collision_object_name = '' 00548 self.collision_support_surface_name = '' 00549 self.grasps_to_evaluate = [] 00550 self.movable_obstacles = [] 00551 00552 def _get_types(self): 00553 """ 00554 internal API method 00555 """ 00556 return self._slot_types 00557 00558 def serialize(self, buff): 00559 """ 00560 serialize message into buffer 00561 @param buff: buffer 00562 @type buff: StringIO 00563 """ 00564 try: 00565 _x = self.arm_name 00566 length = len(_x) 00567 buff.write(struct.pack('<I%ss'%length, length, _x)) 00568 _x = self.target.reference_frame_id 00569 length = len(_x) 00570 buff.write(struct.pack('<I%ss'%length, length, _x)) 00571 length = len(self.target.potential_models) 00572 buff.write(_struct_I.pack(length)) 00573 for val1 in self.target.potential_models: 00574 buff.write(_struct_i.pack(val1.model_id)) 00575 _v1 = val1.pose 00576 _v2 = _v1.header 00577 buff.write(_struct_I.pack(_v2.seq)) 00578 _v3 = _v2.stamp 00579 _x = _v3 00580 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00581 _x = _v2.frame_id 00582 length = len(_x) 00583 buff.write(struct.pack('<I%ss'%length, length, _x)) 00584 _v4 = _v1.pose 00585 _v5 = _v4.position 00586 _x = _v5 00587 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00588 _v6 = _v4.orientation 00589 _x = _v6 00590 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00591 buff.write(_struct_f.pack(val1.confidence)) 00592 _x = val1.detector_name 00593 length = len(_x) 00594 buff.write(struct.pack('<I%ss'%length, length, _x)) 00595 _x = self 00596 buff.write(_struct_3I.pack(_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs)) 00597 _x = self.target.cluster.header.frame_id 00598 length = len(_x) 00599 buff.write(struct.pack('<I%ss'%length, length, _x)) 00600 length = len(self.target.cluster.points) 00601 buff.write(_struct_I.pack(length)) 00602 for val1 in self.target.cluster.points: 00603 _x = val1 00604 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00605 length = len(self.target.cluster.channels) 00606 buff.write(_struct_I.pack(length)) 00607 for val1 in self.target.cluster.channels: 00608 _x = val1.name 00609 length = len(_x) 00610 buff.write(struct.pack('<I%ss'%length, length, _x)) 00611 length = len(val1.values) 00612 buff.write(_struct_I.pack(length)) 00613 pattern = '<%sf'%length 00614 buff.write(struct.pack(pattern, *val1.values)) 00615 _x = self 00616 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)) 00617 _x = self.target.region.cloud.header.frame_id 00618 length = len(_x) 00619 buff.write(struct.pack('<I%ss'%length, length, _x)) 00620 _x = self 00621 buff.write(_struct_2I.pack(_x.target.region.cloud.height, _x.target.region.cloud.width)) 00622 length = len(self.target.region.cloud.fields) 00623 buff.write(_struct_I.pack(length)) 00624 for val1 in self.target.region.cloud.fields: 00625 _x = val1.name 00626 length = len(_x) 00627 buff.write(struct.pack('<I%ss'%length, length, _x)) 00628 _x = val1 00629 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00630 _x = self 00631 buff.write(_struct_B2I.pack(_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step)) 00632 _x = self.target.region.cloud.data 00633 length = len(_x) 00634 # - if encoded as a list instead, serialize as bytes instead of string 00635 if type(_x) in [list, tuple]: 00636 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00637 else: 00638 buff.write(struct.pack('<I%ss'%length, length, _x)) 00639 buff.write(_struct_B.pack(self.target.region.cloud.is_dense)) 00640 length = len(self.target.region.mask) 00641 buff.write(_struct_I.pack(length)) 00642 pattern = '<%si'%length 00643 buff.write(struct.pack(pattern, *self.target.region.mask)) 00644 _x = self 00645 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)) 00646 _x = self.target.region.image.header.frame_id 00647 length = len(_x) 00648 buff.write(struct.pack('<I%ss'%length, length, _x)) 00649 _x = self 00650 buff.write(_struct_2I.pack(_x.target.region.image.height, _x.target.region.image.width)) 00651 _x = self.target.region.image.encoding 00652 length = len(_x) 00653 buff.write(struct.pack('<I%ss'%length, length, _x)) 00654 _x = self 00655 buff.write(_struct_BI.pack(_x.target.region.image.is_bigendian, _x.target.region.image.step)) 00656 _x = self.target.region.image.data 00657 length = len(_x) 00658 # - if encoded as a list instead, serialize as bytes instead of string 00659 if type(_x) in [list, tuple]: 00660 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00661 else: 00662 buff.write(struct.pack('<I%ss'%length, length, _x)) 00663 _x = self 00664 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)) 00665 _x = self.target.region.disparity_image.header.frame_id 00666 length = len(_x) 00667 buff.write(struct.pack('<I%ss'%length, length, _x)) 00668 _x = self 00669 buff.write(_struct_2I.pack(_x.target.region.disparity_image.height, _x.target.region.disparity_image.width)) 00670 _x = self.target.region.disparity_image.encoding 00671 length = len(_x) 00672 buff.write(struct.pack('<I%ss'%length, length, _x)) 00673 _x = self 00674 buff.write(_struct_BI.pack(_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step)) 00675 _x = self.target.region.disparity_image.data 00676 length = len(_x) 00677 # - if encoded as a list instead, serialize as bytes instead of string 00678 if type(_x) in [list, tuple]: 00679 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00680 else: 00681 buff.write(struct.pack('<I%ss'%length, length, _x)) 00682 _x = self 00683 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)) 00684 _x = self.target.region.cam_info.header.frame_id 00685 length = len(_x) 00686 buff.write(struct.pack('<I%ss'%length, length, _x)) 00687 _x = self 00688 buff.write(_struct_2I.pack(_x.target.region.cam_info.height, _x.target.region.cam_info.width)) 00689 _x = self.target.region.cam_info.distortion_model 00690 length = len(_x) 00691 buff.write(struct.pack('<I%ss'%length, length, _x)) 00692 length = len(self.target.region.cam_info.D) 00693 buff.write(_struct_I.pack(length)) 00694 pattern = '<%sd'%length 00695 buff.write(struct.pack(pattern, *self.target.region.cam_info.D)) 00696 buff.write(_struct_9d.pack(*self.target.region.cam_info.K)) 00697 buff.write(_struct_9d.pack(*self.target.region.cam_info.R)) 00698 buff.write(_struct_12d.pack(*self.target.region.cam_info.P)) 00699 _x = self 00700 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)) 00701 _x = self.target.region.roi_box_pose.header.frame_id 00702 length = len(_x) 00703 buff.write(struct.pack('<I%ss'%length, length, _x)) 00704 _x = self 00705 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)) 00706 _x = self.target.collision_name 00707 length = len(_x) 00708 buff.write(struct.pack('<I%ss'%length, length, _x)) 00709 _x = self.collision_object_name 00710 length = len(_x) 00711 buff.write(struct.pack('<I%ss'%length, length, _x)) 00712 _x = self.collision_support_surface_name 00713 length = len(_x) 00714 buff.write(struct.pack('<I%ss'%length, length, _x)) 00715 length = len(self.grasps_to_evaluate) 00716 buff.write(_struct_I.pack(length)) 00717 for val1 in self.grasps_to_evaluate: 00718 _v7 = val1.pre_grasp_posture 00719 _v8 = _v7.header 00720 buff.write(_struct_I.pack(_v8.seq)) 00721 _v9 = _v8.stamp 00722 _x = _v9 00723 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00724 _x = _v8.frame_id 00725 length = len(_x) 00726 buff.write(struct.pack('<I%ss'%length, length, _x)) 00727 length = len(_v7.name) 00728 buff.write(_struct_I.pack(length)) 00729 for val3 in _v7.name: 00730 length = len(val3) 00731 buff.write(struct.pack('<I%ss'%length, length, val3)) 00732 length = len(_v7.position) 00733 buff.write(_struct_I.pack(length)) 00734 pattern = '<%sd'%length 00735 buff.write(struct.pack(pattern, *_v7.position)) 00736 length = len(_v7.velocity) 00737 buff.write(_struct_I.pack(length)) 00738 pattern = '<%sd'%length 00739 buff.write(struct.pack(pattern, *_v7.velocity)) 00740 length = len(_v7.effort) 00741 buff.write(_struct_I.pack(length)) 00742 pattern = '<%sd'%length 00743 buff.write(struct.pack(pattern, *_v7.effort)) 00744 _v10 = val1.grasp_posture 00745 _v11 = _v10.header 00746 buff.write(_struct_I.pack(_v11.seq)) 00747 _v12 = _v11.stamp 00748 _x = _v12 00749 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00750 _x = _v11.frame_id 00751 length = len(_x) 00752 buff.write(struct.pack('<I%ss'%length, length, _x)) 00753 length = len(_v10.name) 00754 buff.write(_struct_I.pack(length)) 00755 for val3 in _v10.name: 00756 length = len(val3) 00757 buff.write(struct.pack('<I%ss'%length, length, val3)) 00758 length = len(_v10.position) 00759 buff.write(_struct_I.pack(length)) 00760 pattern = '<%sd'%length 00761 buff.write(struct.pack(pattern, *_v10.position)) 00762 length = len(_v10.velocity) 00763 buff.write(_struct_I.pack(length)) 00764 pattern = '<%sd'%length 00765 buff.write(struct.pack(pattern, *_v10.velocity)) 00766 length = len(_v10.effort) 00767 buff.write(_struct_I.pack(length)) 00768 pattern = '<%sd'%length 00769 buff.write(struct.pack(pattern, *_v10.effort)) 00770 _v13 = val1.grasp_pose 00771 _v14 = _v13.position 00772 _x = _v14 00773 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00774 _v15 = _v13.orientation 00775 _x = _v15 00776 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00777 _x = val1 00778 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 00779 length = len(val1.moved_obstacles) 00780 buff.write(_struct_I.pack(length)) 00781 for val2 in val1.moved_obstacles: 00782 _x = val2.reference_frame_id 00783 length = len(_x) 00784 buff.write(struct.pack('<I%ss'%length, length, _x)) 00785 length = len(val2.potential_models) 00786 buff.write(_struct_I.pack(length)) 00787 for val3 in val2.potential_models: 00788 buff.write(_struct_i.pack(val3.model_id)) 00789 _v16 = val3.pose 00790 _v17 = _v16.header 00791 buff.write(_struct_I.pack(_v17.seq)) 00792 _v18 = _v17.stamp 00793 _x = _v18 00794 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00795 _x = _v17.frame_id 00796 length = len(_x) 00797 buff.write(struct.pack('<I%ss'%length, length, _x)) 00798 _v19 = _v16.pose 00799 _v20 = _v19.position 00800 _x = _v20 00801 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00802 _v21 = _v19.orientation 00803 _x = _v21 00804 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00805 buff.write(_struct_f.pack(val3.confidence)) 00806 _x = val3.detector_name 00807 length = len(_x) 00808 buff.write(struct.pack('<I%ss'%length, length, _x)) 00809 _v22 = val2.cluster 00810 _v23 = _v22.header 00811 buff.write(_struct_I.pack(_v23.seq)) 00812 _v24 = _v23.stamp 00813 _x = _v24 00814 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00815 _x = _v23.frame_id 00816 length = len(_x) 00817 buff.write(struct.pack('<I%ss'%length, length, _x)) 00818 length = len(_v22.points) 00819 buff.write(_struct_I.pack(length)) 00820 for val4 in _v22.points: 00821 _x = val4 00822 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00823 length = len(_v22.channels) 00824 buff.write(_struct_I.pack(length)) 00825 for val4 in _v22.channels: 00826 _x = val4.name 00827 length = len(_x) 00828 buff.write(struct.pack('<I%ss'%length, length, _x)) 00829 length = len(val4.values) 00830 buff.write(_struct_I.pack(length)) 00831 pattern = '<%sf'%length 00832 buff.write(struct.pack(pattern, *val4.values)) 00833 _v25 = val2.region 00834 _v26 = _v25.cloud 00835 _v27 = _v26.header 00836 buff.write(_struct_I.pack(_v27.seq)) 00837 _v28 = _v27.stamp 00838 _x = _v28 00839 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00840 _x = _v27.frame_id 00841 length = len(_x) 00842 buff.write(struct.pack('<I%ss'%length, length, _x)) 00843 _x = _v26 00844 buff.write(_struct_2I.pack(_x.height, _x.width)) 00845 length = len(_v26.fields) 00846 buff.write(_struct_I.pack(length)) 00847 for val5 in _v26.fields: 00848 _x = val5.name 00849 length = len(_x) 00850 buff.write(struct.pack('<I%ss'%length, length, _x)) 00851 _x = val5 00852 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00853 _x = _v26 00854 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 00855 _x = _v26.data 00856 length = len(_x) 00857 # - if encoded as a list instead, serialize as bytes instead of string 00858 if type(_x) in [list, tuple]: 00859 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00860 else: 00861 buff.write(struct.pack('<I%ss'%length, length, _x)) 00862 buff.write(_struct_B.pack(_v26.is_dense)) 00863 length = len(_v25.mask) 00864 buff.write(_struct_I.pack(length)) 00865 pattern = '<%si'%length 00866 buff.write(struct.pack(pattern, *_v25.mask)) 00867 _v29 = _v25.image 00868 _v30 = _v29.header 00869 buff.write(_struct_I.pack(_v30.seq)) 00870 _v31 = _v30.stamp 00871 _x = _v31 00872 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00873 _x = _v30.frame_id 00874 length = len(_x) 00875 buff.write(struct.pack('<I%ss'%length, length, _x)) 00876 _x = _v29 00877 buff.write(_struct_2I.pack(_x.height, _x.width)) 00878 _x = _v29.encoding 00879 length = len(_x) 00880 buff.write(struct.pack('<I%ss'%length, length, _x)) 00881 _x = _v29 00882 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00883 _x = _v29.data 00884 length = len(_x) 00885 # - if encoded as a list instead, serialize as bytes instead of string 00886 if type(_x) in [list, tuple]: 00887 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00888 else: 00889 buff.write(struct.pack('<I%ss'%length, length, _x)) 00890 _v32 = _v25.disparity_image 00891 _v33 = _v32.header 00892 buff.write(_struct_I.pack(_v33.seq)) 00893 _v34 = _v33.stamp 00894 _x = _v34 00895 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00896 _x = _v33.frame_id 00897 length = len(_x) 00898 buff.write(struct.pack('<I%ss'%length, length, _x)) 00899 _x = _v32 00900 buff.write(_struct_2I.pack(_x.height, _x.width)) 00901 _x = _v32.encoding 00902 length = len(_x) 00903 buff.write(struct.pack('<I%ss'%length, length, _x)) 00904 _x = _v32 00905 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00906 _x = _v32.data 00907 length = len(_x) 00908 # - if encoded as a list instead, serialize as bytes instead of string 00909 if type(_x) in [list, tuple]: 00910 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00911 else: 00912 buff.write(struct.pack('<I%ss'%length, length, _x)) 00913 _v35 = _v25.cam_info 00914 _v36 = _v35.header 00915 buff.write(_struct_I.pack(_v36.seq)) 00916 _v37 = _v36.stamp 00917 _x = _v37 00918 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00919 _x = _v36.frame_id 00920 length = len(_x) 00921 buff.write(struct.pack('<I%ss'%length, length, _x)) 00922 _x = _v35 00923 buff.write(_struct_2I.pack(_x.height, _x.width)) 00924 _x = _v35.distortion_model 00925 length = len(_x) 00926 buff.write(struct.pack('<I%ss'%length, length, _x)) 00927 length = len(_v35.D) 00928 buff.write(_struct_I.pack(length)) 00929 pattern = '<%sd'%length 00930 buff.write(struct.pack(pattern, *_v35.D)) 00931 buff.write(_struct_9d.pack(*_v35.K)) 00932 buff.write(_struct_9d.pack(*_v35.R)) 00933 buff.write(_struct_12d.pack(*_v35.P)) 00934 _x = _v35 00935 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00936 _v38 = _v35.roi 00937 _x = _v38 00938 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00939 _v39 = _v25.roi_box_pose 00940 _v40 = _v39.header 00941 buff.write(_struct_I.pack(_v40.seq)) 00942 _v41 = _v40.stamp 00943 _x = _v41 00944 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00945 _x = _v40.frame_id 00946 length = len(_x) 00947 buff.write(struct.pack('<I%ss'%length, length, _x)) 00948 _v42 = _v39.pose 00949 _v43 = _v42.position 00950 _x = _v43 00951 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00952 _v44 = _v42.orientation 00953 _x = _v44 00954 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00955 _v45 = _v25.roi_box_dims 00956 _x = _v45 00957 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00958 _x = val2.collision_name 00959 length = len(_x) 00960 buff.write(struct.pack('<I%ss'%length, length, _x)) 00961 length = len(self.movable_obstacles) 00962 buff.write(_struct_I.pack(length)) 00963 for val1 in self.movable_obstacles: 00964 _x = val1.reference_frame_id 00965 length = len(_x) 00966 buff.write(struct.pack('<I%ss'%length, length, _x)) 00967 length = len(val1.potential_models) 00968 buff.write(_struct_I.pack(length)) 00969 for val2 in val1.potential_models: 00970 buff.write(_struct_i.pack(val2.model_id)) 00971 _v46 = val2.pose 00972 _v47 = _v46.header 00973 buff.write(_struct_I.pack(_v47.seq)) 00974 _v48 = _v47.stamp 00975 _x = _v48 00976 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00977 _x = _v47.frame_id 00978 length = len(_x) 00979 buff.write(struct.pack('<I%ss'%length, length, _x)) 00980 _v49 = _v46.pose 00981 _v50 = _v49.position 00982 _x = _v50 00983 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00984 _v51 = _v49.orientation 00985 _x = _v51 00986 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00987 buff.write(_struct_f.pack(val2.confidence)) 00988 _x = val2.detector_name 00989 length = len(_x) 00990 buff.write(struct.pack('<I%ss'%length, length, _x)) 00991 _v52 = val1.cluster 00992 _v53 = _v52.header 00993 buff.write(_struct_I.pack(_v53.seq)) 00994 _v54 = _v53.stamp 00995 _x = _v54 00996 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00997 _x = _v53.frame_id 00998 length = len(_x) 00999 buff.write(struct.pack('<I%ss'%length, length, _x)) 01000 length = len(_v52.points) 01001 buff.write(_struct_I.pack(length)) 01002 for val3 in _v52.points: 01003 _x = val3 01004 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01005 length = len(_v52.channels) 01006 buff.write(_struct_I.pack(length)) 01007 for val3 in _v52.channels: 01008 _x = val3.name 01009 length = len(_x) 01010 buff.write(struct.pack('<I%ss'%length, length, _x)) 01011 length = len(val3.values) 01012 buff.write(_struct_I.pack(length)) 01013 pattern = '<%sf'%length 01014 buff.write(struct.pack(pattern, *val3.values)) 01015 _v55 = val1.region 01016 _v56 = _v55.cloud 01017 _v57 = _v56.header 01018 buff.write(_struct_I.pack(_v57.seq)) 01019 _v58 = _v57.stamp 01020 _x = _v58 01021 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01022 _x = _v57.frame_id 01023 length = len(_x) 01024 buff.write(struct.pack('<I%ss'%length, length, _x)) 01025 _x = _v56 01026 buff.write(_struct_2I.pack(_x.height, _x.width)) 01027 length = len(_v56.fields) 01028 buff.write(_struct_I.pack(length)) 01029 for val4 in _v56.fields: 01030 _x = val4.name 01031 length = len(_x) 01032 buff.write(struct.pack('<I%ss'%length, length, _x)) 01033 _x = val4 01034 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01035 _x = _v56 01036 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01037 _x = _v56.data 01038 length = len(_x) 01039 # - if encoded as a list instead, serialize as bytes instead of string 01040 if type(_x) in [list, tuple]: 01041 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01042 else: 01043 buff.write(struct.pack('<I%ss'%length, length, _x)) 01044 buff.write(_struct_B.pack(_v56.is_dense)) 01045 length = len(_v55.mask) 01046 buff.write(_struct_I.pack(length)) 01047 pattern = '<%si'%length 01048 buff.write(struct.pack(pattern, *_v55.mask)) 01049 _v59 = _v55.image 01050 _v60 = _v59.header 01051 buff.write(_struct_I.pack(_v60.seq)) 01052 _v61 = _v60.stamp 01053 _x = _v61 01054 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01055 _x = _v60.frame_id 01056 length = len(_x) 01057 buff.write(struct.pack('<I%ss'%length, length, _x)) 01058 _x = _v59 01059 buff.write(_struct_2I.pack(_x.height, _x.width)) 01060 _x = _v59.encoding 01061 length = len(_x) 01062 buff.write(struct.pack('<I%ss'%length, length, _x)) 01063 _x = _v59 01064 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01065 _x = _v59.data 01066 length = len(_x) 01067 # - if encoded as a list instead, serialize as bytes instead of string 01068 if type(_x) in [list, tuple]: 01069 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01070 else: 01071 buff.write(struct.pack('<I%ss'%length, length, _x)) 01072 _v62 = _v55.disparity_image 01073 _v63 = _v62.header 01074 buff.write(_struct_I.pack(_v63.seq)) 01075 _v64 = _v63.stamp 01076 _x = _v64 01077 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01078 _x = _v63.frame_id 01079 length = len(_x) 01080 buff.write(struct.pack('<I%ss'%length, length, _x)) 01081 _x = _v62 01082 buff.write(_struct_2I.pack(_x.height, _x.width)) 01083 _x = _v62.encoding 01084 length = len(_x) 01085 buff.write(struct.pack('<I%ss'%length, length, _x)) 01086 _x = _v62 01087 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01088 _x = _v62.data 01089 length = len(_x) 01090 # - if encoded as a list instead, serialize as bytes instead of string 01091 if type(_x) in [list, tuple]: 01092 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01093 else: 01094 buff.write(struct.pack('<I%ss'%length, length, _x)) 01095 _v65 = _v55.cam_info 01096 _v66 = _v65.header 01097 buff.write(_struct_I.pack(_v66.seq)) 01098 _v67 = _v66.stamp 01099 _x = _v67 01100 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01101 _x = _v66.frame_id 01102 length = len(_x) 01103 buff.write(struct.pack('<I%ss'%length, length, _x)) 01104 _x = _v65 01105 buff.write(_struct_2I.pack(_x.height, _x.width)) 01106 _x = _v65.distortion_model 01107 length = len(_x) 01108 buff.write(struct.pack('<I%ss'%length, length, _x)) 01109 length = len(_v65.D) 01110 buff.write(_struct_I.pack(length)) 01111 pattern = '<%sd'%length 01112 buff.write(struct.pack(pattern, *_v65.D)) 01113 buff.write(_struct_9d.pack(*_v65.K)) 01114 buff.write(_struct_9d.pack(*_v65.R)) 01115 buff.write(_struct_12d.pack(*_v65.P)) 01116 _x = _v65 01117 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01118 _v68 = _v65.roi 01119 _x = _v68 01120 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01121 _v69 = _v55.roi_box_pose 01122 _v70 = _v69.header 01123 buff.write(_struct_I.pack(_v70.seq)) 01124 _v71 = _v70.stamp 01125 _x = _v71 01126 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01127 _x = _v70.frame_id 01128 length = len(_x) 01129 buff.write(struct.pack('<I%ss'%length, length, _x)) 01130 _v72 = _v69.pose 01131 _v73 = _v72.position 01132 _x = _v73 01133 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01134 _v74 = _v72.orientation 01135 _x = _v74 01136 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01137 _v75 = _v55.roi_box_dims 01138 _x = _v75 01139 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01140 _x = val1.collision_name 01141 length = len(_x) 01142 buff.write(struct.pack('<I%ss'%length, length, _x)) 01143 except struct.error as se: self._check_types(se) 01144 except TypeError as te: self._check_types(te) 01145 01146 def deserialize(self, str): 01147 """ 01148 unpack serialized message in str into this message instance 01149 @param str: byte array of serialized message 01150 @type str: str 01151 """ 01152 try: 01153 if self.target is None: 01154 self.target = object_manipulation_msgs.msg.GraspableObject() 01155 end = 0 01156 start = end 01157 end += 4 01158 (length,) = _struct_I.unpack(str[start:end]) 01159 start = end 01160 end += length 01161 self.arm_name = str[start:end] 01162 start = end 01163 end += 4 01164 (length,) = _struct_I.unpack(str[start:end]) 01165 start = end 01166 end += length 01167 self.target.reference_frame_id = str[start:end] 01168 start = end 01169 end += 4 01170 (length,) = _struct_I.unpack(str[start:end]) 01171 self.target.potential_models = [] 01172 for i in range(0, length): 01173 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 01174 start = end 01175 end += 4 01176 (val1.model_id,) = _struct_i.unpack(str[start:end]) 01177 _v76 = val1.pose 01178 _v77 = _v76.header 01179 start = end 01180 end += 4 01181 (_v77.seq,) = _struct_I.unpack(str[start:end]) 01182 _v78 = _v77.stamp 01183 _x = _v78 01184 start = end 01185 end += 8 01186 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01187 start = end 01188 end += 4 01189 (length,) = _struct_I.unpack(str[start:end]) 01190 start = end 01191 end += length 01192 _v77.frame_id = str[start:end] 01193 _v79 = _v76.pose 01194 _v80 = _v79.position 01195 _x = _v80 01196 start = end 01197 end += 24 01198 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01199 _v81 = _v79.orientation 01200 _x = _v81 01201 start = end 01202 end += 32 01203 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01204 start = end 01205 end += 4 01206 (val1.confidence,) = _struct_f.unpack(str[start:end]) 01207 start = end 01208 end += 4 01209 (length,) = _struct_I.unpack(str[start:end]) 01210 start = end 01211 end += length 01212 val1.detector_name = str[start:end] 01213 self.target.potential_models.append(val1) 01214 _x = self 01215 start = end 01216 end += 12 01217 (_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01218 start = end 01219 end += 4 01220 (length,) = _struct_I.unpack(str[start:end]) 01221 start = end 01222 end += length 01223 self.target.cluster.header.frame_id = str[start:end] 01224 start = end 01225 end += 4 01226 (length,) = _struct_I.unpack(str[start:end]) 01227 self.target.cluster.points = [] 01228 for i in range(0, length): 01229 val1 = geometry_msgs.msg.Point32() 01230 _x = val1 01231 start = end 01232 end += 12 01233 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01234 self.target.cluster.points.append(val1) 01235 start = end 01236 end += 4 01237 (length,) = _struct_I.unpack(str[start:end]) 01238 self.target.cluster.channels = [] 01239 for i in range(0, length): 01240 val1 = sensor_msgs.msg.ChannelFloat32() 01241 start = end 01242 end += 4 01243 (length,) = _struct_I.unpack(str[start:end]) 01244 start = end 01245 end += length 01246 val1.name = str[start:end] 01247 start = end 01248 end += 4 01249 (length,) = _struct_I.unpack(str[start:end]) 01250 pattern = '<%sf'%length 01251 start = end 01252 end += struct.calcsize(pattern) 01253 val1.values = struct.unpack(pattern, str[start:end]) 01254 self.target.cluster.channels.append(val1) 01255 _x = self 01256 start = end 01257 end += 12 01258 (_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]) 01259 start = end 01260 end += 4 01261 (length,) = _struct_I.unpack(str[start:end]) 01262 start = end 01263 end += length 01264 self.target.region.cloud.header.frame_id = str[start:end] 01265 _x = self 01266 start = end 01267 end += 8 01268 (_x.target.region.cloud.height, _x.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 01269 start = end 01270 end += 4 01271 (length,) = _struct_I.unpack(str[start:end]) 01272 self.target.region.cloud.fields = [] 01273 for i in range(0, length): 01274 val1 = sensor_msgs.msg.PointField() 01275 start = end 01276 end += 4 01277 (length,) = _struct_I.unpack(str[start:end]) 01278 start = end 01279 end += length 01280 val1.name = str[start:end] 01281 _x = val1 01282 start = end 01283 end += 9 01284 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01285 self.target.region.cloud.fields.append(val1) 01286 _x = self 01287 start = end 01288 end += 9 01289 (_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 01290 self.target.region.cloud.is_bigendian = bool(self.target.region.cloud.is_bigendian) 01291 start = end 01292 end += 4 01293 (length,) = _struct_I.unpack(str[start:end]) 01294 start = end 01295 end += length 01296 self.target.region.cloud.data = str[start:end] 01297 start = end 01298 end += 1 01299 (self.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 01300 self.target.region.cloud.is_dense = bool(self.target.region.cloud.is_dense) 01301 start = end 01302 end += 4 01303 (length,) = _struct_I.unpack(str[start:end]) 01304 pattern = '<%si'%length 01305 start = end 01306 end += struct.calcsize(pattern) 01307 self.target.region.mask = struct.unpack(pattern, str[start:end]) 01308 _x = self 01309 start = end 01310 end += 12 01311 (_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]) 01312 start = end 01313 end += 4 01314 (length,) = _struct_I.unpack(str[start:end]) 01315 start = end 01316 end += length 01317 self.target.region.image.header.frame_id = str[start:end] 01318 _x = self 01319 start = end 01320 end += 8 01321 (_x.target.region.image.height, _x.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 01322 start = end 01323 end += 4 01324 (length,) = _struct_I.unpack(str[start:end]) 01325 start = end 01326 end += length 01327 self.target.region.image.encoding = str[start:end] 01328 _x = self 01329 start = end 01330 end += 5 01331 (_x.target.region.image.is_bigendian, _x.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 01332 start = end 01333 end += 4 01334 (length,) = _struct_I.unpack(str[start:end]) 01335 start = end 01336 end += length 01337 self.target.region.image.data = str[start:end] 01338 _x = self 01339 start = end 01340 end += 12 01341 (_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]) 01342 start = end 01343 end += 4 01344 (length,) = _struct_I.unpack(str[start:end]) 01345 start = end 01346 end += length 01347 self.target.region.disparity_image.header.frame_id = str[start:end] 01348 _x = self 01349 start = end 01350 end += 8 01351 (_x.target.region.disparity_image.height, _x.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 01352 start = end 01353 end += 4 01354 (length,) = _struct_I.unpack(str[start:end]) 01355 start = end 01356 end += length 01357 self.target.region.disparity_image.encoding = str[start:end] 01358 _x = self 01359 start = end 01360 end += 5 01361 (_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 01362 start = end 01363 end += 4 01364 (length,) = _struct_I.unpack(str[start:end]) 01365 start = end 01366 end += length 01367 self.target.region.disparity_image.data = str[start:end] 01368 _x = self 01369 start = end 01370 end += 12 01371 (_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]) 01372 start = end 01373 end += 4 01374 (length,) = _struct_I.unpack(str[start:end]) 01375 start = end 01376 end += length 01377 self.target.region.cam_info.header.frame_id = str[start:end] 01378 _x = self 01379 start = end 01380 end += 8 01381 (_x.target.region.cam_info.height, _x.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 01382 start = end 01383 end += 4 01384 (length,) = _struct_I.unpack(str[start:end]) 01385 start = end 01386 end += length 01387 self.target.region.cam_info.distortion_model = str[start:end] 01388 start = end 01389 end += 4 01390 (length,) = _struct_I.unpack(str[start:end]) 01391 pattern = '<%sd'%length 01392 start = end 01393 end += struct.calcsize(pattern) 01394 self.target.region.cam_info.D = struct.unpack(pattern, str[start:end]) 01395 start = end 01396 end += 72 01397 self.target.region.cam_info.K = _struct_9d.unpack(str[start:end]) 01398 start = end 01399 end += 72 01400 self.target.region.cam_info.R = _struct_9d.unpack(str[start:end]) 01401 start = end 01402 end += 96 01403 self.target.region.cam_info.P = _struct_12d.unpack(str[start:end]) 01404 _x = self 01405 start = end 01406 end += 37 01407 (_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]) 01408 self.target.region.cam_info.roi.do_rectify = bool(self.target.region.cam_info.roi.do_rectify) 01409 start = end 01410 end += 4 01411 (length,) = _struct_I.unpack(str[start:end]) 01412 start = end 01413 end += length 01414 self.target.region.roi_box_pose.header.frame_id = str[start:end] 01415 _x = self 01416 start = end 01417 end += 80 01418 (_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]) 01419 start = end 01420 end += 4 01421 (length,) = _struct_I.unpack(str[start:end]) 01422 start = end 01423 end += length 01424 self.target.collision_name = str[start:end] 01425 start = end 01426 end += 4 01427 (length,) = _struct_I.unpack(str[start:end]) 01428 start = end 01429 end += length 01430 self.collision_object_name = str[start:end] 01431 start = end 01432 end += 4 01433 (length,) = _struct_I.unpack(str[start:end]) 01434 start = end 01435 end += length 01436 self.collision_support_surface_name = str[start:end] 01437 start = end 01438 end += 4 01439 (length,) = _struct_I.unpack(str[start:end]) 01440 self.grasps_to_evaluate = [] 01441 for i in range(0, length): 01442 val1 = object_manipulation_msgs.msg.Grasp() 01443 _v82 = val1.pre_grasp_posture 01444 _v83 = _v82.header 01445 start = end 01446 end += 4 01447 (_v83.seq,) = _struct_I.unpack(str[start:end]) 01448 _v84 = _v83.stamp 01449 _x = _v84 01450 start = end 01451 end += 8 01452 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01453 start = end 01454 end += 4 01455 (length,) = _struct_I.unpack(str[start:end]) 01456 start = end 01457 end += length 01458 _v83.frame_id = str[start:end] 01459 start = end 01460 end += 4 01461 (length,) = _struct_I.unpack(str[start:end]) 01462 _v82.name = [] 01463 for i in range(0, length): 01464 start = end 01465 end += 4 01466 (length,) = _struct_I.unpack(str[start:end]) 01467 start = end 01468 end += length 01469 val3 = str[start:end] 01470 _v82.name.append(val3) 01471 start = end 01472 end += 4 01473 (length,) = _struct_I.unpack(str[start:end]) 01474 pattern = '<%sd'%length 01475 start = end 01476 end += struct.calcsize(pattern) 01477 _v82.position = struct.unpack(pattern, str[start:end]) 01478 start = end 01479 end += 4 01480 (length,) = _struct_I.unpack(str[start:end]) 01481 pattern = '<%sd'%length 01482 start = end 01483 end += struct.calcsize(pattern) 01484 _v82.velocity = struct.unpack(pattern, str[start:end]) 01485 start = end 01486 end += 4 01487 (length,) = _struct_I.unpack(str[start:end]) 01488 pattern = '<%sd'%length 01489 start = end 01490 end += struct.calcsize(pattern) 01491 _v82.effort = struct.unpack(pattern, str[start:end]) 01492 _v85 = val1.grasp_posture 01493 _v86 = _v85.header 01494 start = end 01495 end += 4 01496 (_v86.seq,) = _struct_I.unpack(str[start:end]) 01497 _v87 = _v86.stamp 01498 _x = _v87 01499 start = end 01500 end += 8 01501 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01502 start = end 01503 end += 4 01504 (length,) = _struct_I.unpack(str[start:end]) 01505 start = end 01506 end += length 01507 _v86.frame_id = str[start:end] 01508 start = end 01509 end += 4 01510 (length,) = _struct_I.unpack(str[start:end]) 01511 _v85.name = [] 01512 for i in range(0, length): 01513 start = end 01514 end += 4 01515 (length,) = _struct_I.unpack(str[start:end]) 01516 start = end 01517 end += length 01518 val3 = str[start:end] 01519 _v85.name.append(val3) 01520 start = end 01521 end += 4 01522 (length,) = _struct_I.unpack(str[start:end]) 01523 pattern = '<%sd'%length 01524 start = end 01525 end += struct.calcsize(pattern) 01526 _v85.position = struct.unpack(pattern, str[start:end]) 01527 start = end 01528 end += 4 01529 (length,) = _struct_I.unpack(str[start:end]) 01530 pattern = '<%sd'%length 01531 start = end 01532 end += struct.calcsize(pattern) 01533 _v85.velocity = struct.unpack(pattern, str[start:end]) 01534 start = end 01535 end += 4 01536 (length,) = _struct_I.unpack(str[start:end]) 01537 pattern = '<%sd'%length 01538 start = end 01539 end += struct.calcsize(pattern) 01540 _v85.effort = struct.unpack(pattern, str[start:end]) 01541 _v88 = val1.grasp_pose 01542 _v89 = _v88.position 01543 _x = _v89 01544 start = end 01545 end += 24 01546 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01547 _v90 = _v88.orientation 01548 _x = _v90 01549 start = end 01550 end += 32 01551 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01552 _x = val1 01553 start = end 01554 end += 17 01555 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 01556 val1.cluster_rep = bool(val1.cluster_rep) 01557 start = end 01558 end += 4 01559 (length,) = _struct_I.unpack(str[start:end]) 01560 val1.moved_obstacles = [] 01561 for i in range(0, length): 01562 val2 = object_manipulation_msgs.msg.GraspableObject() 01563 start = end 01564 end += 4 01565 (length,) = _struct_I.unpack(str[start:end]) 01566 start = end 01567 end += length 01568 val2.reference_frame_id = str[start:end] 01569 start = end 01570 end += 4 01571 (length,) = _struct_I.unpack(str[start:end]) 01572 val2.potential_models = [] 01573 for i in range(0, length): 01574 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 01575 start = end 01576 end += 4 01577 (val3.model_id,) = _struct_i.unpack(str[start:end]) 01578 _v91 = val3.pose 01579 _v92 = _v91.header 01580 start = end 01581 end += 4 01582 (_v92.seq,) = _struct_I.unpack(str[start:end]) 01583 _v93 = _v92.stamp 01584 _x = _v93 01585 start = end 01586 end += 8 01587 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01588 start = end 01589 end += 4 01590 (length,) = _struct_I.unpack(str[start:end]) 01591 start = end 01592 end += length 01593 _v92.frame_id = str[start:end] 01594 _v94 = _v91.pose 01595 _v95 = _v94.position 01596 _x = _v95 01597 start = end 01598 end += 24 01599 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01600 _v96 = _v94.orientation 01601 _x = _v96 01602 start = end 01603 end += 32 01604 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01605 start = end 01606 end += 4 01607 (val3.confidence,) = _struct_f.unpack(str[start:end]) 01608 start = end 01609 end += 4 01610 (length,) = _struct_I.unpack(str[start:end]) 01611 start = end 01612 end += length 01613 val3.detector_name = str[start:end] 01614 val2.potential_models.append(val3) 01615 _v97 = val2.cluster 01616 _v98 = _v97.header 01617 start = end 01618 end += 4 01619 (_v98.seq,) = _struct_I.unpack(str[start:end]) 01620 _v99 = _v98.stamp 01621 _x = _v99 01622 start = end 01623 end += 8 01624 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01625 start = end 01626 end += 4 01627 (length,) = _struct_I.unpack(str[start:end]) 01628 start = end 01629 end += length 01630 _v98.frame_id = str[start:end] 01631 start = end 01632 end += 4 01633 (length,) = _struct_I.unpack(str[start:end]) 01634 _v97.points = [] 01635 for i in range(0, length): 01636 val4 = geometry_msgs.msg.Point32() 01637 _x = val4 01638 start = end 01639 end += 12 01640 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01641 _v97.points.append(val4) 01642 start = end 01643 end += 4 01644 (length,) = _struct_I.unpack(str[start:end]) 01645 _v97.channels = [] 01646 for i in range(0, length): 01647 val4 = sensor_msgs.msg.ChannelFloat32() 01648 start = end 01649 end += 4 01650 (length,) = _struct_I.unpack(str[start:end]) 01651 start = end 01652 end += length 01653 val4.name = str[start:end] 01654 start = end 01655 end += 4 01656 (length,) = _struct_I.unpack(str[start:end]) 01657 pattern = '<%sf'%length 01658 start = end 01659 end += struct.calcsize(pattern) 01660 val4.values = struct.unpack(pattern, str[start:end]) 01661 _v97.channels.append(val4) 01662 _v100 = val2.region 01663 _v101 = _v100.cloud 01664 _v102 = _v101.header 01665 start = end 01666 end += 4 01667 (_v102.seq,) = _struct_I.unpack(str[start:end]) 01668 _v103 = _v102.stamp 01669 _x = _v103 01670 start = end 01671 end += 8 01672 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01673 start = end 01674 end += 4 01675 (length,) = _struct_I.unpack(str[start:end]) 01676 start = end 01677 end += length 01678 _v102.frame_id = str[start:end] 01679 _x = _v101 01680 start = end 01681 end += 8 01682 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01683 start = end 01684 end += 4 01685 (length,) = _struct_I.unpack(str[start:end]) 01686 _v101.fields = [] 01687 for i in range(0, length): 01688 val5 = sensor_msgs.msg.PointField() 01689 start = end 01690 end += 4 01691 (length,) = _struct_I.unpack(str[start:end]) 01692 start = end 01693 end += length 01694 val5.name = str[start:end] 01695 _x = val5 01696 start = end 01697 end += 9 01698 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01699 _v101.fields.append(val5) 01700 _x = _v101 01701 start = end 01702 end += 9 01703 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01704 _v101.is_bigendian = bool(_v101.is_bigendian) 01705 start = end 01706 end += 4 01707 (length,) = _struct_I.unpack(str[start:end]) 01708 start = end 01709 end += length 01710 _v101.data = str[start:end] 01711 start = end 01712 end += 1 01713 (_v101.is_dense,) = _struct_B.unpack(str[start:end]) 01714 _v101.is_dense = bool(_v101.is_dense) 01715 start = end 01716 end += 4 01717 (length,) = _struct_I.unpack(str[start:end]) 01718 pattern = '<%si'%length 01719 start = end 01720 end += struct.calcsize(pattern) 01721 _v100.mask = struct.unpack(pattern, str[start:end]) 01722 _v104 = _v100.image 01723 _v105 = _v104.header 01724 start = end 01725 end += 4 01726 (_v105.seq,) = _struct_I.unpack(str[start:end]) 01727 _v106 = _v105.stamp 01728 _x = _v106 01729 start = end 01730 end += 8 01731 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01732 start = end 01733 end += 4 01734 (length,) = _struct_I.unpack(str[start:end]) 01735 start = end 01736 end += length 01737 _v105.frame_id = str[start:end] 01738 _x = _v104 01739 start = end 01740 end += 8 01741 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01742 start = end 01743 end += 4 01744 (length,) = _struct_I.unpack(str[start:end]) 01745 start = end 01746 end += length 01747 _v104.encoding = str[start:end] 01748 _x = _v104 01749 start = end 01750 end += 5 01751 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01752 start = end 01753 end += 4 01754 (length,) = _struct_I.unpack(str[start:end]) 01755 start = end 01756 end += length 01757 _v104.data = str[start:end] 01758 _v107 = _v100.disparity_image 01759 _v108 = _v107.header 01760 start = end 01761 end += 4 01762 (_v108.seq,) = _struct_I.unpack(str[start:end]) 01763 _v109 = _v108.stamp 01764 _x = _v109 01765 start = end 01766 end += 8 01767 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01768 start = end 01769 end += 4 01770 (length,) = _struct_I.unpack(str[start:end]) 01771 start = end 01772 end += length 01773 _v108.frame_id = str[start:end] 01774 _x = _v107 01775 start = end 01776 end += 8 01777 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01778 start = end 01779 end += 4 01780 (length,) = _struct_I.unpack(str[start:end]) 01781 start = end 01782 end += length 01783 _v107.encoding = str[start:end] 01784 _x = _v107 01785 start = end 01786 end += 5 01787 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01788 start = end 01789 end += 4 01790 (length,) = _struct_I.unpack(str[start:end]) 01791 start = end 01792 end += length 01793 _v107.data = str[start:end] 01794 _v110 = _v100.cam_info 01795 _v111 = _v110.header 01796 start = end 01797 end += 4 01798 (_v111.seq,) = _struct_I.unpack(str[start:end]) 01799 _v112 = _v111.stamp 01800 _x = _v112 01801 start = end 01802 end += 8 01803 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01804 start = end 01805 end += 4 01806 (length,) = _struct_I.unpack(str[start:end]) 01807 start = end 01808 end += length 01809 _v111.frame_id = str[start:end] 01810 _x = _v110 01811 start = end 01812 end += 8 01813 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01814 start = end 01815 end += 4 01816 (length,) = _struct_I.unpack(str[start:end]) 01817 start = end 01818 end += length 01819 _v110.distortion_model = str[start:end] 01820 start = end 01821 end += 4 01822 (length,) = _struct_I.unpack(str[start:end]) 01823 pattern = '<%sd'%length 01824 start = end 01825 end += struct.calcsize(pattern) 01826 _v110.D = struct.unpack(pattern, str[start:end]) 01827 start = end 01828 end += 72 01829 _v110.K = _struct_9d.unpack(str[start:end]) 01830 start = end 01831 end += 72 01832 _v110.R = _struct_9d.unpack(str[start:end]) 01833 start = end 01834 end += 96 01835 _v110.P = _struct_12d.unpack(str[start:end]) 01836 _x = _v110 01837 start = end 01838 end += 8 01839 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01840 _v113 = _v110.roi 01841 _x = _v113 01842 start = end 01843 end += 17 01844 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01845 _v113.do_rectify = bool(_v113.do_rectify) 01846 _v114 = _v100.roi_box_pose 01847 _v115 = _v114.header 01848 start = end 01849 end += 4 01850 (_v115.seq,) = _struct_I.unpack(str[start:end]) 01851 _v116 = _v115.stamp 01852 _x = _v116 01853 start = end 01854 end += 8 01855 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01856 start = end 01857 end += 4 01858 (length,) = _struct_I.unpack(str[start:end]) 01859 start = end 01860 end += length 01861 _v115.frame_id = str[start:end] 01862 _v117 = _v114.pose 01863 _v118 = _v117.position 01864 _x = _v118 01865 start = end 01866 end += 24 01867 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01868 _v119 = _v117.orientation 01869 _x = _v119 01870 start = end 01871 end += 32 01872 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01873 _v120 = _v100.roi_box_dims 01874 _x = _v120 01875 start = end 01876 end += 24 01877 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01878 start = end 01879 end += 4 01880 (length,) = _struct_I.unpack(str[start:end]) 01881 start = end 01882 end += length 01883 val2.collision_name = str[start:end] 01884 val1.moved_obstacles.append(val2) 01885 self.grasps_to_evaluate.append(val1) 01886 start = end 01887 end += 4 01888 (length,) = _struct_I.unpack(str[start:end]) 01889 self.movable_obstacles = [] 01890 for i in range(0, length): 01891 val1 = object_manipulation_msgs.msg.GraspableObject() 01892 start = end 01893 end += 4 01894 (length,) = _struct_I.unpack(str[start:end]) 01895 start = end 01896 end += length 01897 val1.reference_frame_id = str[start:end] 01898 start = end 01899 end += 4 01900 (length,) = _struct_I.unpack(str[start:end]) 01901 val1.potential_models = [] 01902 for i in range(0, length): 01903 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 01904 start = end 01905 end += 4 01906 (val2.model_id,) = _struct_i.unpack(str[start:end]) 01907 _v121 = val2.pose 01908 _v122 = _v121.header 01909 start = end 01910 end += 4 01911 (_v122.seq,) = _struct_I.unpack(str[start:end]) 01912 _v123 = _v122.stamp 01913 _x = _v123 01914 start = end 01915 end += 8 01916 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01917 start = end 01918 end += 4 01919 (length,) = _struct_I.unpack(str[start:end]) 01920 start = end 01921 end += length 01922 _v122.frame_id = str[start:end] 01923 _v124 = _v121.pose 01924 _v125 = _v124.position 01925 _x = _v125 01926 start = end 01927 end += 24 01928 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01929 _v126 = _v124.orientation 01930 _x = _v126 01931 start = end 01932 end += 32 01933 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01934 start = end 01935 end += 4 01936 (val2.confidence,) = _struct_f.unpack(str[start:end]) 01937 start = end 01938 end += 4 01939 (length,) = _struct_I.unpack(str[start:end]) 01940 start = end 01941 end += length 01942 val2.detector_name = str[start:end] 01943 val1.potential_models.append(val2) 01944 _v127 = val1.cluster 01945 _v128 = _v127.header 01946 start = end 01947 end += 4 01948 (_v128.seq,) = _struct_I.unpack(str[start:end]) 01949 _v129 = _v128.stamp 01950 _x = _v129 01951 start = end 01952 end += 8 01953 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01954 start = end 01955 end += 4 01956 (length,) = _struct_I.unpack(str[start:end]) 01957 start = end 01958 end += length 01959 _v128.frame_id = str[start:end] 01960 start = end 01961 end += 4 01962 (length,) = _struct_I.unpack(str[start:end]) 01963 _v127.points = [] 01964 for i in range(0, length): 01965 val3 = geometry_msgs.msg.Point32() 01966 _x = val3 01967 start = end 01968 end += 12 01969 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01970 _v127.points.append(val3) 01971 start = end 01972 end += 4 01973 (length,) = _struct_I.unpack(str[start:end]) 01974 _v127.channels = [] 01975 for i in range(0, length): 01976 val3 = sensor_msgs.msg.ChannelFloat32() 01977 start = end 01978 end += 4 01979 (length,) = _struct_I.unpack(str[start:end]) 01980 start = end 01981 end += length 01982 val3.name = str[start:end] 01983 start = end 01984 end += 4 01985 (length,) = _struct_I.unpack(str[start:end]) 01986 pattern = '<%sf'%length 01987 start = end 01988 end += struct.calcsize(pattern) 01989 val3.values = struct.unpack(pattern, str[start:end]) 01990 _v127.channels.append(val3) 01991 _v130 = val1.region 01992 _v131 = _v130.cloud 01993 _v132 = _v131.header 01994 start = end 01995 end += 4 01996 (_v132.seq,) = _struct_I.unpack(str[start:end]) 01997 _v133 = _v132.stamp 01998 _x = _v133 01999 start = end 02000 end += 8 02001 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02002 start = end 02003 end += 4 02004 (length,) = _struct_I.unpack(str[start:end]) 02005 start = end 02006 end += length 02007 _v132.frame_id = str[start:end] 02008 _x = _v131 02009 start = end 02010 end += 8 02011 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02012 start = end 02013 end += 4 02014 (length,) = _struct_I.unpack(str[start:end]) 02015 _v131.fields = [] 02016 for i in range(0, length): 02017 val4 = sensor_msgs.msg.PointField() 02018 start = end 02019 end += 4 02020 (length,) = _struct_I.unpack(str[start:end]) 02021 start = end 02022 end += length 02023 val4.name = str[start:end] 02024 _x = val4 02025 start = end 02026 end += 9 02027 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02028 _v131.fields.append(val4) 02029 _x = _v131 02030 start = end 02031 end += 9 02032 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 02033 _v131.is_bigendian = bool(_v131.is_bigendian) 02034 start = end 02035 end += 4 02036 (length,) = _struct_I.unpack(str[start:end]) 02037 start = end 02038 end += length 02039 _v131.data = str[start:end] 02040 start = end 02041 end += 1 02042 (_v131.is_dense,) = _struct_B.unpack(str[start:end]) 02043 _v131.is_dense = bool(_v131.is_dense) 02044 start = end 02045 end += 4 02046 (length,) = _struct_I.unpack(str[start:end]) 02047 pattern = '<%si'%length 02048 start = end 02049 end += struct.calcsize(pattern) 02050 _v130.mask = struct.unpack(pattern, str[start:end]) 02051 _v134 = _v130.image 02052 _v135 = _v134.header 02053 start = end 02054 end += 4 02055 (_v135.seq,) = _struct_I.unpack(str[start:end]) 02056 _v136 = _v135.stamp 02057 _x = _v136 02058 start = end 02059 end += 8 02060 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02061 start = end 02062 end += 4 02063 (length,) = _struct_I.unpack(str[start:end]) 02064 start = end 02065 end += length 02066 _v135.frame_id = str[start:end] 02067 _x = _v134 02068 start = end 02069 end += 8 02070 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02071 start = end 02072 end += 4 02073 (length,) = _struct_I.unpack(str[start:end]) 02074 start = end 02075 end += length 02076 _v134.encoding = str[start:end] 02077 _x = _v134 02078 start = end 02079 end += 5 02080 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02081 start = end 02082 end += 4 02083 (length,) = _struct_I.unpack(str[start:end]) 02084 start = end 02085 end += length 02086 _v134.data = str[start:end] 02087 _v137 = _v130.disparity_image 02088 _v138 = _v137.header 02089 start = end 02090 end += 4 02091 (_v138.seq,) = _struct_I.unpack(str[start:end]) 02092 _v139 = _v138.stamp 02093 _x = _v139 02094 start = end 02095 end += 8 02096 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02097 start = end 02098 end += 4 02099 (length,) = _struct_I.unpack(str[start:end]) 02100 start = end 02101 end += length 02102 _v138.frame_id = str[start:end] 02103 _x = _v137 02104 start = end 02105 end += 8 02106 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02107 start = end 02108 end += 4 02109 (length,) = _struct_I.unpack(str[start:end]) 02110 start = end 02111 end += length 02112 _v137.encoding = str[start:end] 02113 _x = _v137 02114 start = end 02115 end += 5 02116 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02117 start = end 02118 end += 4 02119 (length,) = _struct_I.unpack(str[start:end]) 02120 start = end 02121 end += length 02122 _v137.data = str[start:end] 02123 _v140 = _v130.cam_info 02124 _v141 = _v140.header 02125 start = end 02126 end += 4 02127 (_v141.seq,) = _struct_I.unpack(str[start:end]) 02128 _v142 = _v141.stamp 02129 _x = _v142 02130 start = end 02131 end += 8 02132 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02133 start = end 02134 end += 4 02135 (length,) = _struct_I.unpack(str[start:end]) 02136 start = end 02137 end += length 02138 _v141.frame_id = str[start:end] 02139 _x = _v140 02140 start = end 02141 end += 8 02142 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02143 start = end 02144 end += 4 02145 (length,) = _struct_I.unpack(str[start:end]) 02146 start = end 02147 end += length 02148 _v140.distortion_model = str[start:end] 02149 start = end 02150 end += 4 02151 (length,) = _struct_I.unpack(str[start:end]) 02152 pattern = '<%sd'%length 02153 start = end 02154 end += struct.calcsize(pattern) 02155 _v140.D = struct.unpack(pattern, str[start:end]) 02156 start = end 02157 end += 72 02158 _v140.K = _struct_9d.unpack(str[start:end]) 02159 start = end 02160 end += 72 02161 _v140.R = _struct_9d.unpack(str[start:end]) 02162 start = end 02163 end += 96 02164 _v140.P = _struct_12d.unpack(str[start:end]) 02165 _x = _v140 02166 start = end 02167 end += 8 02168 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02169 _v143 = _v140.roi 02170 _x = _v143 02171 start = end 02172 end += 17 02173 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02174 _v143.do_rectify = bool(_v143.do_rectify) 02175 _v144 = _v130.roi_box_pose 02176 _v145 = _v144.header 02177 start = end 02178 end += 4 02179 (_v145.seq,) = _struct_I.unpack(str[start:end]) 02180 _v146 = _v145.stamp 02181 _x = _v146 02182 start = end 02183 end += 8 02184 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02185 start = end 02186 end += 4 02187 (length,) = _struct_I.unpack(str[start:end]) 02188 start = end 02189 end += length 02190 _v145.frame_id = str[start:end] 02191 _v147 = _v144.pose 02192 _v148 = _v147.position 02193 _x = _v148 02194 start = end 02195 end += 24 02196 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02197 _v149 = _v147.orientation 02198 _x = _v149 02199 start = end 02200 end += 32 02201 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02202 _v150 = _v130.roi_box_dims 02203 _x = _v150 02204 start = end 02205 end += 24 02206 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02207 start = end 02208 end += 4 02209 (length,) = _struct_I.unpack(str[start:end]) 02210 start = end 02211 end += length 02212 val1.collision_name = str[start:end] 02213 self.movable_obstacles.append(val1) 02214 return self 02215 except struct.error as e: 02216 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02217 02218 02219 def serialize_numpy(self, buff, numpy): 02220 """ 02221 serialize message with numpy array types into buffer 02222 @param buff: buffer 02223 @type buff: StringIO 02224 @param numpy: numpy python module 02225 @type numpy module 02226 """ 02227 try: 02228 _x = self.arm_name 02229 length = len(_x) 02230 buff.write(struct.pack('<I%ss'%length, length, _x)) 02231 _x = self.target.reference_frame_id 02232 length = len(_x) 02233 buff.write(struct.pack('<I%ss'%length, length, _x)) 02234 length = len(self.target.potential_models) 02235 buff.write(_struct_I.pack(length)) 02236 for val1 in self.target.potential_models: 02237 buff.write(_struct_i.pack(val1.model_id)) 02238 _v151 = val1.pose 02239 _v152 = _v151.header 02240 buff.write(_struct_I.pack(_v152.seq)) 02241 _v153 = _v152.stamp 02242 _x = _v153 02243 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02244 _x = _v152.frame_id 02245 length = len(_x) 02246 buff.write(struct.pack('<I%ss'%length, length, _x)) 02247 _v154 = _v151.pose 02248 _v155 = _v154.position 02249 _x = _v155 02250 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02251 _v156 = _v154.orientation 02252 _x = _v156 02253 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02254 buff.write(_struct_f.pack(val1.confidence)) 02255 _x = val1.detector_name 02256 length = len(_x) 02257 buff.write(struct.pack('<I%ss'%length, length, _x)) 02258 _x = self 02259 buff.write(_struct_3I.pack(_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs)) 02260 _x = self.target.cluster.header.frame_id 02261 length = len(_x) 02262 buff.write(struct.pack('<I%ss'%length, length, _x)) 02263 length = len(self.target.cluster.points) 02264 buff.write(_struct_I.pack(length)) 02265 for val1 in self.target.cluster.points: 02266 _x = val1 02267 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02268 length = len(self.target.cluster.channels) 02269 buff.write(_struct_I.pack(length)) 02270 for val1 in self.target.cluster.channels: 02271 _x = val1.name 02272 length = len(_x) 02273 buff.write(struct.pack('<I%ss'%length, length, _x)) 02274 length = len(val1.values) 02275 buff.write(_struct_I.pack(length)) 02276 pattern = '<%sf'%length 02277 buff.write(val1.values.tostring()) 02278 _x = self 02279 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)) 02280 _x = self.target.region.cloud.header.frame_id 02281 length = len(_x) 02282 buff.write(struct.pack('<I%ss'%length, length, _x)) 02283 _x = self 02284 buff.write(_struct_2I.pack(_x.target.region.cloud.height, _x.target.region.cloud.width)) 02285 length = len(self.target.region.cloud.fields) 02286 buff.write(_struct_I.pack(length)) 02287 for val1 in self.target.region.cloud.fields: 02288 _x = val1.name 02289 length = len(_x) 02290 buff.write(struct.pack('<I%ss'%length, length, _x)) 02291 _x = val1 02292 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02293 _x = self 02294 buff.write(_struct_B2I.pack(_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step)) 02295 _x = self.target.region.cloud.data 02296 length = len(_x) 02297 # - if encoded as a list instead, serialize as bytes instead of string 02298 if type(_x) in [list, tuple]: 02299 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02300 else: 02301 buff.write(struct.pack('<I%ss'%length, length, _x)) 02302 buff.write(_struct_B.pack(self.target.region.cloud.is_dense)) 02303 length = len(self.target.region.mask) 02304 buff.write(_struct_I.pack(length)) 02305 pattern = '<%si'%length 02306 buff.write(self.target.region.mask.tostring()) 02307 _x = self 02308 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)) 02309 _x = self.target.region.image.header.frame_id 02310 length = len(_x) 02311 buff.write(struct.pack('<I%ss'%length, length, _x)) 02312 _x = self 02313 buff.write(_struct_2I.pack(_x.target.region.image.height, _x.target.region.image.width)) 02314 _x = self.target.region.image.encoding 02315 length = len(_x) 02316 buff.write(struct.pack('<I%ss'%length, length, _x)) 02317 _x = self 02318 buff.write(_struct_BI.pack(_x.target.region.image.is_bigendian, _x.target.region.image.step)) 02319 _x = self.target.region.image.data 02320 length = len(_x) 02321 # - if encoded as a list instead, serialize as bytes instead of string 02322 if type(_x) in [list, tuple]: 02323 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02324 else: 02325 buff.write(struct.pack('<I%ss'%length, length, _x)) 02326 _x = self 02327 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)) 02328 _x = self.target.region.disparity_image.header.frame_id 02329 length = len(_x) 02330 buff.write(struct.pack('<I%ss'%length, length, _x)) 02331 _x = self 02332 buff.write(_struct_2I.pack(_x.target.region.disparity_image.height, _x.target.region.disparity_image.width)) 02333 _x = self.target.region.disparity_image.encoding 02334 length = len(_x) 02335 buff.write(struct.pack('<I%ss'%length, length, _x)) 02336 _x = self 02337 buff.write(_struct_BI.pack(_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step)) 02338 _x = self.target.region.disparity_image.data 02339 length = len(_x) 02340 # - if encoded as a list instead, serialize as bytes instead of string 02341 if type(_x) in [list, tuple]: 02342 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02343 else: 02344 buff.write(struct.pack('<I%ss'%length, length, _x)) 02345 _x = self 02346 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)) 02347 _x = self.target.region.cam_info.header.frame_id 02348 length = len(_x) 02349 buff.write(struct.pack('<I%ss'%length, length, _x)) 02350 _x = self 02351 buff.write(_struct_2I.pack(_x.target.region.cam_info.height, _x.target.region.cam_info.width)) 02352 _x = self.target.region.cam_info.distortion_model 02353 length = len(_x) 02354 buff.write(struct.pack('<I%ss'%length, length, _x)) 02355 length = len(self.target.region.cam_info.D) 02356 buff.write(_struct_I.pack(length)) 02357 pattern = '<%sd'%length 02358 buff.write(self.target.region.cam_info.D.tostring()) 02359 buff.write(self.target.region.cam_info.K.tostring()) 02360 buff.write(self.target.region.cam_info.R.tostring()) 02361 buff.write(self.target.region.cam_info.P.tostring()) 02362 _x = self 02363 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)) 02364 _x = self.target.region.roi_box_pose.header.frame_id 02365 length = len(_x) 02366 buff.write(struct.pack('<I%ss'%length, length, _x)) 02367 _x = self 02368 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)) 02369 _x = self.target.collision_name 02370 length = len(_x) 02371 buff.write(struct.pack('<I%ss'%length, length, _x)) 02372 _x = self.collision_object_name 02373 length = len(_x) 02374 buff.write(struct.pack('<I%ss'%length, length, _x)) 02375 _x = self.collision_support_surface_name 02376 length = len(_x) 02377 buff.write(struct.pack('<I%ss'%length, length, _x)) 02378 length = len(self.grasps_to_evaluate) 02379 buff.write(_struct_I.pack(length)) 02380 for val1 in self.grasps_to_evaluate: 02381 _v157 = val1.pre_grasp_posture 02382 _v158 = _v157.header 02383 buff.write(_struct_I.pack(_v158.seq)) 02384 _v159 = _v158.stamp 02385 _x = _v159 02386 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02387 _x = _v158.frame_id 02388 length = len(_x) 02389 buff.write(struct.pack('<I%ss'%length, length, _x)) 02390 length = len(_v157.name) 02391 buff.write(_struct_I.pack(length)) 02392 for val3 in _v157.name: 02393 length = len(val3) 02394 buff.write(struct.pack('<I%ss'%length, length, val3)) 02395 length = len(_v157.position) 02396 buff.write(_struct_I.pack(length)) 02397 pattern = '<%sd'%length 02398 buff.write(_v157.position.tostring()) 02399 length = len(_v157.velocity) 02400 buff.write(_struct_I.pack(length)) 02401 pattern = '<%sd'%length 02402 buff.write(_v157.velocity.tostring()) 02403 length = len(_v157.effort) 02404 buff.write(_struct_I.pack(length)) 02405 pattern = '<%sd'%length 02406 buff.write(_v157.effort.tostring()) 02407 _v160 = val1.grasp_posture 02408 _v161 = _v160.header 02409 buff.write(_struct_I.pack(_v161.seq)) 02410 _v162 = _v161.stamp 02411 _x = _v162 02412 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02413 _x = _v161.frame_id 02414 length = len(_x) 02415 buff.write(struct.pack('<I%ss'%length, length, _x)) 02416 length = len(_v160.name) 02417 buff.write(_struct_I.pack(length)) 02418 for val3 in _v160.name: 02419 length = len(val3) 02420 buff.write(struct.pack('<I%ss'%length, length, val3)) 02421 length = len(_v160.position) 02422 buff.write(_struct_I.pack(length)) 02423 pattern = '<%sd'%length 02424 buff.write(_v160.position.tostring()) 02425 length = len(_v160.velocity) 02426 buff.write(_struct_I.pack(length)) 02427 pattern = '<%sd'%length 02428 buff.write(_v160.velocity.tostring()) 02429 length = len(_v160.effort) 02430 buff.write(_struct_I.pack(length)) 02431 pattern = '<%sd'%length 02432 buff.write(_v160.effort.tostring()) 02433 _v163 = val1.grasp_pose 02434 _v164 = _v163.position 02435 _x = _v164 02436 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02437 _v165 = _v163.orientation 02438 _x = _v165 02439 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02440 _x = val1 02441 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 02442 length = len(val1.moved_obstacles) 02443 buff.write(_struct_I.pack(length)) 02444 for val2 in val1.moved_obstacles: 02445 _x = val2.reference_frame_id 02446 length = len(_x) 02447 buff.write(struct.pack('<I%ss'%length, length, _x)) 02448 length = len(val2.potential_models) 02449 buff.write(_struct_I.pack(length)) 02450 for val3 in val2.potential_models: 02451 buff.write(_struct_i.pack(val3.model_id)) 02452 _v166 = val3.pose 02453 _v167 = _v166.header 02454 buff.write(_struct_I.pack(_v167.seq)) 02455 _v168 = _v167.stamp 02456 _x = _v168 02457 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02458 _x = _v167.frame_id 02459 length = len(_x) 02460 buff.write(struct.pack('<I%ss'%length, length, _x)) 02461 _v169 = _v166.pose 02462 _v170 = _v169.position 02463 _x = _v170 02464 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02465 _v171 = _v169.orientation 02466 _x = _v171 02467 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02468 buff.write(_struct_f.pack(val3.confidence)) 02469 _x = val3.detector_name 02470 length = len(_x) 02471 buff.write(struct.pack('<I%ss'%length, length, _x)) 02472 _v172 = val2.cluster 02473 _v173 = _v172.header 02474 buff.write(_struct_I.pack(_v173.seq)) 02475 _v174 = _v173.stamp 02476 _x = _v174 02477 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02478 _x = _v173.frame_id 02479 length = len(_x) 02480 buff.write(struct.pack('<I%ss'%length, length, _x)) 02481 length = len(_v172.points) 02482 buff.write(_struct_I.pack(length)) 02483 for val4 in _v172.points: 02484 _x = val4 02485 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02486 length = len(_v172.channels) 02487 buff.write(_struct_I.pack(length)) 02488 for val4 in _v172.channels: 02489 _x = val4.name 02490 length = len(_x) 02491 buff.write(struct.pack('<I%ss'%length, length, _x)) 02492 length = len(val4.values) 02493 buff.write(_struct_I.pack(length)) 02494 pattern = '<%sf'%length 02495 buff.write(val4.values.tostring()) 02496 _v175 = val2.region 02497 _v176 = _v175.cloud 02498 _v177 = _v176.header 02499 buff.write(_struct_I.pack(_v177.seq)) 02500 _v178 = _v177.stamp 02501 _x = _v178 02502 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02503 _x = _v177.frame_id 02504 length = len(_x) 02505 buff.write(struct.pack('<I%ss'%length, length, _x)) 02506 _x = _v176 02507 buff.write(_struct_2I.pack(_x.height, _x.width)) 02508 length = len(_v176.fields) 02509 buff.write(_struct_I.pack(length)) 02510 for val5 in _v176.fields: 02511 _x = val5.name 02512 length = len(_x) 02513 buff.write(struct.pack('<I%ss'%length, length, _x)) 02514 _x = val5 02515 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02516 _x = _v176 02517 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 02518 _x = _v176.data 02519 length = len(_x) 02520 # - if encoded as a list instead, serialize as bytes instead of string 02521 if type(_x) in [list, tuple]: 02522 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02523 else: 02524 buff.write(struct.pack('<I%ss'%length, length, _x)) 02525 buff.write(_struct_B.pack(_v176.is_dense)) 02526 length = len(_v175.mask) 02527 buff.write(_struct_I.pack(length)) 02528 pattern = '<%si'%length 02529 buff.write(_v175.mask.tostring()) 02530 _v179 = _v175.image 02531 _v180 = _v179.header 02532 buff.write(_struct_I.pack(_v180.seq)) 02533 _v181 = _v180.stamp 02534 _x = _v181 02535 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02536 _x = _v180.frame_id 02537 length = len(_x) 02538 buff.write(struct.pack('<I%ss'%length, length, _x)) 02539 _x = _v179 02540 buff.write(_struct_2I.pack(_x.height, _x.width)) 02541 _x = _v179.encoding 02542 length = len(_x) 02543 buff.write(struct.pack('<I%ss'%length, length, _x)) 02544 _x = _v179 02545 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02546 _x = _v179.data 02547 length = len(_x) 02548 # - if encoded as a list instead, serialize as bytes instead of string 02549 if type(_x) in [list, tuple]: 02550 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02551 else: 02552 buff.write(struct.pack('<I%ss'%length, length, _x)) 02553 _v182 = _v175.disparity_image 02554 _v183 = _v182.header 02555 buff.write(_struct_I.pack(_v183.seq)) 02556 _v184 = _v183.stamp 02557 _x = _v184 02558 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02559 _x = _v183.frame_id 02560 length = len(_x) 02561 buff.write(struct.pack('<I%ss'%length, length, _x)) 02562 _x = _v182 02563 buff.write(_struct_2I.pack(_x.height, _x.width)) 02564 _x = _v182.encoding 02565 length = len(_x) 02566 buff.write(struct.pack('<I%ss'%length, length, _x)) 02567 _x = _v182 02568 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02569 _x = _v182.data 02570 length = len(_x) 02571 # - if encoded as a list instead, serialize as bytes instead of string 02572 if type(_x) in [list, tuple]: 02573 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02574 else: 02575 buff.write(struct.pack('<I%ss'%length, length, _x)) 02576 _v185 = _v175.cam_info 02577 _v186 = _v185.header 02578 buff.write(_struct_I.pack(_v186.seq)) 02579 _v187 = _v186.stamp 02580 _x = _v187 02581 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02582 _x = _v186.frame_id 02583 length = len(_x) 02584 buff.write(struct.pack('<I%ss'%length, length, _x)) 02585 _x = _v185 02586 buff.write(_struct_2I.pack(_x.height, _x.width)) 02587 _x = _v185.distortion_model 02588 length = len(_x) 02589 buff.write(struct.pack('<I%ss'%length, length, _x)) 02590 length = len(_v185.D) 02591 buff.write(_struct_I.pack(length)) 02592 pattern = '<%sd'%length 02593 buff.write(_v185.D.tostring()) 02594 buff.write(_v185.K.tostring()) 02595 buff.write(_v185.R.tostring()) 02596 buff.write(_v185.P.tostring()) 02597 _x = _v185 02598 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02599 _v188 = _v185.roi 02600 _x = _v188 02601 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02602 _v189 = _v175.roi_box_pose 02603 _v190 = _v189.header 02604 buff.write(_struct_I.pack(_v190.seq)) 02605 _v191 = _v190.stamp 02606 _x = _v191 02607 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02608 _x = _v190.frame_id 02609 length = len(_x) 02610 buff.write(struct.pack('<I%ss'%length, length, _x)) 02611 _v192 = _v189.pose 02612 _v193 = _v192.position 02613 _x = _v193 02614 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02615 _v194 = _v192.orientation 02616 _x = _v194 02617 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02618 _v195 = _v175.roi_box_dims 02619 _x = _v195 02620 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02621 _x = val2.collision_name 02622 length = len(_x) 02623 buff.write(struct.pack('<I%ss'%length, length, _x)) 02624 length = len(self.movable_obstacles) 02625 buff.write(_struct_I.pack(length)) 02626 for val1 in self.movable_obstacles: 02627 _x = val1.reference_frame_id 02628 length = len(_x) 02629 buff.write(struct.pack('<I%ss'%length, length, _x)) 02630 length = len(val1.potential_models) 02631 buff.write(_struct_I.pack(length)) 02632 for val2 in val1.potential_models: 02633 buff.write(_struct_i.pack(val2.model_id)) 02634 _v196 = val2.pose 02635 _v197 = _v196.header 02636 buff.write(_struct_I.pack(_v197.seq)) 02637 _v198 = _v197.stamp 02638 _x = _v198 02639 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02640 _x = _v197.frame_id 02641 length = len(_x) 02642 buff.write(struct.pack('<I%ss'%length, length, _x)) 02643 _v199 = _v196.pose 02644 _v200 = _v199.position 02645 _x = _v200 02646 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02647 _v201 = _v199.orientation 02648 _x = _v201 02649 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02650 buff.write(_struct_f.pack(val2.confidence)) 02651 _x = val2.detector_name 02652 length = len(_x) 02653 buff.write(struct.pack('<I%ss'%length, length, _x)) 02654 _v202 = val1.cluster 02655 _v203 = _v202.header 02656 buff.write(_struct_I.pack(_v203.seq)) 02657 _v204 = _v203.stamp 02658 _x = _v204 02659 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02660 _x = _v203.frame_id 02661 length = len(_x) 02662 buff.write(struct.pack('<I%ss'%length, length, _x)) 02663 length = len(_v202.points) 02664 buff.write(_struct_I.pack(length)) 02665 for val3 in _v202.points: 02666 _x = val3 02667 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02668 length = len(_v202.channels) 02669 buff.write(_struct_I.pack(length)) 02670 for val3 in _v202.channels: 02671 _x = val3.name 02672 length = len(_x) 02673 buff.write(struct.pack('<I%ss'%length, length, _x)) 02674 length = len(val3.values) 02675 buff.write(_struct_I.pack(length)) 02676 pattern = '<%sf'%length 02677 buff.write(val3.values.tostring()) 02678 _v205 = val1.region 02679 _v206 = _v205.cloud 02680 _v207 = _v206.header 02681 buff.write(_struct_I.pack(_v207.seq)) 02682 _v208 = _v207.stamp 02683 _x = _v208 02684 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02685 _x = _v207.frame_id 02686 length = len(_x) 02687 buff.write(struct.pack('<I%ss'%length, length, _x)) 02688 _x = _v206 02689 buff.write(_struct_2I.pack(_x.height, _x.width)) 02690 length = len(_v206.fields) 02691 buff.write(_struct_I.pack(length)) 02692 for val4 in _v206.fields: 02693 _x = val4.name 02694 length = len(_x) 02695 buff.write(struct.pack('<I%ss'%length, length, _x)) 02696 _x = val4 02697 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02698 _x = _v206 02699 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 02700 _x = _v206.data 02701 length = len(_x) 02702 # - if encoded as a list instead, serialize as bytes instead of string 02703 if type(_x) in [list, tuple]: 02704 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02705 else: 02706 buff.write(struct.pack('<I%ss'%length, length, _x)) 02707 buff.write(_struct_B.pack(_v206.is_dense)) 02708 length = len(_v205.mask) 02709 buff.write(_struct_I.pack(length)) 02710 pattern = '<%si'%length 02711 buff.write(_v205.mask.tostring()) 02712 _v209 = _v205.image 02713 _v210 = _v209.header 02714 buff.write(_struct_I.pack(_v210.seq)) 02715 _v211 = _v210.stamp 02716 _x = _v211 02717 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02718 _x = _v210.frame_id 02719 length = len(_x) 02720 buff.write(struct.pack('<I%ss'%length, length, _x)) 02721 _x = _v209 02722 buff.write(_struct_2I.pack(_x.height, _x.width)) 02723 _x = _v209.encoding 02724 length = len(_x) 02725 buff.write(struct.pack('<I%ss'%length, length, _x)) 02726 _x = _v209 02727 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02728 _x = _v209.data 02729 length = len(_x) 02730 # - if encoded as a list instead, serialize as bytes instead of string 02731 if type(_x) in [list, tuple]: 02732 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02733 else: 02734 buff.write(struct.pack('<I%ss'%length, length, _x)) 02735 _v212 = _v205.disparity_image 02736 _v213 = _v212.header 02737 buff.write(_struct_I.pack(_v213.seq)) 02738 _v214 = _v213.stamp 02739 _x = _v214 02740 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02741 _x = _v213.frame_id 02742 length = len(_x) 02743 buff.write(struct.pack('<I%ss'%length, length, _x)) 02744 _x = _v212 02745 buff.write(_struct_2I.pack(_x.height, _x.width)) 02746 _x = _v212.encoding 02747 length = len(_x) 02748 buff.write(struct.pack('<I%ss'%length, length, _x)) 02749 _x = _v212 02750 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02751 _x = _v212.data 02752 length = len(_x) 02753 # - if encoded as a list instead, serialize as bytes instead of string 02754 if type(_x) in [list, tuple]: 02755 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02756 else: 02757 buff.write(struct.pack('<I%ss'%length, length, _x)) 02758 _v215 = _v205.cam_info 02759 _v216 = _v215.header 02760 buff.write(_struct_I.pack(_v216.seq)) 02761 _v217 = _v216.stamp 02762 _x = _v217 02763 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02764 _x = _v216.frame_id 02765 length = len(_x) 02766 buff.write(struct.pack('<I%ss'%length, length, _x)) 02767 _x = _v215 02768 buff.write(_struct_2I.pack(_x.height, _x.width)) 02769 _x = _v215.distortion_model 02770 length = len(_x) 02771 buff.write(struct.pack('<I%ss'%length, length, _x)) 02772 length = len(_v215.D) 02773 buff.write(_struct_I.pack(length)) 02774 pattern = '<%sd'%length 02775 buff.write(_v215.D.tostring()) 02776 buff.write(_v215.K.tostring()) 02777 buff.write(_v215.R.tostring()) 02778 buff.write(_v215.P.tostring()) 02779 _x = _v215 02780 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02781 _v218 = _v215.roi 02782 _x = _v218 02783 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02784 _v219 = _v205.roi_box_pose 02785 _v220 = _v219.header 02786 buff.write(_struct_I.pack(_v220.seq)) 02787 _v221 = _v220.stamp 02788 _x = _v221 02789 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02790 _x = _v220.frame_id 02791 length = len(_x) 02792 buff.write(struct.pack('<I%ss'%length, length, _x)) 02793 _v222 = _v219.pose 02794 _v223 = _v222.position 02795 _x = _v223 02796 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02797 _v224 = _v222.orientation 02798 _x = _v224 02799 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02800 _v225 = _v205.roi_box_dims 02801 _x = _v225 02802 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02803 _x = val1.collision_name 02804 length = len(_x) 02805 buff.write(struct.pack('<I%ss'%length, length, _x)) 02806 except struct.error as se: self._check_types(se) 02807 except TypeError as te: self._check_types(te) 02808 02809 def deserialize_numpy(self, str, numpy): 02810 """ 02811 unpack serialized message in str into this message instance using numpy for array types 02812 @param str: byte array of serialized message 02813 @type str: str 02814 @param numpy: numpy python module 02815 @type numpy: module 02816 """ 02817 try: 02818 if self.target is None: 02819 self.target = object_manipulation_msgs.msg.GraspableObject() 02820 end = 0 02821 start = end 02822 end += 4 02823 (length,) = _struct_I.unpack(str[start:end]) 02824 start = end 02825 end += length 02826 self.arm_name = str[start:end] 02827 start = end 02828 end += 4 02829 (length,) = _struct_I.unpack(str[start:end]) 02830 start = end 02831 end += length 02832 self.target.reference_frame_id = str[start:end] 02833 start = end 02834 end += 4 02835 (length,) = _struct_I.unpack(str[start:end]) 02836 self.target.potential_models = [] 02837 for i in range(0, length): 02838 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 02839 start = end 02840 end += 4 02841 (val1.model_id,) = _struct_i.unpack(str[start:end]) 02842 _v226 = val1.pose 02843 _v227 = _v226.header 02844 start = end 02845 end += 4 02846 (_v227.seq,) = _struct_I.unpack(str[start:end]) 02847 _v228 = _v227.stamp 02848 _x = _v228 02849 start = end 02850 end += 8 02851 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02852 start = end 02853 end += 4 02854 (length,) = _struct_I.unpack(str[start:end]) 02855 start = end 02856 end += length 02857 _v227.frame_id = str[start:end] 02858 _v229 = _v226.pose 02859 _v230 = _v229.position 02860 _x = _v230 02861 start = end 02862 end += 24 02863 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02864 _v231 = _v229.orientation 02865 _x = _v231 02866 start = end 02867 end += 32 02868 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02869 start = end 02870 end += 4 02871 (val1.confidence,) = _struct_f.unpack(str[start:end]) 02872 start = end 02873 end += 4 02874 (length,) = _struct_I.unpack(str[start:end]) 02875 start = end 02876 end += length 02877 val1.detector_name = str[start:end] 02878 self.target.potential_models.append(val1) 02879 _x = self 02880 start = end 02881 end += 12 02882 (_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02883 start = end 02884 end += 4 02885 (length,) = _struct_I.unpack(str[start:end]) 02886 start = end 02887 end += length 02888 self.target.cluster.header.frame_id = str[start:end] 02889 start = end 02890 end += 4 02891 (length,) = _struct_I.unpack(str[start:end]) 02892 self.target.cluster.points = [] 02893 for i in range(0, length): 02894 val1 = geometry_msgs.msg.Point32() 02895 _x = val1 02896 start = end 02897 end += 12 02898 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02899 self.target.cluster.points.append(val1) 02900 start = end 02901 end += 4 02902 (length,) = _struct_I.unpack(str[start:end]) 02903 self.target.cluster.channels = [] 02904 for i in range(0, length): 02905 val1 = sensor_msgs.msg.ChannelFloat32() 02906 start = end 02907 end += 4 02908 (length,) = _struct_I.unpack(str[start:end]) 02909 start = end 02910 end += length 02911 val1.name = str[start:end] 02912 start = end 02913 end += 4 02914 (length,) = _struct_I.unpack(str[start:end]) 02915 pattern = '<%sf'%length 02916 start = end 02917 end += struct.calcsize(pattern) 02918 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 02919 self.target.cluster.channels.append(val1) 02920 _x = self 02921 start = end 02922 end += 12 02923 (_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]) 02924 start = end 02925 end += 4 02926 (length,) = _struct_I.unpack(str[start:end]) 02927 start = end 02928 end += length 02929 self.target.region.cloud.header.frame_id = str[start:end] 02930 _x = self 02931 start = end 02932 end += 8 02933 (_x.target.region.cloud.height, _x.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 02934 start = end 02935 end += 4 02936 (length,) = _struct_I.unpack(str[start:end]) 02937 self.target.region.cloud.fields = [] 02938 for i in range(0, length): 02939 val1 = sensor_msgs.msg.PointField() 02940 start = end 02941 end += 4 02942 (length,) = _struct_I.unpack(str[start:end]) 02943 start = end 02944 end += length 02945 val1.name = str[start:end] 02946 _x = val1 02947 start = end 02948 end += 9 02949 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02950 self.target.region.cloud.fields.append(val1) 02951 _x = self 02952 start = end 02953 end += 9 02954 (_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 02955 self.target.region.cloud.is_bigendian = bool(self.target.region.cloud.is_bigendian) 02956 start = end 02957 end += 4 02958 (length,) = _struct_I.unpack(str[start:end]) 02959 start = end 02960 end += length 02961 self.target.region.cloud.data = str[start:end] 02962 start = end 02963 end += 1 02964 (self.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 02965 self.target.region.cloud.is_dense = bool(self.target.region.cloud.is_dense) 02966 start = end 02967 end += 4 02968 (length,) = _struct_I.unpack(str[start:end]) 02969 pattern = '<%si'%length 02970 start = end 02971 end += struct.calcsize(pattern) 02972 self.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 02973 _x = self 02974 start = end 02975 end += 12 02976 (_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]) 02977 start = end 02978 end += 4 02979 (length,) = _struct_I.unpack(str[start:end]) 02980 start = end 02981 end += length 02982 self.target.region.image.header.frame_id = str[start:end] 02983 _x = self 02984 start = end 02985 end += 8 02986 (_x.target.region.image.height, _x.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 02987 start = end 02988 end += 4 02989 (length,) = _struct_I.unpack(str[start:end]) 02990 start = end 02991 end += length 02992 self.target.region.image.encoding = str[start:end] 02993 _x = self 02994 start = end 02995 end += 5 02996 (_x.target.region.image.is_bigendian, _x.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 02997 start = end 02998 end += 4 02999 (length,) = _struct_I.unpack(str[start:end]) 03000 start = end 03001 end += length 03002 self.target.region.image.data = str[start:end] 03003 _x = self 03004 start = end 03005 end += 12 03006 (_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]) 03007 start = end 03008 end += 4 03009 (length,) = _struct_I.unpack(str[start:end]) 03010 start = end 03011 end += length 03012 self.target.region.disparity_image.header.frame_id = str[start:end] 03013 _x = self 03014 start = end 03015 end += 8 03016 (_x.target.region.disparity_image.height, _x.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 03017 start = end 03018 end += 4 03019 (length,) = _struct_I.unpack(str[start:end]) 03020 start = end 03021 end += length 03022 self.target.region.disparity_image.encoding = str[start:end] 03023 _x = self 03024 start = end 03025 end += 5 03026 (_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 03027 start = end 03028 end += 4 03029 (length,) = _struct_I.unpack(str[start:end]) 03030 start = end 03031 end += length 03032 self.target.region.disparity_image.data = str[start:end] 03033 _x = self 03034 start = end 03035 end += 12 03036 (_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]) 03037 start = end 03038 end += 4 03039 (length,) = _struct_I.unpack(str[start:end]) 03040 start = end 03041 end += length 03042 self.target.region.cam_info.header.frame_id = str[start:end] 03043 _x = self 03044 start = end 03045 end += 8 03046 (_x.target.region.cam_info.height, _x.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 03047 start = end 03048 end += 4 03049 (length,) = _struct_I.unpack(str[start:end]) 03050 start = end 03051 end += length 03052 self.target.region.cam_info.distortion_model = str[start:end] 03053 start = end 03054 end += 4 03055 (length,) = _struct_I.unpack(str[start:end]) 03056 pattern = '<%sd'%length 03057 start = end 03058 end += struct.calcsize(pattern) 03059 self.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03060 start = end 03061 end += 72 03062 self.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03063 start = end 03064 end += 72 03065 self.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03066 start = end 03067 end += 96 03068 self.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 03069 _x = self 03070 start = end 03071 end += 37 03072 (_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]) 03073 self.target.region.cam_info.roi.do_rectify = bool(self.target.region.cam_info.roi.do_rectify) 03074 start = end 03075 end += 4 03076 (length,) = _struct_I.unpack(str[start:end]) 03077 start = end 03078 end += length 03079 self.target.region.roi_box_pose.header.frame_id = str[start:end] 03080 _x = self 03081 start = end 03082 end += 80 03083 (_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]) 03084 start = end 03085 end += 4 03086 (length,) = _struct_I.unpack(str[start:end]) 03087 start = end 03088 end += length 03089 self.target.collision_name = str[start:end] 03090 start = end 03091 end += 4 03092 (length,) = _struct_I.unpack(str[start:end]) 03093 start = end 03094 end += length 03095 self.collision_object_name = str[start:end] 03096 start = end 03097 end += 4 03098 (length,) = _struct_I.unpack(str[start:end]) 03099 start = end 03100 end += length 03101 self.collision_support_surface_name = str[start:end] 03102 start = end 03103 end += 4 03104 (length,) = _struct_I.unpack(str[start:end]) 03105 self.grasps_to_evaluate = [] 03106 for i in range(0, length): 03107 val1 = object_manipulation_msgs.msg.Grasp() 03108 _v232 = val1.pre_grasp_posture 03109 _v233 = _v232.header 03110 start = end 03111 end += 4 03112 (_v233.seq,) = _struct_I.unpack(str[start:end]) 03113 _v234 = _v233.stamp 03114 _x = _v234 03115 start = end 03116 end += 8 03117 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03118 start = end 03119 end += 4 03120 (length,) = _struct_I.unpack(str[start:end]) 03121 start = end 03122 end += length 03123 _v233.frame_id = str[start:end] 03124 start = end 03125 end += 4 03126 (length,) = _struct_I.unpack(str[start:end]) 03127 _v232.name = [] 03128 for i in range(0, length): 03129 start = end 03130 end += 4 03131 (length,) = _struct_I.unpack(str[start:end]) 03132 start = end 03133 end += length 03134 val3 = str[start:end] 03135 _v232.name.append(val3) 03136 start = end 03137 end += 4 03138 (length,) = _struct_I.unpack(str[start:end]) 03139 pattern = '<%sd'%length 03140 start = end 03141 end += struct.calcsize(pattern) 03142 _v232.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03143 start = end 03144 end += 4 03145 (length,) = _struct_I.unpack(str[start:end]) 03146 pattern = '<%sd'%length 03147 start = end 03148 end += struct.calcsize(pattern) 03149 _v232.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03150 start = end 03151 end += 4 03152 (length,) = _struct_I.unpack(str[start:end]) 03153 pattern = '<%sd'%length 03154 start = end 03155 end += struct.calcsize(pattern) 03156 _v232.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03157 _v235 = val1.grasp_posture 03158 _v236 = _v235.header 03159 start = end 03160 end += 4 03161 (_v236.seq,) = _struct_I.unpack(str[start:end]) 03162 _v237 = _v236.stamp 03163 _x = _v237 03164 start = end 03165 end += 8 03166 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03167 start = end 03168 end += 4 03169 (length,) = _struct_I.unpack(str[start:end]) 03170 start = end 03171 end += length 03172 _v236.frame_id = str[start:end] 03173 start = end 03174 end += 4 03175 (length,) = _struct_I.unpack(str[start:end]) 03176 _v235.name = [] 03177 for i in range(0, length): 03178 start = end 03179 end += 4 03180 (length,) = _struct_I.unpack(str[start:end]) 03181 start = end 03182 end += length 03183 val3 = str[start:end] 03184 _v235.name.append(val3) 03185 start = end 03186 end += 4 03187 (length,) = _struct_I.unpack(str[start:end]) 03188 pattern = '<%sd'%length 03189 start = end 03190 end += struct.calcsize(pattern) 03191 _v235.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03192 start = end 03193 end += 4 03194 (length,) = _struct_I.unpack(str[start:end]) 03195 pattern = '<%sd'%length 03196 start = end 03197 end += struct.calcsize(pattern) 03198 _v235.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03199 start = end 03200 end += 4 03201 (length,) = _struct_I.unpack(str[start:end]) 03202 pattern = '<%sd'%length 03203 start = end 03204 end += struct.calcsize(pattern) 03205 _v235.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03206 _v238 = val1.grasp_pose 03207 _v239 = _v238.position 03208 _x = _v239 03209 start = end 03210 end += 24 03211 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03212 _v240 = _v238.orientation 03213 _x = _v240 03214 start = end 03215 end += 32 03216 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03217 _x = val1 03218 start = end 03219 end += 17 03220 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 03221 val1.cluster_rep = bool(val1.cluster_rep) 03222 start = end 03223 end += 4 03224 (length,) = _struct_I.unpack(str[start:end]) 03225 val1.moved_obstacles = [] 03226 for i in range(0, length): 03227 val2 = object_manipulation_msgs.msg.GraspableObject() 03228 start = end 03229 end += 4 03230 (length,) = _struct_I.unpack(str[start:end]) 03231 start = end 03232 end += length 03233 val2.reference_frame_id = str[start:end] 03234 start = end 03235 end += 4 03236 (length,) = _struct_I.unpack(str[start:end]) 03237 val2.potential_models = [] 03238 for i in range(0, length): 03239 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 03240 start = end 03241 end += 4 03242 (val3.model_id,) = _struct_i.unpack(str[start:end]) 03243 _v241 = val3.pose 03244 _v242 = _v241.header 03245 start = end 03246 end += 4 03247 (_v242.seq,) = _struct_I.unpack(str[start:end]) 03248 _v243 = _v242.stamp 03249 _x = _v243 03250 start = end 03251 end += 8 03252 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03253 start = end 03254 end += 4 03255 (length,) = _struct_I.unpack(str[start:end]) 03256 start = end 03257 end += length 03258 _v242.frame_id = str[start:end] 03259 _v244 = _v241.pose 03260 _v245 = _v244.position 03261 _x = _v245 03262 start = end 03263 end += 24 03264 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03265 _v246 = _v244.orientation 03266 _x = _v246 03267 start = end 03268 end += 32 03269 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03270 start = end 03271 end += 4 03272 (val3.confidence,) = _struct_f.unpack(str[start:end]) 03273 start = end 03274 end += 4 03275 (length,) = _struct_I.unpack(str[start:end]) 03276 start = end 03277 end += length 03278 val3.detector_name = str[start:end] 03279 val2.potential_models.append(val3) 03280 _v247 = val2.cluster 03281 _v248 = _v247.header 03282 start = end 03283 end += 4 03284 (_v248.seq,) = _struct_I.unpack(str[start:end]) 03285 _v249 = _v248.stamp 03286 _x = _v249 03287 start = end 03288 end += 8 03289 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03290 start = end 03291 end += 4 03292 (length,) = _struct_I.unpack(str[start:end]) 03293 start = end 03294 end += length 03295 _v248.frame_id = str[start:end] 03296 start = end 03297 end += 4 03298 (length,) = _struct_I.unpack(str[start:end]) 03299 _v247.points = [] 03300 for i in range(0, length): 03301 val4 = geometry_msgs.msg.Point32() 03302 _x = val4 03303 start = end 03304 end += 12 03305 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03306 _v247.points.append(val4) 03307 start = end 03308 end += 4 03309 (length,) = _struct_I.unpack(str[start:end]) 03310 _v247.channels = [] 03311 for i in range(0, length): 03312 val4 = sensor_msgs.msg.ChannelFloat32() 03313 start = end 03314 end += 4 03315 (length,) = _struct_I.unpack(str[start:end]) 03316 start = end 03317 end += length 03318 val4.name = str[start:end] 03319 start = end 03320 end += 4 03321 (length,) = _struct_I.unpack(str[start:end]) 03322 pattern = '<%sf'%length 03323 start = end 03324 end += struct.calcsize(pattern) 03325 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 03326 _v247.channels.append(val4) 03327 _v250 = val2.region 03328 _v251 = _v250.cloud 03329 _v252 = _v251.header 03330 start = end 03331 end += 4 03332 (_v252.seq,) = _struct_I.unpack(str[start:end]) 03333 _v253 = _v252.stamp 03334 _x = _v253 03335 start = end 03336 end += 8 03337 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03338 start = end 03339 end += 4 03340 (length,) = _struct_I.unpack(str[start:end]) 03341 start = end 03342 end += length 03343 _v252.frame_id = str[start:end] 03344 _x = _v251 03345 start = end 03346 end += 8 03347 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03348 start = end 03349 end += 4 03350 (length,) = _struct_I.unpack(str[start:end]) 03351 _v251.fields = [] 03352 for i in range(0, length): 03353 val5 = sensor_msgs.msg.PointField() 03354 start = end 03355 end += 4 03356 (length,) = _struct_I.unpack(str[start:end]) 03357 start = end 03358 end += length 03359 val5.name = str[start:end] 03360 _x = val5 03361 start = end 03362 end += 9 03363 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03364 _v251.fields.append(val5) 03365 _x = _v251 03366 start = end 03367 end += 9 03368 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 03369 _v251.is_bigendian = bool(_v251.is_bigendian) 03370 start = end 03371 end += 4 03372 (length,) = _struct_I.unpack(str[start:end]) 03373 start = end 03374 end += length 03375 _v251.data = str[start:end] 03376 start = end 03377 end += 1 03378 (_v251.is_dense,) = _struct_B.unpack(str[start:end]) 03379 _v251.is_dense = bool(_v251.is_dense) 03380 start = end 03381 end += 4 03382 (length,) = _struct_I.unpack(str[start:end]) 03383 pattern = '<%si'%length 03384 start = end 03385 end += struct.calcsize(pattern) 03386 _v250.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03387 _v254 = _v250.image 03388 _v255 = _v254.header 03389 start = end 03390 end += 4 03391 (_v255.seq,) = _struct_I.unpack(str[start:end]) 03392 _v256 = _v255.stamp 03393 _x = _v256 03394 start = end 03395 end += 8 03396 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03397 start = end 03398 end += 4 03399 (length,) = _struct_I.unpack(str[start:end]) 03400 start = end 03401 end += length 03402 _v255.frame_id = str[start:end] 03403 _x = _v254 03404 start = end 03405 end += 8 03406 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03407 start = end 03408 end += 4 03409 (length,) = _struct_I.unpack(str[start:end]) 03410 start = end 03411 end += length 03412 _v254.encoding = str[start:end] 03413 _x = _v254 03414 start = end 03415 end += 5 03416 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03417 start = end 03418 end += 4 03419 (length,) = _struct_I.unpack(str[start:end]) 03420 start = end 03421 end += length 03422 _v254.data = str[start:end] 03423 _v257 = _v250.disparity_image 03424 _v258 = _v257.header 03425 start = end 03426 end += 4 03427 (_v258.seq,) = _struct_I.unpack(str[start:end]) 03428 _v259 = _v258.stamp 03429 _x = _v259 03430 start = end 03431 end += 8 03432 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03433 start = end 03434 end += 4 03435 (length,) = _struct_I.unpack(str[start:end]) 03436 start = end 03437 end += length 03438 _v258.frame_id = str[start:end] 03439 _x = _v257 03440 start = end 03441 end += 8 03442 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03443 start = end 03444 end += 4 03445 (length,) = _struct_I.unpack(str[start:end]) 03446 start = end 03447 end += length 03448 _v257.encoding = str[start:end] 03449 _x = _v257 03450 start = end 03451 end += 5 03452 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03453 start = end 03454 end += 4 03455 (length,) = _struct_I.unpack(str[start:end]) 03456 start = end 03457 end += length 03458 _v257.data = str[start:end] 03459 _v260 = _v250.cam_info 03460 _v261 = _v260.header 03461 start = end 03462 end += 4 03463 (_v261.seq,) = _struct_I.unpack(str[start:end]) 03464 _v262 = _v261.stamp 03465 _x = _v262 03466 start = end 03467 end += 8 03468 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03469 start = end 03470 end += 4 03471 (length,) = _struct_I.unpack(str[start:end]) 03472 start = end 03473 end += length 03474 _v261.frame_id = str[start:end] 03475 _x = _v260 03476 start = end 03477 end += 8 03478 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03479 start = end 03480 end += 4 03481 (length,) = _struct_I.unpack(str[start:end]) 03482 start = end 03483 end += length 03484 _v260.distortion_model = str[start:end] 03485 start = end 03486 end += 4 03487 (length,) = _struct_I.unpack(str[start:end]) 03488 pattern = '<%sd'%length 03489 start = end 03490 end += struct.calcsize(pattern) 03491 _v260.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03492 start = end 03493 end += 72 03494 _v260.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03495 start = end 03496 end += 72 03497 _v260.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03498 start = end 03499 end += 96 03500 _v260.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 03501 _x = _v260 03502 start = end 03503 end += 8 03504 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 03505 _v263 = _v260.roi 03506 _x = _v263 03507 start = end 03508 end += 17 03509 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 03510 _v263.do_rectify = bool(_v263.do_rectify) 03511 _v264 = _v250.roi_box_pose 03512 _v265 = _v264.header 03513 start = end 03514 end += 4 03515 (_v265.seq,) = _struct_I.unpack(str[start:end]) 03516 _v266 = _v265.stamp 03517 _x = _v266 03518 start = end 03519 end += 8 03520 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03521 start = end 03522 end += 4 03523 (length,) = _struct_I.unpack(str[start:end]) 03524 start = end 03525 end += length 03526 _v265.frame_id = str[start:end] 03527 _v267 = _v264.pose 03528 _v268 = _v267.position 03529 _x = _v268 03530 start = end 03531 end += 24 03532 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03533 _v269 = _v267.orientation 03534 _x = _v269 03535 start = end 03536 end += 32 03537 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03538 _v270 = _v250.roi_box_dims 03539 _x = _v270 03540 start = end 03541 end += 24 03542 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03543 start = end 03544 end += 4 03545 (length,) = _struct_I.unpack(str[start:end]) 03546 start = end 03547 end += length 03548 val2.collision_name = str[start:end] 03549 val1.moved_obstacles.append(val2) 03550 self.grasps_to_evaluate.append(val1) 03551 start = end 03552 end += 4 03553 (length,) = _struct_I.unpack(str[start:end]) 03554 self.movable_obstacles = [] 03555 for i in range(0, length): 03556 val1 = object_manipulation_msgs.msg.GraspableObject() 03557 start = end 03558 end += 4 03559 (length,) = _struct_I.unpack(str[start:end]) 03560 start = end 03561 end += length 03562 val1.reference_frame_id = str[start:end] 03563 start = end 03564 end += 4 03565 (length,) = _struct_I.unpack(str[start:end]) 03566 val1.potential_models = [] 03567 for i in range(0, length): 03568 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 03569 start = end 03570 end += 4 03571 (val2.model_id,) = _struct_i.unpack(str[start:end]) 03572 _v271 = val2.pose 03573 _v272 = _v271.header 03574 start = end 03575 end += 4 03576 (_v272.seq,) = _struct_I.unpack(str[start:end]) 03577 _v273 = _v272.stamp 03578 _x = _v273 03579 start = end 03580 end += 8 03581 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03582 start = end 03583 end += 4 03584 (length,) = _struct_I.unpack(str[start:end]) 03585 start = end 03586 end += length 03587 _v272.frame_id = str[start:end] 03588 _v274 = _v271.pose 03589 _v275 = _v274.position 03590 _x = _v275 03591 start = end 03592 end += 24 03593 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03594 _v276 = _v274.orientation 03595 _x = _v276 03596 start = end 03597 end += 32 03598 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03599 start = end 03600 end += 4 03601 (val2.confidence,) = _struct_f.unpack(str[start:end]) 03602 start = end 03603 end += 4 03604 (length,) = _struct_I.unpack(str[start:end]) 03605 start = end 03606 end += length 03607 val2.detector_name = str[start:end] 03608 val1.potential_models.append(val2) 03609 _v277 = val1.cluster 03610 _v278 = _v277.header 03611 start = end 03612 end += 4 03613 (_v278.seq,) = _struct_I.unpack(str[start:end]) 03614 _v279 = _v278.stamp 03615 _x = _v279 03616 start = end 03617 end += 8 03618 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03619 start = end 03620 end += 4 03621 (length,) = _struct_I.unpack(str[start:end]) 03622 start = end 03623 end += length 03624 _v278.frame_id = str[start:end] 03625 start = end 03626 end += 4 03627 (length,) = _struct_I.unpack(str[start:end]) 03628 _v277.points = [] 03629 for i in range(0, length): 03630 val3 = geometry_msgs.msg.Point32() 03631 _x = val3 03632 start = end 03633 end += 12 03634 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03635 _v277.points.append(val3) 03636 start = end 03637 end += 4 03638 (length,) = _struct_I.unpack(str[start:end]) 03639 _v277.channels = [] 03640 for i in range(0, length): 03641 val3 = sensor_msgs.msg.ChannelFloat32() 03642 start = end 03643 end += 4 03644 (length,) = _struct_I.unpack(str[start:end]) 03645 start = end 03646 end += length 03647 val3.name = str[start:end] 03648 start = end 03649 end += 4 03650 (length,) = _struct_I.unpack(str[start:end]) 03651 pattern = '<%sf'%length 03652 start = end 03653 end += struct.calcsize(pattern) 03654 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 03655 _v277.channels.append(val3) 03656 _v280 = val1.region 03657 _v281 = _v280.cloud 03658 _v282 = _v281.header 03659 start = end 03660 end += 4 03661 (_v282.seq,) = _struct_I.unpack(str[start:end]) 03662 _v283 = _v282.stamp 03663 _x = _v283 03664 start = end 03665 end += 8 03666 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03667 start = end 03668 end += 4 03669 (length,) = _struct_I.unpack(str[start:end]) 03670 start = end 03671 end += length 03672 _v282.frame_id = str[start:end] 03673 _x = _v281 03674 start = end 03675 end += 8 03676 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03677 start = end 03678 end += 4 03679 (length,) = _struct_I.unpack(str[start:end]) 03680 _v281.fields = [] 03681 for i in range(0, length): 03682 val4 = sensor_msgs.msg.PointField() 03683 start = end 03684 end += 4 03685 (length,) = _struct_I.unpack(str[start:end]) 03686 start = end 03687 end += length 03688 val4.name = str[start:end] 03689 _x = val4 03690 start = end 03691 end += 9 03692 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03693 _v281.fields.append(val4) 03694 _x = _v281 03695 start = end 03696 end += 9 03697 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 03698 _v281.is_bigendian = bool(_v281.is_bigendian) 03699 start = end 03700 end += 4 03701 (length,) = _struct_I.unpack(str[start:end]) 03702 start = end 03703 end += length 03704 _v281.data = str[start:end] 03705 start = end 03706 end += 1 03707 (_v281.is_dense,) = _struct_B.unpack(str[start:end]) 03708 _v281.is_dense = bool(_v281.is_dense) 03709 start = end 03710 end += 4 03711 (length,) = _struct_I.unpack(str[start:end]) 03712 pattern = '<%si'%length 03713 start = end 03714 end += struct.calcsize(pattern) 03715 _v280.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03716 _v284 = _v280.image 03717 _v285 = _v284.header 03718 start = end 03719 end += 4 03720 (_v285.seq,) = _struct_I.unpack(str[start:end]) 03721 _v286 = _v285.stamp 03722 _x = _v286 03723 start = end 03724 end += 8 03725 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03726 start = end 03727 end += 4 03728 (length,) = _struct_I.unpack(str[start:end]) 03729 start = end 03730 end += length 03731 _v285.frame_id = str[start:end] 03732 _x = _v284 03733 start = end 03734 end += 8 03735 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03736 start = end 03737 end += 4 03738 (length,) = _struct_I.unpack(str[start:end]) 03739 start = end 03740 end += length 03741 _v284.encoding = str[start:end] 03742 _x = _v284 03743 start = end 03744 end += 5 03745 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03746 start = end 03747 end += 4 03748 (length,) = _struct_I.unpack(str[start:end]) 03749 start = end 03750 end += length 03751 _v284.data = str[start:end] 03752 _v287 = _v280.disparity_image 03753 _v288 = _v287.header 03754 start = end 03755 end += 4 03756 (_v288.seq,) = _struct_I.unpack(str[start:end]) 03757 _v289 = _v288.stamp 03758 _x = _v289 03759 start = end 03760 end += 8 03761 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03762 start = end 03763 end += 4 03764 (length,) = _struct_I.unpack(str[start:end]) 03765 start = end 03766 end += length 03767 _v288.frame_id = str[start:end] 03768 _x = _v287 03769 start = end 03770 end += 8 03771 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03772 start = end 03773 end += 4 03774 (length,) = _struct_I.unpack(str[start:end]) 03775 start = end 03776 end += length 03777 _v287.encoding = str[start:end] 03778 _x = _v287 03779 start = end 03780 end += 5 03781 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03782 start = end 03783 end += 4 03784 (length,) = _struct_I.unpack(str[start:end]) 03785 start = end 03786 end += length 03787 _v287.data = str[start:end] 03788 _v290 = _v280.cam_info 03789 _v291 = _v290.header 03790 start = end 03791 end += 4 03792 (_v291.seq,) = _struct_I.unpack(str[start:end]) 03793 _v292 = _v291.stamp 03794 _x = _v292 03795 start = end 03796 end += 8 03797 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03798 start = end 03799 end += 4 03800 (length,) = _struct_I.unpack(str[start:end]) 03801 start = end 03802 end += length 03803 _v291.frame_id = str[start:end] 03804 _x = _v290 03805 start = end 03806 end += 8 03807 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03808 start = end 03809 end += 4 03810 (length,) = _struct_I.unpack(str[start:end]) 03811 start = end 03812 end += length 03813 _v290.distortion_model = str[start:end] 03814 start = end 03815 end += 4 03816 (length,) = _struct_I.unpack(str[start:end]) 03817 pattern = '<%sd'%length 03818 start = end 03819 end += struct.calcsize(pattern) 03820 _v290.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03821 start = end 03822 end += 72 03823 _v290.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03824 start = end 03825 end += 72 03826 _v290.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03827 start = end 03828 end += 96 03829 _v290.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 03830 _x = _v290 03831 start = end 03832 end += 8 03833 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 03834 _v293 = _v290.roi 03835 _x = _v293 03836 start = end 03837 end += 17 03838 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 03839 _v293.do_rectify = bool(_v293.do_rectify) 03840 _v294 = _v280.roi_box_pose 03841 _v295 = _v294.header 03842 start = end 03843 end += 4 03844 (_v295.seq,) = _struct_I.unpack(str[start:end]) 03845 _v296 = _v295.stamp 03846 _x = _v296 03847 start = end 03848 end += 8 03849 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03850 start = end 03851 end += 4 03852 (length,) = _struct_I.unpack(str[start:end]) 03853 start = end 03854 end += length 03855 _v295.frame_id = str[start:end] 03856 _v297 = _v294.pose 03857 _v298 = _v297.position 03858 _x = _v298 03859 start = end 03860 end += 24 03861 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03862 _v299 = _v297.orientation 03863 _x = _v299 03864 start = end 03865 end += 32 03866 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03867 _v300 = _v280.roi_box_dims 03868 _x = _v300 03869 start = end 03870 end += 24 03871 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03872 start = end 03873 end += 4 03874 (length,) = _struct_I.unpack(str[start:end]) 03875 start = end 03876 end += length 03877 val1.collision_name = str[start:end] 03878 self.movable_obstacles.append(val1) 03879 return self 03880 except struct.error as e: 03881 raise roslib.message.DeserializationError(e) #most likely buffer underfill 03882 03883 _struct_I = roslib.message.struct_I 03884 _struct_IBI = struct.Struct("<IBI") 03885 _struct_6IB3I = struct.Struct("<6IB3I") 03886 _struct_B = struct.Struct("<B") 03887 _struct_12d = struct.Struct("<12d") 03888 _struct_f = struct.Struct("<f") 03889 _struct_i = struct.Struct("<i") 03890 _struct_dB2f = struct.Struct("<dB2f") 03891 _struct_BI = struct.Struct("<BI") 03892 _struct_10d = struct.Struct("<10d") 03893 _struct_3f = struct.Struct("<3f") 03894 _struct_3I = struct.Struct("<3I") 03895 _struct_9d = struct.Struct("<9d") 03896 _struct_B2I = struct.Struct("<B2I") 03897 _struct_4d = struct.Struct("<4d") 03898 _struct_2I = struct.Struct("<2I") 03899 _struct_4IB = struct.Struct("<4IB") 03900 _struct_3d = struct.Struct("<3d") 03901 """autogenerated by genmsg_py from GraspPlanningResponse.msg. Do not edit.""" 03902 import roslib.message 03903 import struct 03904 03905 import geometry_msgs.msg 03906 import std_msgs.msg 03907 import object_manipulation_msgs.msg 03908 import household_objects_database_msgs.msg 03909 import sensor_msgs.msg 03910 03911 class GraspPlanningResponse(roslib.message.Message): 03912 _md5sum = "998e3d06c509abfc46d7fdf96fe1c188" 03913 _type = "object_manipulation_msgs/GraspPlanningResponse" 03914 _has_header = False #flag to mark the presence of a Header object 03915 _full_text = """ 03916 03917 Grasp[] grasps 03918 03919 03920 GraspPlanningErrorCode error_code 03921 03922 03923 ================================================================================ 03924 MSG: object_manipulation_msgs/Grasp 03925 03926 # The internal posture of the hand for the pre-grasp 03927 # only positions are used 03928 sensor_msgs/JointState pre_grasp_posture 03929 03930 # The internal posture of the hand for the grasp 03931 # positions and efforts are used 03932 sensor_msgs/JointState grasp_posture 03933 03934 # The position of the end-effector for the grasp relative to a reference frame 03935 # (that is always specified elsewhere, not in this message) 03936 geometry_msgs/Pose grasp_pose 03937 03938 # The estimated probability of success for this grasp 03939 float64 success_probability 03940 03941 # Debug flag to indicate that this grasp would be the best in its cluster 03942 bool cluster_rep 03943 03944 # how far the pre-grasp should ideally be away from the grasp 03945 float32 desired_approach_distance 03946 03947 # how much distance between pre-grasp and grasp must actually be feasible 03948 # for the grasp not to be rejected 03949 float32 min_approach_distance 03950 03951 # an optional list of obstacles that we have semantic information about 03952 # and that we expect might move in the course of executing this grasp 03953 # the grasp planner is expected to make sure they move in an OK way; during 03954 # execution, grasp executors will not check for collisions against these objects 03955 GraspableObject[] moved_obstacles 03956 03957 ================================================================================ 03958 MSG: sensor_msgs/JointState 03959 # This is a message that holds data to describe the state of a set of torque controlled joints. 03960 # 03961 # The state of each joint (revolute or prismatic) is defined by: 03962 # * the position of the joint (rad or m), 03963 # * the velocity of the joint (rad/s or m/s) and 03964 # * the effort that is applied in the joint (Nm or N). 03965 # 03966 # Each joint is uniquely identified by its name 03967 # The header specifies the time at which the joint states were recorded. All the joint states 03968 # in one message have to be recorded at the same time. 03969 # 03970 # This message consists of a multiple arrays, one for each part of the joint state. 03971 # The goal is to make each of the fields optional. When e.g. your joints have no 03972 # effort associated with them, you can leave the effort array empty. 03973 # 03974 # All arrays in this message should have the same size, or be empty. 03975 # This is the only way to uniquely associate the joint name with the correct 03976 # states. 03977 03978 03979 Header header 03980 03981 string[] name 03982 float64[] position 03983 float64[] velocity 03984 float64[] effort 03985 03986 ================================================================================ 03987 MSG: std_msgs/Header 03988 # Standard metadata for higher-level stamped data types. 03989 # This is generally used to communicate timestamped data 03990 # in a particular coordinate frame. 03991 # 03992 # sequence ID: consecutively increasing ID 03993 uint32 seq 03994 #Two-integer timestamp that is expressed as: 03995 # * stamp.secs: seconds (stamp_secs) since epoch 03996 # * stamp.nsecs: nanoseconds since stamp_secs 03997 # time-handling sugar is provided by the client library 03998 time stamp 03999 #Frame this data is associated with 04000 # 0: no frame 04001 # 1: global frame 04002 string frame_id 04003 04004 ================================================================================ 04005 MSG: geometry_msgs/Pose 04006 # A representation of pose in free space, composed of postion and orientation. 04007 Point position 04008 Quaternion orientation 04009 04010 ================================================================================ 04011 MSG: geometry_msgs/Point 04012 # This contains the position of a point in free space 04013 float64 x 04014 float64 y 04015 float64 z 04016 04017 ================================================================================ 04018 MSG: geometry_msgs/Quaternion 04019 # This represents an orientation in free space in quaternion form. 04020 04021 float64 x 04022 float64 y 04023 float64 z 04024 float64 w 04025 04026 ================================================================================ 04027 MSG: object_manipulation_msgs/GraspableObject 04028 # an object that the object_manipulator can work on 04029 04030 # a graspable object can be represented in multiple ways. This message 04031 # can contain all of them. Which one is actually used is up to the receiver 04032 # of this message. When adding new representations, one must be careful that 04033 # they have reasonable lightweight defaults indicating that that particular 04034 # representation is not available. 04035 04036 # the tf frame to be used as a reference frame when combining information from 04037 # the different representations below 04038 string reference_frame_id 04039 04040 # potential recognition results from a database of models 04041 # all poses are relative to the object reference pose 04042 household_objects_database_msgs/DatabaseModelPose[] potential_models 04043 04044 # the point cloud itself 04045 sensor_msgs/PointCloud cluster 04046 04047 # a region of a PointCloud2 of interest 04048 object_manipulation_msgs/SceneRegion region 04049 04050 # the name that this object has in the collision environment 04051 string collision_name 04052 ================================================================================ 04053 MSG: household_objects_database_msgs/DatabaseModelPose 04054 # Informs that a specific model from the Model Database has been 04055 # identified at a certain location 04056 04057 # the database id of the model 04058 int32 model_id 04059 04060 # the pose that it can be found in 04061 geometry_msgs/PoseStamped pose 04062 04063 # a measure of the confidence level in this detection result 04064 float32 confidence 04065 04066 # the name of the object detector that generated this detection result 04067 string detector_name 04068 04069 ================================================================================ 04070 MSG: geometry_msgs/PoseStamped 04071 # A Pose with reference coordinate frame and timestamp 04072 Header header 04073 Pose pose 04074 04075 ================================================================================ 04076 MSG: sensor_msgs/PointCloud 04077 # This message holds a collection of 3d points, plus optional additional 04078 # information about each point. 04079 04080 # Time of sensor data acquisition, coordinate frame ID. 04081 Header header 04082 04083 # Array of 3d points. Each Point32 should be interpreted as a 3d point 04084 # in the frame given in the header. 04085 geometry_msgs/Point32[] points 04086 04087 # Each channel should have the same number of elements as points array, 04088 # and the data in each channel should correspond 1:1 with each point. 04089 # Channel names in common practice are listed in ChannelFloat32.msg. 04090 ChannelFloat32[] channels 04091 04092 ================================================================================ 04093 MSG: geometry_msgs/Point32 04094 # This contains the position of a point in free space(with 32 bits of precision). 04095 # It is recommeded to use Point wherever possible instead of Point32. 04096 # 04097 # This recommendation is to promote interoperability. 04098 # 04099 # This message is designed to take up less space when sending 04100 # lots of points at once, as in the case of a PointCloud. 04101 04102 float32 x 04103 float32 y 04104 float32 z 04105 ================================================================================ 04106 MSG: sensor_msgs/ChannelFloat32 04107 # This message is used by the PointCloud message to hold optional data 04108 # associated with each point in the cloud. The length of the values 04109 # array should be the same as the length of the points array in the 04110 # PointCloud, and each value should be associated with the corresponding 04111 # point. 04112 04113 # Channel names in existing practice include: 04114 # "u", "v" - row and column (respectively) in the left stereo image. 04115 # This is opposite to usual conventions but remains for 04116 # historical reasons. The newer PointCloud2 message has no 04117 # such problem. 04118 # "rgb" - For point clouds produced by color stereo cameras. uint8 04119 # (R,G,B) values packed into the least significant 24 bits, 04120 # in order. 04121 # "intensity" - laser or pixel intensity. 04122 # "distance" 04123 04124 # The channel name should give semantics of the channel (e.g. 04125 # "intensity" instead of "value"). 04126 string name 04127 04128 # The values array should be 1-1 with the elements of the associated 04129 # PointCloud. 04130 float32[] values 04131 04132 ================================================================================ 04133 MSG: object_manipulation_msgs/SceneRegion 04134 # Point cloud 04135 sensor_msgs/PointCloud2 cloud 04136 04137 # Indices for the region of interest 04138 int32[] mask 04139 04140 # One of the corresponding 2D images, if applicable 04141 sensor_msgs/Image image 04142 04143 # The disparity image, if applicable 04144 sensor_msgs/Image disparity_image 04145 04146 # Camera info for the camera that took the image 04147 sensor_msgs/CameraInfo cam_info 04148 04149 # a 3D region of interest for grasp planning 04150 geometry_msgs/PoseStamped roi_box_pose 04151 geometry_msgs/Vector3 roi_box_dims 04152 04153 ================================================================================ 04154 MSG: sensor_msgs/PointCloud2 04155 # This message holds a collection of N-dimensional points, which may 04156 # contain additional information such as normals, intensity, etc. The 04157 # point data is stored as a binary blob, its layout described by the 04158 # contents of the "fields" array. 04159 04160 # The point cloud data may be organized 2d (image-like) or 1d 04161 # (unordered). Point clouds organized as 2d images may be produced by 04162 # camera depth sensors such as stereo or time-of-flight. 04163 04164 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 04165 # points). 04166 Header header 04167 04168 # 2D structure of the point cloud. If the cloud is unordered, height is 04169 # 1 and width is the length of the point cloud. 04170 uint32 height 04171 uint32 width 04172 04173 # Describes the channels and their layout in the binary data blob. 04174 PointField[] fields 04175 04176 bool is_bigendian # Is this data bigendian? 04177 uint32 point_step # Length of a point in bytes 04178 uint32 row_step # Length of a row in bytes 04179 uint8[] data # Actual point data, size is (row_step*height) 04180 04181 bool is_dense # True if there are no invalid points 04182 04183 ================================================================================ 04184 MSG: sensor_msgs/PointField 04185 # This message holds the description of one point entry in the 04186 # PointCloud2 message format. 04187 uint8 INT8 = 1 04188 uint8 UINT8 = 2 04189 uint8 INT16 = 3 04190 uint8 UINT16 = 4 04191 uint8 INT32 = 5 04192 uint8 UINT32 = 6 04193 uint8 FLOAT32 = 7 04194 uint8 FLOAT64 = 8 04195 04196 string name # Name of field 04197 uint32 offset # Offset from start of point struct 04198 uint8 datatype # Datatype enumeration, see above 04199 uint32 count # How many elements in the field 04200 04201 ================================================================================ 04202 MSG: sensor_msgs/Image 04203 # This message contains an uncompressed image 04204 # (0, 0) is at top-left corner of image 04205 # 04206 04207 Header header # Header timestamp should be acquisition time of image 04208 # Header frame_id should be optical frame of camera 04209 # origin of frame should be optical center of cameara 04210 # +x should point to the right in the image 04211 # +y should point down in the image 04212 # +z should point into to plane of the image 04213 # If the frame_id here and the frame_id of the CameraInfo 04214 # message associated with the image conflict 04215 # the behavior is undefined 04216 04217 uint32 height # image height, that is, number of rows 04218 uint32 width # image width, that is, number of columns 04219 04220 # The legal values for encoding are in file src/image_encodings.cpp 04221 # If you want to standardize a new string format, join 04222 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 04223 04224 string encoding # Encoding of pixels -- channel meaning, ordering, size 04225 # taken from the list of strings in src/image_encodings.cpp 04226 04227 uint8 is_bigendian # is this data bigendian? 04228 uint32 step # Full row length in bytes 04229 uint8[] data # actual matrix data, size is (step * rows) 04230 04231 ================================================================================ 04232 MSG: sensor_msgs/CameraInfo 04233 # This message defines meta information for a camera. It should be in a 04234 # camera namespace on topic "camera_info" and accompanied by up to five 04235 # image topics named: 04236 # 04237 # image_raw - raw data from the camera driver, possibly Bayer encoded 04238 # image - monochrome, distorted 04239 # image_color - color, distorted 04240 # image_rect - monochrome, rectified 04241 # image_rect_color - color, rectified 04242 # 04243 # The image_pipeline contains packages (image_proc, stereo_image_proc) 04244 # for producing the four processed image topics from image_raw and 04245 # camera_info. The meaning of the camera parameters are described in 04246 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 04247 # 04248 # The image_geometry package provides a user-friendly interface to 04249 # common operations using this meta information. If you want to, e.g., 04250 # project a 3d point into image coordinates, we strongly recommend 04251 # using image_geometry. 04252 # 04253 # If the camera is uncalibrated, the matrices D, K, R, P should be left 04254 # zeroed out. In particular, clients may assume that K[0] == 0.0 04255 # indicates an uncalibrated camera. 04256 04257 ####################################################################### 04258 # Image acquisition info # 04259 ####################################################################### 04260 04261 # Time of image acquisition, camera coordinate frame ID 04262 Header header # Header timestamp should be acquisition time of image 04263 # Header frame_id should be optical frame of camera 04264 # origin of frame should be optical center of camera 04265 # +x should point to the right in the image 04266 # +y should point down in the image 04267 # +z should point into the plane of the image 04268 04269 04270 ####################################################################### 04271 # Calibration Parameters # 04272 ####################################################################### 04273 # These are fixed during camera calibration. Their values will be the # 04274 # same in all messages until the camera is recalibrated. Note that # 04275 # self-calibrating systems may "recalibrate" frequently. # 04276 # # 04277 # The internal parameters can be used to warp a raw (distorted) image # 04278 # to: # 04279 # 1. An undistorted image (requires D and K) # 04280 # 2. A rectified image (requires D, K, R) # 04281 # The projection matrix P projects 3D points into the rectified image.# 04282 ####################################################################### 04283 04284 # The image dimensions with which the camera was calibrated. Normally 04285 # this will be the full camera resolution in pixels. 04286 uint32 height 04287 uint32 width 04288 04289 # The distortion model used. Supported models are listed in 04290 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 04291 # simple model of radial and tangential distortion - is sufficent. 04292 string distortion_model 04293 04294 # The distortion parameters, size depending on the distortion model. 04295 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 04296 float64[] D 04297 04298 # Intrinsic camera matrix for the raw (distorted) images. 04299 # [fx 0 cx] 04300 # K = [ 0 fy cy] 04301 # [ 0 0 1] 04302 # Projects 3D points in the camera coordinate frame to 2D pixel 04303 # coordinates using the focal lengths (fx, fy) and principal point 04304 # (cx, cy). 04305 float64[9] K # 3x3 row-major matrix 04306 04307 # Rectification matrix (stereo cameras only) 04308 # A rotation matrix aligning the camera coordinate system to the ideal 04309 # stereo image plane so that epipolar lines in both stereo images are 04310 # parallel. 04311 float64[9] R # 3x3 row-major matrix 04312 04313 # Projection/camera matrix 04314 # [fx' 0 cx' Tx] 04315 # P = [ 0 fy' cy' Ty] 04316 # [ 0 0 1 0] 04317 # By convention, this matrix specifies the intrinsic (camera) matrix 04318 # of the processed (rectified) image. That is, the left 3x3 portion 04319 # is the normal camera intrinsic matrix for the rectified image. 04320 # It projects 3D points in the camera coordinate frame to 2D pixel 04321 # coordinates using the focal lengths (fx', fy') and principal point 04322 # (cx', cy') - these may differ from the values in K. 04323 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 04324 # also have R = the identity and P[1:3,1:3] = K. 04325 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 04326 # position of the optical center of the second camera in the first 04327 # camera's frame. We assume Tz = 0 so both cameras are in the same 04328 # stereo image plane. The first camera always has Tx = Ty = 0. For 04329 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 04330 # Tx = -fx' * B, where B is the baseline between the cameras. 04331 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 04332 # the rectified image is given by: 04333 # [u v w]' = P * [X Y Z 1]' 04334 # x = u / w 04335 # y = v / w 04336 # This holds for both images of a stereo pair. 04337 float64[12] P # 3x4 row-major matrix 04338 04339 04340 ####################################################################### 04341 # Operational Parameters # 04342 ####################################################################### 04343 # These define the image region actually captured by the camera # 04344 # driver. Although they affect the geometry of the output image, they # 04345 # may be changed freely without recalibrating the camera. # 04346 ####################################################################### 04347 04348 # Binning refers here to any camera setting which combines rectangular 04349 # neighborhoods of pixels into larger "super-pixels." It reduces the 04350 # resolution of the output image to 04351 # (width / binning_x) x (height / binning_y). 04352 # The default values binning_x = binning_y = 0 is considered the same 04353 # as binning_x = binning_y = 1 (no subsampling). 04354 uint32 binning_x 04355 uint32 binning_y 04356 04357 # Region of interest (subwindow of full camera resolution), given in 04358 # full resolution (unbinned) image coordinates. A particular ROI 04359 # always denotes the same window of pixels on the camera sensor, 04360 # regardless of binning settings. 04361 # The default setting of roi (all values 0) is considered the same as 04362 # full resolution (roi.width = width, roi.height = height). 04363 RegionOfInterest roi 04364 04365 ================================================================================ 04366 MSG: sensor_msgs/RegionOfInterest 04367 # This message is used to specify a region of interest within an image. 04368 # 04369 # When used to specify the ROI setting of the camera when the image was 04370 # taken, the height and width fields should either match the height and 04371 # width fields for the associated image; or height = width = 0 04372 # indicates that the full resolution image was captured. 04373 04374 uint32 x_offset # Leftmost pixel of the ROI 04375 # (0 if the ROI includes the left edge of the image) 04376 uint32 y_offset # Topmost pixel of the ROI 04377 # (0 if the ROI includes the top edge of the image) 04378 uint32 height # Height of ROI 04379 uint32 width # Width of ROI 04380 04381 # True if a distinct rectified ROI should be calculated from the "raw" 04382 # ROI in this message. Typically this should be False if the full image 04383 # is captured (ROI not used), and True if a subwindow is captured (ROI 04384 # used). 04385 bool do_rectify 04386 04387 ================================================================================ 04388 MSG: geometry_msgs/Vector3 04389 # This represents a vector in free space. 04390 04391 float64 x 04392 float64 y 04393 float64 z 04394 ================================================================================ 04395 MSG: object_manipulation_msgs/GraspPlanningErrorCode 04396 # Error codes for grasp and place planning 04397 04398 # plan completed as expected 04399 int32 SUCCESS = 0 04400 04401 # tf error encountered while transforming 04402 int32 TF_ERROR = 1 04403 04404 # some other error 04405 int32 OTHER_ERROR = 2 04406 04407 # the actual value of this error code 04408 int32 value 04409 """ 04410 __slots__ = ['grasps','error_code'] 04411 _slot_types = ['object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspPlanningErrorCode'] 04412 04413 def __init__(self, *args, **kwds): 04414 """ 04415 Constructor. Any message fields that are implicitly/explicitly 04416 set to None will be assigned a default value. The recommend 04417 use is keyword arguments as this is more robust to future message 04418 changes. You cannot mix in-order arguments and keyword arguments. 04419 04420 The available fields are: 04421 grasps,error_code 04422 04423 @param args: complete set of field values, in .msg order 04424 @param kwds: use keyword arguments corresponding to message field names 04425 to set specific fields. 04426 """ 04427 if args or kwds: 04428 super(GraspPlanningResponse, self).__init__(*args, **kwds) 04429 #message fields cannot be None, assign default values for those that are 04430 if self.grasps is None: 04431 self.grasps = [] 04432 if self.error_code is None: 04433 self.error_code = object_manipulation_msgs.msg.GraspPlanningErrorCode() 04434 else: 04435 self.grasps = [] 04436 self.error_code = object_manipulation_msgs.msg.GraspPlanningErrorCode() 04437 04438 def _get_types(self): 04439 """ 04440 internal API method 04441 """ 04442 return self._slot_types 04443 04444 def serialize(self, buff): 04445 """ 04446 serialize message into buffer 04447 @param buff: buffer 04448 @type buff: StringIO 04449 """ 04450 try: 04451 length = len(self.grasps) 04452 buff.write(_struct_I.pack(length)) 04453 for val1 in self.grasps: 04454 _v301 = val1.pre_grasp_posture 04455 _v302 = _v301.header 04456 buff.write(_struct_I.pack(_v302.seq)) 04457 _v303 = _v302.stamp 04458 _x = _v303 04459 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04460 _x = _v302.frame_id 04461 length = len(_x) 04462 buff.write(struct.pack('<I%ss'%length, length, _x)) 04463 length = len(_v301.name) 04464 buff.write(_struct_I.pack(length)) 04465 for val3 in _v301.name: 04466 length = len(val3) 04467 buff.write(struct.pack('<I%ss'%length, length, val3)) 04468 length = len(_v301.position) 04469 buff.write(_struct_I.pack(length)) 04470 pattern = '<%sd'%length 04471 buff.write(struct.pack(pattern, *_v301.position)) 04472 length = len(_v301.velocity) 04473 buff.write(_struct_I.pack(length)) 04474 pattern = '<%sd'%length 04475 buff.write(struct.pack(pattern, *_v301.velocity)) 04476 length = len(_v301.effort) 04477 buff.write(_struct_I.pack(length)) 04478 pattern = '<%sd'%length 04479 buff.write(struct.pack(pattern, *_v301.effort)) 04480 _v304 = val1.grasp_posture 04481 _v305 = _v304.header 04482 buff.write(_struct_I.pack(_v305.seq)) 04483 _v306 = _v305.stamp 04484 _x = _v306 04485 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04486 _x = _v305.frame_id 04487 length = len(_x) 04488 buff.write(struct.pack('<I%ss'%length, length, _x)) 04489 length = len(_v304.name) 04490 buff.write(_struct_I.pack(length)) 04491 for val3 in _v304.name: 04492 length = len(val3) 04493 buff.write(struct.pack('<I%ss'%length, length, val3)) 04494 length = len(_v304.position) 04495 buff.write(_struct_I.pack(length)) 04496 pattern = '<%sd'%length 04497 buff.write(struct.pack(pattern, *_v304.position)) 04498 length = len(_v304.velocity) 04499 buff.write(_struct_I.pack(length)) 04500 pattern = '<%sd'%length 04501 buff.write(struct.pack(pattern, *_v304.velocity)) 04502 length = len(_v304.effort) 04503 buff.write(_struct_I.pack(length)) 04504 pattern = '<%sd'%length 04505 buff.write(struct.pack(pattern, *_v304.effort)) 04506 _v307 = val1.grasp_pose 04507 _v308 = _v307.position 04508 _x = _v308 04509 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04510 _v309 = _v307.orientation 04511 _x = _v309 04512 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04513 _x = val1 04514 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 04515 length = len(val1.moved_obstacles) 04516 buff.write(_struct_I.pack(length)) 04517 for val2 in val1.moved_obstacles: 04518 _x = val2.reference_frame_id 04519 length = len(_x) 04520 buff.write(struct.pack('<I%ss'%length, length, _x)) 04521 length = len(val2.potential_models) 04522 buff.write(_struct_I.pack(length)) 04523 for val3 in val2.potential_models: 04524 buff.write(_struct_i.pack(val3.model_id)) 04525 _v310 = val3.pose 04526 _v311 = _v310.header 04527 buff.write(_struct_I.pack(_v311.seq)) 04528 _v312 = _v311.stamp 04529 _x = _v312 04530 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04531 _x = _v311.frame_id 04532 length = len(_x) 04533 buff.write(struct.pack('<I%ss'%length, length, _x)) 04534 _v313 = _v310.pose 04535 _v314 = _v313.position 04536 _x = _v314 04537 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04538 _v315 = _v313.orientation 04539 _x = _v315 04540 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04541 buff.write(_struct_f.pack(val3.confidence)) 04542 _x = val3.detector_name 04543 length = len(_x) 04544 buff.write(struct.pack('<I%ss'%length, length, _x)) 04545 _v316 = val2.cluster 04546 _v317 = _v316.header 04547 buff.write(_struct_I.pack(_v317.seq)) 04548 _v318 = _v317.stamp 04549 _x = _v318 04550 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04551 _x = _v317.frame_id 04552 length = len(_x) 04553 buff.write(struct.pack('<I%ss'%length, length, _x)) 04554 length = len(_v316.points) 04555 buff.write(_struct_I.pack(length)) 04556 for val4 in _v316.points: 04557 _x = val4 04558 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 04559 length = len(_v316.channels) 04560 buff.write(_struct_I.pack(length)) 04561 for val4 in _v316.channels: 04562 _x = val4.name 04563 length = len(_x) 04564 buff.write(struct.pack('<I%ss'%length, length, _x)) 04565 length = len(val4.values) 04566 buff.write(_struct_I.pack(length)) 04567 pattern = '<%sf'%length 04568 buff.write(struct.pack(pattern, *val4.values)) 04569 _v319 = val2.region 04570 _v320 = _v319.cloud 04571 _v321 = _v320.header 04572 buff.write(_struct_I.pack(_v321.seq)) 04573 _v322 = _v321.stamp 04574 _x = _v322 04575 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04576 _x = _v321.frame_id 04577 length = len(_x) 04578 buff.write(struct.pack('<I%ss'%length, length, _x)) 04579 _x = _v320 04580 buff.write(_struct_2I.pack(_x.height, _x.width)) 04581 length = len(_v320.fields) 04582 buff.write(_struct_I.pack(length)) 04583 for val5 in _v320.fields: 04584 _x = val5.name 04585 length = len(_x) 04586 buff.write(struct.pack('<I%ss'%length, length, _x)) 04587 _x = val5 04588 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 04589 _x = _v320 04590 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 04591 _x = _v320.data 04592 length = len(_x) 04593 # - if encoded as a list instead, serialize as bytes instead of string 04594 if type(_x) in [list, tuple]: 04595 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04596 else: 04597 buff.write(struct.pack('<I%ss'%length, length, _x)) 04598 buff.write(_struct_B.pack(_v320.is_dense)) 04599 length = len(_v319.mask) 04600 buff.write(_struct_I.pack(length)) 04601 pattern = '<%si'%length 04602 buff.write(struct.pack(pattern, *_v319.mask)) 04603 _v323 = _v319.image 04604 _v324 = _v323.header 04605 buff.write(_struct_I.pack(_v324.seq)) 04606 _v325 = _v324.stamp 04607 _x = _v325 04608 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04609 _x = _v324.frame_id 04610 length = len(_x) 04611 buff.write(struct.pack('<I%ss'%length, length, _x)) 04612 _x = _v323 04613 buff.write(_struct_2I.pack(_x.height, _x.width)) 04614 _x = _v323.encoding 04615 length = len(_x) 04616 buff.write(struct.pack('<I%ss'%length, length, _x)) 04617 _x = _v323 04618 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 04619 _x = _v323.data 04620 length = len(_x) 04621 # - if encoded as a list instead, serialize as bytes instead of string 04622 if type(_x) in [list, tuple]: 04623 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04624 else: 04625 buff.write(struct.pack('<I%ss'%length, length, _x)) 04626 _v326 = _v319.disparity_image 04627 _v327 = _v326.header 04628 buff.write(_struct_I.pack(_v327.seq)) 04629 _v328 = _v327.stamp 04630 _x = _v328 04631 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04632 _x = _v327.frame_id 04633 length = len(_x) 04634 buff.write(struct.pack('<I%ss'%length, length, _x)) 04635 _x = _v326 04636 buff.write(_struct_2I.pack(_x.height, _x.width)) 04637 _x = _v326.encoding 04638 length = len(_x) 04639 buff.write(struct.pack('<I%ss'%length, length, _x)) 04640 _x = _v326 04641 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 04642 _x = _v326.data 04643 length = len(_x) 04644 # - if encoded as a list instead, serialize as bytes instead of string 04645 if type(_x) in [list, tuple]: 04646 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04647 else: 04648 buff.write(struct.pack('<I%ss'%length, length, _x)) 04649 _v329 = _v319.cam_info 04650 _v330 = _v329.header 04651 buff.write(_struct_I.pack(_v330.seq)) 04652 _v331 = _v330.stamp 04653 _x = _v331 04654 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04655 _x = _v330.frame_id 04656 length = len(_x) 04657 buff.write(struct.pack('<I%ss'%length, length, _x)) 04658 _x = _v329 04659 buff.write(_struct_2I.pack(_x.height, _x.width)) 04660 _x = _v329.distortion_model 04661 length = len(_x) 04662 buff.write(struct.pack('<I%ss'%length, length, _x)) 04663 length = len(_v329.D) 04664 buff.write(_struct_I.pack(length)) 04665 pattern = '<%sd'%length 04666 buff.write(struct.pack(pattern, *_v329.D)) 04667 buff.write(_struct_9d.pack(*_v329.K)) 04668 buff.write(_struct_9d.pack(*_v329.R)) 04669 buff.write(_struct_12d.pack(*_v329.P)) 04670 _x = _v329 04671 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 04672 _v332 = _v329.roi 04673 _x = _v332 04674 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 04675 _v333 = _v319.roi_box_pose 04676 _v334 = _v333.header 04677 buff.write(_struct_I.pack(_v334.seq)) 04678 _v335 = _v334.stamp 04679 _x = _v335 04680 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04681 _x = _v334.frame_id 04682 length = len(_x) 04683 buff.write(struct.pack('<I%ss'%length, length, _x)) 04684 _v336 = _v333.pose 04685 _v337 = _v336.position 04686 _x = _v337 04687 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04688 _v338 = _v336.orientation 04689 _x = _v338 04690 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04691 _v339 = _v319.roi_box_dims 04692 _x = _v339 04693 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04694 _x = val2.collision_name 04695 length = len(_x) 04696 buff.write(struct.pack('<I%ss'%length, length, _x)) 04697 buff.write(_struct_i.pack(self.error_code.value)) 04698 except struct.error as se: self._check_types(se) 04699 except TypeError as te: self._check_types(te) 04700 04701 def deserialize(self, str): 04702 """ 04703 unpack serialized message in str into this message instance 04704 @param str: byte array of serialized message 04705 @type str: str 04706 """ 04707 try: 04708 if self.error_code is None: 04709 self.error_code = object_manipulation_msgs.msg.GraspPlanningErrorCode() 04710 end = 0 04711 start = end 04712 end += 4 04713 (length,) = _struct_I.unpack(str[start:end]) 04714 self.grasps = [] 04715 for i in range(0, length): 04716 val1 = object_manipulation_msgs.msg.Grasp() 04717 _v340 = val1.pre_grasp_posture 04718 _v341 = _v340.header 04719 start = end 04720 end += 4 04721 (_v341.seq,) = _struct_I.unpack(str[start:end]) 04722 _v342 = _v341.stamp 04723 _x = _v342 04724 start = end 04725 end += 8 04726 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04727 start = end 04728 end += 4 04729 (length,) = _struct_I.unpack(str[start:end]) 04730 start = end 04731 end += length 04732 _v341.frame_id = str[start:end] 04733 start = end 04734 end += 4 04735 (length,) = _struct_I.unpack(str[start:end]) 04736 _v340.name = [] 04737 for i in range(0, length): 04738 start = end 04739 end += 4 04740 (length,) = _struct_I.unpack(str[start:end]) 04741 start = end 04742 end += length 04743 val3 = str[start:end] 04744 _v340.name.append(val3) 04745 start = end 04746 end += 4 04747 (length,) = _struct_I.unpack(str[start:end]) 04748 pattern = '<%sd'%length 04749 start = end 04750 end += struct.calcsize(pattern) 04751 _v340.position = struct.unpack(pattern, str[start:end]) 04752 start = end 04753 end += 4 04754 (length,) = _struct_I.unpack(str[start:end]) 04755 pattern = '<%sd'%length 04756 start = end 04757 end += struct.calcsize(pattern) 04758 _v340.velocity = struct.unpack(pattern, str[start:end]) 04759 start = end 04760 end += 4 04761 (length,) = _struct_I.unpack(str[start:end]) 04762 pattern = '<%sd'%length 04763 start = end 04764 end += struct.calcsize(pattern) 04765 _v340.effort = struct.unpack(pattern, str[start:end]) 04766 _v343 = val1.grasp_posture 04767 _v344 = _v343.header 04768 start = end 04769 end += 4 04770 (_v344.seq,) = _struct_I.unpack(str[start:end]) 04771 _v345 = _v344.stamp 04772 _x = _v345 04773 start = end 04774 end += 8 04775 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04776 start = end 04777 end += 4 04778 (length,) = _struct_I.unpack(str[start:end]) 04779 start = end 04780 end += length 04781 _v344.frame_id = str[start:end] 04782 start = end 04783 end += 4 04784 (length,) = _struct_I.unpack(str[start:end]) 04785 _v343.name = [] 04786 for i in range(0, length): 04787 start = end 04788 end += 4 04789 (length,) = _struct_I.unpack(str[start:end]) 04790 start = end 04791 end += length 04792 val3 = str[start:end] 04793 _v343.name.append(val3) 04794 start = end 04795 end += 4 04796 (length,) = _struct_I.unpack(str[start:end]) 04797 pattern = '<%sd'%length 04798 start = end 04799 end += struct.calcsize(pattern) 04800 _v343.position = struct.unpack(pattern, str[start:end]) 04801 start = end 04802 end += 4 04803 (length,) = _struct_I.unpack(str[start:end]) 04804 pattern = '<%sd'%length 04805 start = end 04806 end += struct.calcsize(pattern) 04807 _v343.velocity = struct.unpack(pattern, str[start:end]) 04808 start = end 04809 end += 4 04810 (length,) = _struct_I.unpack(str[start:end]) 04811 pattern = '<%sd'%length 04812 start = end 04813 end += struct.calcsize(pattern) 04814 _v343.effort = struct.unpack(pattern, str[start:end]) 04815 _v346 = val1.grasp_pose 04816 _v347 = _v346.position 04817 _x = _v347 04818 start = end 04819 end += 24 04820 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04821 _v348 = _v346.orientation 04822 _x = _v348 04823 start = end 04824 end += 32 04825 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04826 _x = val1 04827 start = end 04828 end += 17 04829 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 04830 val1.cluster_rep = bool(val1.cluster_rep) 04831 start = end 04832 end += 4 04833 (length,) = _struct_I.unpack(str[start:end]) 04834 val1.moved_obstacles = [] 04835 for i in range(0, length): 04836 val2 = object_manipulation_msgs.msg.GraspableObject() 04837 start = end 04838 end += 4 04839 (length,) = _struct_I.unpack(str[start:end]) 04840 start = end 04841 end += length 04842 val2.reference_frame_id = str[start:end] 04843 start = end 04844 end += 4 04845 (length,) = _struct_I.unpack(str[start:end]) 04846 val2.potential_models = [] 04847 for i in range(0, length): 04848 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 04849 start = end 04850 end += 4 04851 (val3.model_id,) = _struct_i.unpack(str[start:end]) 04852 _v349 = val3.pose 04853 _v350 = _v349.header 04854 start = end 04855 end += 4 04856 (_v350.seq,) = _struct_I.unpack(str[start:end]) 04857 _v351 = _v350.stamp 04858 _x = _v351 04859 start = end 04860 end += 8 04861 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04862 start = end 04863 end += 4 04864 (length,) = _struct_I.unpack(str[start:end]) 04865 start = end 04866 end += length 04867 _v350.frame_id = str[start:end] 04868 _v352 = _v349.pose 04869 _v353 = _v352.position 04870 _x = _v353 04871 start = end 04872 end += 24 04873 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04874 _v354 = _v352.orientation 04875 _x = _v354 04876 start = end 04877 end += 32 04878 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04879 start = end 04880 end += 4 04881 (val3.confidence,) = _struct_f.unpack(str[start:end]) 04882 start = end 04883 end += 4 04884 (length,) = _struct_I.unpack(str[start:end]) 04885 start = end 04886 end += length 04887 val3.detector_name = str[start:end] 04888 val2.potential_models.append(val3) 04889 _v355 = val2.cluster 04890 _v356 = _v355.header 04891 start = end 04892 end += 4 04893 (_v356.seq,) = _struct_I.unpack(str[start:end]) 04894 _v357 = _v356.stamp 04895 _x = _v357 04896 start = end 04897 end += 8 04898 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04899 start = end 04900 end += 4 04901 (length,) = _struct_I.unpack(str[start:end]) 04902 start = end 04903 end += length 04904 _v356.frame_id = str[start:end] 04905 start = end 04906 end += 4 04907 (length,) = _struct_I.unpack(str[start:end]) 04908 _v355.points = [] 04909 for i in range(0, length): 04910 val4 = geometry_msgs.msg.Point32() 04911 _x = val4 04912 start = end 04913 end += 12 04914 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 04915 _v355.points.append(val4) 04916 start = end 04917 end += 4 04918 (length,) = _struct_I.unpack(str[start:end]) 04919 _v355.channels = [] 04920 for i in range(0, length): 04921 val4 = sensor_msgs.msg.ChannelFloat32() 04922 start = end 04923 end += 4 04924 (length,) = _struct_I.unpack(str[start:end]) 04925 start = end 04926 end += length 04927 val4.name = str[start:end] 04928 start = end 04929 end += 4 04930 (length,) = _struct_I.unpack(str[start:end]) 04931 pattern = '<%sf'%length 04932 start = end 04933 end += struct.calcsize(pattern) 04934 val4.values = struct.unpack(pattern, str[start:end]) 04935 _v355.channels.append(val4) 04936 _v358 = val2.region 04937 _v359 = _v358.cloud 04938 _v360 = _v359.header 04939 start = end 04940 end += 4 04941 (_v360.seq,) = _struct_I.unpack(str[start:end]) 04942 _v361 = _v360.stamp 04943 _x = _v361 04944 start = end 04945 end += 8 04946 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04947 start = end 04948 end += 4 04949 (length,) = _struct_I.unpack(str[start:end]) 04950 start = end 04951 end += length 04952 _v360.frame_id = str[start:end] 04953 _x = _v359 04954 start = end 04955 end += 8 04956 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04957 start = end 04958 end += 4 04959 (length,) = _struct_I.unpack(str[start:end]) 04960 _v359.fields = [] 04961 for i in range(0, length): 04962 val5 = sensor_msgs.msg.PointField() 04963 start = end 04964 end += 4 04965 (length,) = _struct_I.unpack(str[start:end]) 04966 start = end 04967 end += length 04968 val5.name = str[start:end] 04969 _x = val5 04970 start = end 04971 end += 9 04972 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 04973 _v359.fields.append(val5) 04974 _x = _v359 04975 start = end 04976 end += 9 04977 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 04978 _v359.is_bigendian = bool(_v359.is_bigendian) 04979 start = end 04980 end += 4 04981 (length,) = _struct_I.unpack(str[start:end]) 04982 start = end 04983 end += length 04984 _v359.data = str[start:end] 04985 start = end 04986 end += 1 04987 (_v359.is_dense,) = _struct_B.unpack(str[start:end]) 04988 _v359.is_dense = bool(_v359.is_dense) 04989 start = end 04990 end += 4 04991 (length,) = _struct_I.unpack(str[start:end]) 04992 pattern = '<%si'%length 04993 start = end 04994 end += struct.calcsize(pattern) 04995 _v358.mask = struct.unpack(pattern, str[start:end]) 04996 _v362 = _v358.image 04997 _v363 = _v362.header 04998 start = end 04999 end += 4 05000 (_v363.seq,) = _struct_I.unpack(str[start:end]) 05001 _v364 = _v363.stamp 05002 _x = _v364 05003 start = end 05004 end += 8 05005 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05006 start = end 05007 end += 4 05008 (length,) = _struct_I.unpack(str[start:end]) 05009 start = end 05010 end += length 05011 _v363.frame_id = str[start:end] 05012 _x = _v362 05013 start = end 05014 end += 8 05015 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05016 start = end 05017 end += 4 05018 (length,) = _struct_I.unpack(str[start:end]) 05019 start = end 05020 end += length 05021 _v362.encoding = str[start:end] 05022 _x = _v362 05023 start = end 05024 end += 5 05025 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 05026 start = end 05027 end += 4 05028 (length,) = _struct_I.unpack(str[start:end]) 05029 start = end 05030 end += length 05031 _v362.data = str[start:end] 05032 _v365 = _v358.disparity_image 05033 _v366 = _v365.header 05034 start = end 05035 end += 4 05036 (_v366.seq,) = _struct_I.unpack(str[start:end]) 05037 _v367 = _v366.stamp 05038 _x = _v367 05039 start = end 05040 end += 8 05041 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05042 start = end 05043 end += 4 05044 (length,) = _struct_I.unpack(str[start:end]) 05045 start = end 05046 end += length 05047 _v366.frame_id = str[start:end] 05048 _x = _v365 05049 start = end 05050 end += 8 05051 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05052 start = end 05053 end += 4 05054 (length,) = _struct_I.unpack(str[start:end]) 05055 start = end 05056 end += length 05057 _v365.encoding = str[start:end] 05058 _x = _v365 05059 start = end 05060 end += 5 05061 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 05062 start = end 05063 end += 4 05064 (length,) = _struct_I.unpack(str[start:end]) 05065 start = end 05066 end += length 05067 _v365.data = str[start:end] 05068 _v368 = _v358.cam_info 05069 _v369 = _v368.header 05070 start = end 05071 end += 4 05072 (_v369.seq,) = _struct_I.unpack(str[start:end]) 05073 _v370 = _v369.stamp 05074 _x = _v370 05075 start = end 05076 end += 8 05077 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05078 start = end 05079 end += 4 05080 (length,) = _struct_I.unpack(str[start:end]) 05081 start = end 05082 end += length 05083 _v369.frame_id = str[start:end] 05084 _x = _v368 05085 start = end 05086 end += 8 05087 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05088 start = end 05089 end += 4 05090 (length,) = _struct_I.unpack(str[start:end]) 05091 start = end 05092 end += length 05093 _v368.distortion_model = str[start:end] 05094 start = end 05095 end += 4 05096 (length,) = _struct_I.unpack(str[start:end]) 05097 pattern = '<%sd'%length 05098 start = end 05099 end += struct.calcsize(pattern) 05100 _v368.D = struct.unpack(pattern, str[start:end]) 05101 start = end 05102 end += 72 05103 _v368.K = _struct_9d.unpack(str[start:end]) 05104 start = end 05105 end += 72 05106 _v368.R = _struct_9d.unpack(str[start:end]) 05107 start = end 05108 end += 96 05109 _v368.P = _struct_12d.unpack(str[start:end]) 05110 _x = _v368 05111 start = end 05112 end += 8 05113 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 05114 _v371 = _v368.roi 05115 _x = _v371 05116 start = end 05117 end += 17 05118 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 05119 _v371.do_rectify = bool(_v371.do_rectify) 05120 _v372 = _v358.roi_box_pose 05121 _v373 = _v372.header 05122 start = end 05123 end += 4 05124 (_v373.seq,) = _struct_I.unpack(str[start:end]) 05125 _v374 = _v373.stamp 05126 _x = _v374 05127 start = end 05128 end += 8 05129 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05130 start = end 05131 end += 4 05132 (length,) = _struct_I.unpack(str[start:end]) 05133 start = end 05134 end += length 05135 _v373.frame_id = str[start:end] 05136 _v375 = _v372.pose 05137 _v376 = _v375.position 05138 _x = _v376 05139 start = end 05140 end += 24 05141 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05142 _v377 = _v375.orientation 05143 _x = _v377 05144 start = end 05145 end += 32 05146 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 05147 _v378 = _v358.roi_box_dims 05148 _x = _v378 05149 start = end 05150 end += 24 05151 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05152 start = end 05153 end += 4 05154 (length,) = _struct_I.unpack(str[start:end]) 05155 start = end 05156 end += length 05157 val2.collision_name = str[start:end] 05158 val1.moved_obstacles.append(val2) 05159 self.grasps.append(val1) 05160 start = end 05161 end += 4 05162 (self.error_code.value,) = _struct_i.unpack(str[start:end]) 05163 return self 05164 except struct.error as e: 05165 raise roslib.message.DeserializationError(e) #most likely buffer underfill 05166 05167 05168 def serialize_numpy(self, buff, numpy): 05169 """ 05170 serialize message with numpy array types into buffer 05171 @param buff: buffer 05172 @type buff: StringIO 05173 @param numpy: numpy python module 05174 @type numpy module 05175 """ 05176 try: 05177 length = len(self.grasps) 05178 buff.write(_struct_I.pack(length)) 05179 for val1 in self.grasps: 05180 _v379 = val1.pre_grasp_posture 05181 _v380 = _v379.header 05182 buff.write(_struct_I.pack(_v380.seq)) 05183 _v381 = _v380.stamp 05184 _x = _v381 05185 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05186 _x = _v380.frame_id 05187 length = len(_x) 05188 buff.write(struct.pack('<I%ss'%length, length, _x)) 05189 length = len(_v379.name) 05190 buff.write(_struct_I.pack(length)) 05191 for val3 in _v379.name: 05192 length = len(val3) 05193 buff.write(struct.pack('<I%ss'%length, length, val3)) 05194 length = len(_v379.position) 05195 buff.write(_struct_I.pack(length)) 05196 pattern = '<%sd'%length 05197 buff.write(_v379.position.tostring()) 05198 length = len(_v379.velocity) 05199 buff.write(_struct_I.pack(length)) 05200 pattern = '<%sd'%length 05201 buff.write(_v379.velocity.tostring()) 05202 length = len(_v379.effort) 05203 buff.write(_struct_I.pack(length)) 05204 pattern = '<%sd'%length 05205 buff.write(_v379.effort.tostring()) 05206 _v382 = val1.grasp_posture 05207 _v383 = _v382.header 05208 buff.write(_struct_I.pack(_v383.seq)) 05209 _v384 = _v383.stamp 05210 _x = _v384 05211 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05212 _x = _v383.frame_id 05213 length = len(_x) 05214 buff.write(struct.pack('<I%ss'%length, length, _x)) 05215 length = len(_v382.name) 05216 buff.write(_struct_I.pack(length)) 05217 for val3 in _v382.name: 05218 length = len(val3) 05219 buff.write(struct.pack('<I%ss'%length, length, val3)) 05220 length = len(_v382.position) 05221 buff.write(_struct_I.pack(length)) 05222 pattern = '<%sd'%length 05223 buff.write(_v382.position.tostring()) 05224 length = len(_v382.velocity) 05225 buff.write(_struct_I.pack(length)) 05226 pattern = '<%sd'%length 05227 buff.write(_v382.velocity.tostring()) 05228 length = len(_v382.effort) 05229 buff.write(_struct_I.pack(length)) 05230 pattern = '<%sd'%length 05231 buff.write(_v382.effort.tostring()) 05232 _v385 = val1.grasp_pose 05233 _v386 = _v385.position 05234 _x = _v386 05235 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05236 _v387 = _v385.orientation 05237 _x = _v387 05238 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05239 _x = val1 05240 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 05241 length = len(val1.moved_obstacles) 05242 buff.write(_struct_I.pack(length)) 05243 for val2 in val1.moved_obstacles: 05244 _x = val2.reference_frame_id 05245 length = len(_x) 05246 buff.write(struct.pack('<I%ss'%length, length, _x)) 05247 length = len(val2.potential_models) 05248 buff.write(_struct_I.pack(length)) 05249 for val3 in val2.potential_models: 05250 buff.write(_struct_i.pack(val3.model_id)) 05251 _v388 = val3.pose 05252 _v389 = _v388.header 05253 buff.write(_struct_I.pack(_v389.seq)) 05254 _v390 = _v389.stamp 05255 _x = _v390 05256 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05257 _x = _v389.frame_id 05258 length = len(_x) 05259 buff.write(struct.pack('<I%ss'%length, length, _x)) 05260 _v391 = _v388.pose 05261 _v392 = _v391.position 05262 _x = _v392 05263 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05264 _v393 = _v391.orientation 05265 _x = _v393 05266 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05267 buff.write(_struct_f.pack(val3.confidence)) 05268 _x = val3.detector_name 05269 length = len(_x) 05270 buff.write(struct.pack('<I%ss'%length, length, _x)) 05271 _v394 = val2.cluster 05272 _v395 = _v394.header 05273 buff.write(_struct_I.pack(_v395.seq)) 05274 _v396 = _v395.stamp 05275 _x = _v396 05276 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05277 _x = _v395.frame_id 05278 length = len(_x) 05279 buff.write(struct.pack('<I%ss'%length, length, _x)) 05280 length = len(_v394.points) 05281 buff.write(_struct_I.pack(length)) 05282 for val4 in _v394.points: 05283 _x = val4 05284 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 05285 length = len(_v394.channels) 05286 buff.write(_struct_I.pack(length)) 05287 for val4 in _v394.channels: 05288 _x = val4.name 05289 length = len(_x) 05290 buff.write(struct.pack('<I%ss'%length, length, _x)) 05291 length = len(val4.values) 05292 buff.write(_struct_I.pack(length)) 05293 pattern = '<%sf'%length 05294 buff.write(val4.values.tostring()) 05295 _v397 = val2.region 05296 _v398 = _v397.cloud 05297 _v399 = _v398.header 05298 buff.write(_struct_I.pack(_v399.seq)) 05299 _v400 = _v399.stamp 05300 _x = _v400 05301 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05302 _x = _v399.frame_id 05303 length = len(_x) 05304 buff.write(struct.pack('<I%ss'%length, length, _x)) 05305 _x = _v398 05306 buff.write(_struct_2I.pack(_x.height, _x.width)) 05307 length = len(_v398.fields) 05308 buff.write(_struct_I.pack(length)) 05309 for val5 in _v398.fields: 05310 _x = val5.name 05311 length = len(_x) 05312 buff.write(struct.pack('<I%ss'%length, length, _x)) 05313 _x = val5 05314 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 05315 _x = _v398 05316 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 05317 _x = _v398.data 05318 length = len(_x) 05319 # - if encoded as a list instead, serialize as bytes instead of string 05320 if type(_x) in [list, tuple]: 05321 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05322 else: 05323 buff.write(struct.pack('<I%ss'%length, length, _x)) 05324 buff.write(_struct_B.pack(_v398.is_dense)) 05325 length = len(_v397.mask) 05326 buff.write(_struct_I.pack(length)) 05327 pattern = '<%si'%length 05328 buff.write(_v397.mask.tostring()) 05329 _v401 = _v397.image 05330 _v402 = _v401.header 05331 buff.write(_struct_I.pack(_v402.seq)) 05332 _v403 = _v402.stamp 05333 _x = _v403 05334 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05335 _x = _v402.frame_id 05336 length = len(_x) 05337 buff.write(struct.pack('<I%ss'%length, length, _x)) 05338 _x = _v401 05339 buff.write(_struct_2I.pack(_x.height, _x.width)) 05340 _x = _v401.encoding 05341 length = len(_x) 05342 buff.write(struct.pack('<I%ss'%length, length, _x)) 05343 _x = _v401 05344 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05345 _x = _v401.data 05346 length = len(_x) 05347 # - if encoded as a list instead, serialize as bytes instead of string 05348 if type(_x) in [list, tuple]: 05349 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05350 else: 05351 buff.write(struct.pack('<I%ss'%length, length, _x)) 05352 _v404 = _v397.disparity_image 05353 _v405 = _v404.header 05354 buff.write(_struct_I.pack(_v405.seq)) 05355 _v406 = _v405.stamp 05356 _x = _v406 05357 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05358 _x = _v405.frame_id 05359 length = len(_x) 05360 buff.write(struct.pack('<I%ss'%length, length, _x)) 05361 _x = _v404 05362 buff.write(_struct_2I.pack(_x.height, _x.width)) 05363 _x = _v404.encoding 05364 length = len(_x) 05365 buff.write(struct.pack('<I%ss'%length, length, _x)) 05366 _x = _v404 05367 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05368 _x = _v404.data 05369 length = len(_x) 05370 # - if encoded as a list instead, serialize as bytes instead of string 05371 if type(_x) in [list, tuple]: 05372 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05373 else: 05374 buff.write(struct.pack('<I%ss'%length, length, _x)) 05375 _v407 = _v397.cam_info 05376 _v408 = _v407.header 05377 buff.write(_struct_I.pack(_v408.seq)) 05378 _v409 = _v408.stamp 05379 _x = _v409 05380 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05381 _x = _v408.frame_id 05382 length = len(_x) 05383 buff.write(struct.pack('<I%ss'%length, length, _x)) 05384 _x = _v407 05385 buff.write(_struct_2I.pack(_x.height, _x.width)) 05386 _x = _v407.distortion_model 05387 length = len(_x) 05388 buff.write(struct.pack('<I%ss'%length, length, _x)) 05389 length = len(_v407.D) 05390 buff.write(_struct_I.pack(length)) 05391 pattern = '<%sd'%length 05392 buff.write(_v407.D.tostring()) 05393 buff.write(_v407.K.tostring()) 05394 buff.write(_v407.R.tostring()) 05395 buff.write(_v407.P.tostring()) 05396 _x = _v407 05397 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 05398 _v410 = _v407.roi 05399 _x = _v410 05400 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 05401 _v411 = _v397.roi_box_pose 05402 _v412 = _v411.header 05403 buff.write(_struct_I.pack(_v412.seq)) 05404 _v413 = _v412.stamp 05405 _x = _v413 05406 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05407 _x = _v412.frame_id 05408 length = len(_x) 05409 buff.write(struct.pack('<I%ss'%length, length, _x)) 05410 _v414 = _v411.pose 05411 _v415 = _v414.position 05412 _x = _v415 05413 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05414 _v416 = _v414.orientation 05415 _x = _v416 05416 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05417 _v417 = _v397.roi_box_dims 05418 _x = _v417 05419 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05420 _x = val2.collision_name 05421 length = len(_x) 05422 buff.write(struct.pack('<I%ss'%length, length, _x)) 05423 buff.write(_struct_i.pack(self.error_code.value)) 05424 except struct.error as se: self._check_types(se) 05425 except TypeError as te: self._check_types(te) 05426 05427 def deserialize_numpy(self, str, numpy): 05428 """ 05429 unpack serialized message in str into this message instance using numpy for array types 05430 @param str: byte array of serialized message 05431 @type str: str 05432 @param numpy: numpy python module 05433 @type numpy: module 05434 """ 05435 try: 05436 if self.error_code is None: 05437 self.error_code = object_manipulation_msgs.msg.GraspPlanningErrorCode() 05438 end = 0 05439 start = end 05440 end += 4 05441 (length,) = _struct_I.unpack(str[start:end]) 05442 self.grasps = [] 05443 for i in range(0, length): 05444 val1 = object_manipulation_msgs.msg.Grasp() 05445 _v418 = val1.pre_grasp_posture 05446 _v419 = _v418.header 05447 start = end 05448 end += 4 05449 (_v419.seq,) = _struct_I.unpack(str[start:end]) 05450 _v420 = _v419.stamp 05451 _x = _v420 05452 start = end 05453 end += 8 05454 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05455 start = end 05456 end += 4 05457 (length,) = _struct_I.unpack(str[start:end]) 05458 start = end 05459 end += length 05460 _v419.frame_id = str[start:end] 05461 start = end 05462 end += 4 05463 (length,) = _struct_I.unpack(str[start:end]) 05464 _v418.name = [] 05465 for i in range(0, length): 05466 start = end 05467 end += 4 05468 (length,) = _struct_I.unpack(str[start:end]) 05469 start = end 05470 end += length 05471 val3 = str[start:end] 05472 _v418.name.append(val3) 05473 start = end 05474 end += 4 05475 (length,) = _struct_I.unpack(str[start:end]) 05476 pattern = '<%sd'%length 05477 start = end 05478 end += struct.calcsize(pattern) 05479 _v418.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05480 start = end 05481 end += 4 05482 (length,) = _struct_I.unpack(str[start:end]) 05483 pattern = '<%sd'%length 05484 start = end 05485 end += struct.calcsize(pattern) 05486 _v418.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05487 start = end 05488 end += 4 05489 (length,) = _struct_I.unpack(str[start:end]) 05490 pattern = '<%sd'%length 05491 start = end 05492 end += struct.calcsize(pattern) 05493 _v418.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05494 _v421 = val1.grasp_posture 05495 _v422 = _v421.header 05496 start = end 05497 end += 4 05498 (_v422.seq,) = _struct_I.unpack(str[start:end]) 05499 _v423 = _v422.stamp 05500 _x = _v423 05501 start = end 05502 end += 8 05503 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05504 start = end 05505 end += 4 05506 (length,) = _struct_I.unpack(str[start:end]) 05507 start = end 05508 end += length 05509 _v422.frame_id = str[start:end] 05510 start = end 05511 end += 4 05512 (length,) = _struct_I.unpack(str[start:end]) 05513 _v421.name = [] 05514 for i in range(0, length): 05515 start = end 05516 end += 4 05517 (length,) = _struct_I.unpack(str[start:end]) 05518 start = end 05519 end += length 05520 val3 = str[start:end] 05521 _v421.name.append(val3) 05522 start = end 05523 end += 4 05524 (length,) = _struct_I.unpack(str[start:end]) 05525 pattern = '<%sd'%length 05526 start = end 05527 end += struct.calcsize(pattern) 05528 _v421.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05529 start = end 05530 end += 4 05531 (length,) = _struct_I.unpack(str[start:end]) 05532 pattern = '<%sd'%length 05533 start = end 05534 end += struct.calcsize(pattern) 05535 _v421.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05536 start = end 05537 end += 4 05538 (length,) = _struct_I.unpack(str[start:end]) 05539 pattern = '<%sd'%length 05540 start = end 05541 end += struct.calcsize(pattern) 05542 _v421.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05543 _v424 = val1.grasp_pose 05544 _v425 = _v424.position 05545 _x = _v425 05546 start = end 05547 end += 24 05548 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05549 _v426 = _v424.orientation 05550 _x = _v426 05551 start = end 05552 end += 32 05553 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 05554 _x = val1 05555 start = end 05556 end += 17 05557 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 05558 val1.cluster_rep = bool(val1.cluster_rep) 05559 start = end 05560 end += 4 05561 (length,) = _struct_I.unpack(str[start:end]) 05562 val1.moved_obstacles = [] 05563 for i in range(0, length): 05564 val2 = object_manipulation_msgs.msg.GraspableObject() 05565 start = end 05566 end += 4 05567 (length,) = _struct_I.unpack(str[start:end]) 05568 start = end 05569 end += length 05570 val2.reference_frame_id = str[start:end] 05571 start = end 05572 end += 4 05573 (length,) = _struct_I.unpack(str[start:end]) 05574 val2.potential_models = [] 05575 for i in range(0, length): 05576 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 05577 start = end 05578 end += 4 05579 (val3.model_id,) = _struct_i.unpack(str[start:end]) 05580 _v427 = val3.pose 05581 _v428 = _v427.header 05582 start = end 05583 end += 4 05584 (_v428.seq,) = _struct_I.unpack(str[start:end]) 05585 _v429 = _v428.stamp 05586 _x = _v429 05587 start = end 05588 end += 8 05589 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05590 start = end 05591 end += 4 05592 (length,) = _struct_I.unpack(str[start:end]) 05593 start = end 05594 end += length 05595 _v428.frame_id = str[start:end] 05596 _v430 = _v427.pose 05597 _v431 = _v430.position 05598 _x = _v431 05599 start = end 05600 end += 24 05601 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05602 _v432 = _v430.orientation 05603 _x = _v432 05604 start = end 05605 end += 32 05606 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 05607 start = end 05608 end += 4 05609 (val3.confidence,) = _struct_f.unpack(str[start:end]) 05610 start = end 05611 end += 4 05612 (length,) = _struct_I.unpack(str[start:end]) 05613 start = end 05614 end += length 05615 val3.detector_name = str[start:end] 05616 val2.potential_models.append(val3) 05617 _v433 = val2.cluster 05618 _v434 = _v433.header 05619 start = end 05620 end += 4 05621 (_v434.seq,) = _struct_I.unpack(str[start:end]) 05622 _v435 = _v434.stamp 05623 _x = _v435 05624 start = end 05625 end += 8 05626 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05627 start = end 05628 end += 4 05629 (length,) = _struct_I.unpack(str[start:end]) 05630 start = end 05631 end += length 05632 _v434.frame_id = str[start:end] 05633 start = end 05634 end += 4 05635 (length,) = _struct_I.unpack(str[start:end]) 05636 _v433.points = [] 05637 for i in range(0, length): 05638 val4 = geometry_msgs.msg.Point32() 05639 _x = val4 05640 start = end 05641 end += 12 05642 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 05643 _v433.points.append(val4) 05644 start = end 05645 end += 4 05646 (length,) = _struct_I.unpack(str[start:end]) 05647 _v433.channels = [] 05648 for i in range(0, length): 05649 val4 = sensor_msgs.msg.ChannelFloat32() 05650 start = end 05651 end += 4 05652 (length,) = _struct_I.unpack(str[start:end]) 05653 start = end 05654 end += length 05655 val4.name = str[start:end] 05656 start = end 05657 end += 4 05658 (length,) = _struct_I.unpack(str[start:end]) 05659 pattern = '<%sf'%length 05660 start = end 05661 end += struct.calcsize(pattern) 05662 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 05663 _v433.channels.append(val4) 05664 _v436 = val2.region 05665 _v437 = _v436.cloud 05666 _v438 = _v437.header 05667 start = end 05668 end += 4 05669 (_v438.seq,) = _struct_I.unpack(str[start:end]) 05670 _v439 = _v438.stamp 05671 _x = _v439 05672 start = end 05673 end += 8 05674 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05675 start = end 05676 end += 4 05677 (length,) = _struct_I.unpack(str[start:end]) 05678 start = end 05679 end += length 05680 _v438.frame_id = str[start:end] 05681 _x = _v437 05682 start = end 05683 end += 8 05684 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05685 start = end 05686 end += 4 05687 (length,) = _struct_I.unpack(str[start:end]) 05688 _v437.fields = [] 05689 for i in range(0, length): 05690 val5 = sensor_msgs.msg.PointField() 05691 start = end 05692 end += 4 05693 (length,) = _struct_I.unpack(str[start:end]) 05694 start = end 05695 end += length 05696 val5.name = str[start:end] 05697 _x = val5 05698 start = end 05699 end += 9 05700 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 05701 _v437.fields.append(val5) 05702 _x = _v437 05703 start = end 05704 end += 9 05705 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 05706 _v437.is_bigendian = bool(_v437.is_bigendian) 05707 start = end 05708 end += 4 05709 (length,) = _struct_I.unpack(str[start:end]) 05710 start = end 05711 end += length 05712 _v437.data = str[start:end] 05713 start = end 05714 end += 1 05715 (_v437.is_dense,) = _struct_B.unpack(str[start:end]) 05716 _v437.is_dense = bool(_v437.is_dense) 05717 start = end 05718 end += 4 05719 (length,) = _struct_I.unpack(str[start:end]) 05720 pattern = '<%si'%length 05721 start = end 05722 end += struct.calcsize(pattern) 05723 _v436.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 05724 _v440 = _v436.image 05725 _v441 = _v440.header 05726 start = end 05727 end += 4 05728 (_v441.seq,) = _struct_I.unpack(str[start:end]) 05729 _v442 = _v441.stamp 05730 _x = _v442 05731 start = end 05732 end += 8 05733 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05734 start = end 05735 end += 4 05736 (length,) = _struct_I.unpack(str[start:end]) 05737 start = end 05738 end += length 05739 _v441.frame_id = str[start:end] 05740 _x = _v440 05741 start = end 05742 end += 8 05743 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05744 start = end 05745 end += 4 05746 (length,) = _struct_I.unpack(str[start:end]) 05747 start = end 05748 end += length 05749 _v440.encoding = str[start:end] 05750 _x = _v440 05751 start = end 05752 end += 5 05753 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 05754 start = end 05755 end += 4 05756 (length,) = _struct_I.unpack(str[start:end]) 05757 start = end 05758 end += length 05759 _v440.data = str[start:end] 05760 _v443 = _v436.disparity_image 05761 _v444 = _v443.header 05762 start = end 05763 end += 4 05764 (_v444.seq,) = _struct_I.unpack(str[start:end]) 05765 _v445 = _v444.stamp 05766 _x = _v445 05767 start = end 05768 end += 8 05769 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05770 start = end 05771 end += 4 05772 (length,) = _struct_I.unpack(str[start:end]) 05773 start = end 05774 end += length 05775 _v444.frame_id = str[start:end] 05776 _x = _v443 05777 start = end 05778 end += 8 05779 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05780 start = end 05781 end += 4 05782 (length,) = _struct_I.unpack(str[start:end]) 05783 start = end 05784 end += length 05785 _v443.encoding = str[start:end] 05786 _x = _v443 05787 start = end 05788 end += 5 05789 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 05790 start = end 05791 end += 4 05792 (length,) = _struct_I.unpack(str[start:end]) 05793 start = end 05794 end += length 05795 _v443.data = str[start:end] 05796 _v446 = _v436.cam_info 05797 _v447 = _v446.header 05798 start = end 05799 end += 4 05800 (_v447.seq,) = _struct_I.unpack(str[start:end]) 05801 _v448 = _v447.stamp 05802 _x = _v448 05803 start = end 05804 end += 8 05805 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05806 start = end 05807 end += 4 05808 (length,) = _struct_I.unpack(str[start:end]) 05809 start = end 05810 end += length 05811 _v447.frame_id = str[start:end] 05812 _x = _v446 05813 start = end 05814 end += 8 05815 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 05816 start = end 05817 end += 4 05818 (length,) = _struct_I.unpack(str[start:end]) 05819 start = end 05820 end += length 05821 _v446.distortion_model = str[start:end] 05822 start = end 05823 end += 4 05824 (length,) = _struct_I.unpack(str[start:end]) 05825 pattern = '<%sd'%length 05826 start = end 05827 end += struct.calcsize(pattern) 05828 _v446.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 05829 start = end 05830 end += 72 05831 _v446.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 05832 start = end 05833 end += 72 05834 _v446.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 05835 start = end 05836 end += 96 05837 _v446.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 05838 _x = _v446 05839 start = end 05840 end += 8 05841 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 05842 _v449 = _v446.roi 05843 _x = _v449 05844 start = end 05845 end += 17 05846 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 05847 _v449.do_rectify = bool(_v449.do_rectify) 05848 _v450 = _v436.roi_box_pose 05849 _v451 = _v450.header 05850 start = end 05851 end += 4 05852 (_v451.seq,) = _struct_I.unpack(str[start:end]) 05853 _v452 = _v451.stamp 05854 _x = _v452 05855 start = end 05856 end += 8 05857 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05858 start = end 05859 end += 4 05860 (length,) = _struct_I.unpack(str[start:end]) 05861 start = end 05862 end += length 05863 _v451.frame_id = str[start:end] 05864 _v453 = _v450.pose 05865 _v454 = _v453.position 05866 _x = _v454 05867 start = end 05868 end += 24 05869 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05870 _v455 = _v453.orientation 05871 _x = _v455 05872 start = end 05873 end += 32 05874 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 05875 _v456 = _v436.roi_box_dims 05876 _x = _v456 05877 start = end 05878 end += 24 05879 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05880 start = end 05881 end += 4 05882 (length,) = _struct_I.unpack(str[start:end]) 05883 start = end 05884 end += length 05885 val2.collision_name = str[start:end] 05886 val1.moved_obstacles.append(val2) 05887 self.grasps.append(val1) 05888 start = end 05889 end += 4 05890 (self.error_code.value,) = _struct_i.unpack(str[start:end]) 05891 return self 05892 except struct.error as e: 05893 raise roslib.message.DeserializationError(e) #most likely buffer underfill 05894 05895 _struct_I = roslib.message.struct_I 05896 _struct_IBI = struct.Struct("<IBI") 05897 _struct_B = struct.Struct("<B") 05898 _struct_12d = struct.Struct("<12d") 05899 _struct_f = struct.Struct("<f") 05900 _struct_dB2f = struct.Struct("<dB2f") 05901 _struct_BI = struct.Struct("<BI") 05902 _struct_3f = struct.Struct("<3f") 05903 _struct_i = struct.Struct("<i") 05904 _struct_9d = struct.Struct("<9d") 05905 _struct_B2I = struct.Struct("<B2I") 05906 _struct_4d = struct.Struct("<4d") 05907 _struct_2I = struct.Struct("<2I") 05908 _struct_4IB = struct.Struct("<4IB") 05909 _struct_3d = struct.Struct("<3d") 05910 class GraspPlanning(roslib.message.ServiceDefinition): 05911 _type = 'object_manipulation_msgs/GraspPlanning' 05912 _md5sum = '01a11c1cdea613dd8705f368e1dc93dc' 05913 _request_class = GraspPlanningRequest 05914 _response_class = GraspPlanningResponse