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