$search
00001 """autogenerated by genmsg_py from GetGripperPoseActionGoal.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import pr2_object_manipulation_msgs.msg 00006 import object_manipulation_msgs.msg 00007 import roslib.rostime 00008 import actionlib_msgs.msg 00009 import geometry_msgs.msg 00010 import sensor_msgs.msg 00011 import household_objects_database_msgs.msg 00012 import std_msgs.msg 00013 00014 class GetGripperPoseActionGoal(roslib.message.Message): 00015 _md5sum = "1967a7e9bdf026bfaa86707548b502d9" 00016 _type = "pr2_object_manipulation_msgs/GetGripperPoseActionGoal" 00017 _has_header = True #flag to mark the presence of a Header object 00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00019 00020 Header header 00021 actionlib_msgs/GoalID goal_id 00022 GetGripperPoseGoal goal 00023 00024 ================================================================================ 00025 MSG: std_msgs/Header 00026 # Standard metadata for higher-level stamped data types. 00027 # This is generally used to communicate timestamped data 00028 # in a particular coordinate frame. 00029 # 00030 # sequence ID: consecutively increasing ID 00031 uint32 seq 00032 #Two-integer timestamp that is expressed as: 00033 # * stamp.secs: seconds (stamp_secs) since epoch 00034 # * stamp.nsecs: nanoseconds since stamp_secs 00035 # time-handling sugar is provided by the client library 00036 time stamp 00037 #Frame this data is associated with 00038 # 0: no frame 00039 # 1: global frame 00040 string frame_id 00041 00042 ================================================================================ 00043 MSG: actionlib_msgs/GoalID 00044 # The stamp should store the time at which this goal was requested. 00045 # It is used by an action server when it tries to preempt all 00046 # goals that were requested before a certain time 00047 time stamp 00048 00049 # The id provides a way to associate feedback and 00050 # result message with specific goal requests. The id 00051 # specified must be unique. 00052 string id 00053 00054 00055 ================================================================================ 00056 MSG: pr2_object_manipulation_msgs/GetGripperPoseGoal 00057 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00058 00059 # for which arm are we requesting a pose 00060 # useful for knowing which arm to initialize from when seed is empty 00061 string arm_name 00062 00063 # a seed position for the ghosted gripper. If emtpy (quaternion of norm 0) 00064 # the ghosted gripper will be initialized at the current position of the gripper 00065 geometry_msgs/PoseStamped gripper_pose 00066 float32 gripper_opening 00067 00068 # An object that the gripper is holding. May be empty. 00069 object_manipulation_msgs/GraspableObject object 00070 00071 # how we are holding the object, if any. 00072 object_manipulation_msgs/Grasp grasp 00073 00074 00075 ================================================================================ 00076 MSG: geometry_msgs/PoseStamped 00077 # A Pose with reference coordinate frame and timestamp 00078 Header header 00079 Pose pose 00080 00081 ================================================================================ 00082 MSG: geometry_msgs/Pose 00083 # A representation of pose in free space, composed of postion and orientation. 00084 Point position 00085 Quaternion orientation 00086 00087 ================================================================================ 00088 MSG: geometry_msgs/Point 00089 # This contains the position of a point in free space 00090 float64 x 00091 float64 y 00092 float64 z 00093 00094 ================================================================================ 00095 MSG: geometry_msgs/Quaternion 00096 # This represents an orientation in free space in quaternion form. 00097 00098 float64 x 00099 float64 y 00100 float64 z 00101 float64 w 00102 00103 ================================================================================ 00104 MSG: object_manipulation_msgs/GraspableObject 00105 # an object that the object_manipulator can work on 00106 00107 # a graspable object can be represented in multiple ways. This message 00108 # can contain all of them. Which one is actually used is up to the receiver 00109 # of this message. When adding new representations, one must be careful that 00110 # they have reasonable lightweight defaults indicating that that particular 00111 # representation is not available. 00112 00113 # the tf frame to be used as a reference frame when combining information from 00114 # the different representations below 00115 string reference_frame_id 00116 00117 # potential recognition results from a database of models 00118 # all poses are relative to the object reference pose 00119 household_objects_database_msgs/DatabaseModelPose[] potential_models 00120 00121 # the point cloud itself 00122 sensor_msgs/PointCloud cluster 00123 00124 # a region of a PointCloud2 of interest 00125 object_manipulation_msgs/SceneRegion region 00126 00127 # the name that this object has in the collision environment 00128 string collision_name 00129 ================================================================================ 00130 MSG: household_objects_database_msgs/DatabaseModelPose 00131 # Informs that a specific model from the Model Database has been 00132 # identified at a certain location 00133 00134 # the database id of the model 00135 int32 model_id 00136 00137 # the pose that it can be found in 00138 geometry_msgs/PoseStamped pose 00139 00140 # a measure of the confidence level in this detection result 00141 float32 confidence 00142 00143 # the name of the object detector that generated this detection result 00144 string detector_name 00145 00146 ================================================================================ 00147 MSG: sensor_msgs/PointCloud 00148 # This message holds a collection of 3d points, plus optional additional 00149 # information about each point. 00150 00151 # Time of sensor data acquisition, coordinate frame ID. 00152 Header header 00153 00154 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00155 # in the frame given in the header. 00156 geometry_msgs/Point32[] points 00157 00158 # Each channel should have the same number of elements as points array, 00159 # and the data in each channel should correspond 1:1 with each point. 00160 # Channel names in common practice are listed in ChannelFloat32.msg. 00161 ChannelFloat32[] channels 00162 00163 ================================================================================ 00164 MSG: geometry_msgs/Point32 00165 # This contains the position of a point in free space(with 32 bits of precision). 00166 # It is recommeded to use Point wherever possible instead of Point32. 00167 # 00168 # This recommendation is to promote interoperability. 00169 # 00170 # This message is designed to take up less space when sending 00171 # lots of points at once, as in the case of a PointCloud. 00172 00173 float32 x 00174 float32 y 00175 float32 z 00176 ================================================================================ 00177 MSG: sensor_msgs/ChannelFloat32 00178 # This message is used by the PointCloud message to hold optional data 00179 # associated with each point in the cloud. The length of the values 00180 # array should be the same as the length of the points array in the 00181 # PointCloud, and each value should be associated with the corresponding 00182 # point. 00183 00184 # Channel names in existing practice include: 00185 # "u", "v" - row and column (respectively) in the left stereo image. 00186 # This is opposite to usual conventions but remains for 00187 # historical reasons. The newer PointCloud2 message has no 00188 # such problem. 00189 # "rgb" - For point clouds produced by color stereo cameras. uint8 00190 # (R,G,B) values packed into the least significant 24 bits, 00191 # in order. 00192 # "intensity" - laser or pixel intensity. 00193 # "distance" 00194 00195 # The channel name should give semantics of the channel (e.g. 00196 # "intensity" instead of "value"). 00197 string name 00198 00199 # The values array should be 1-1 with the elements of the associated 00200 # PointCloud. 00201 float32[] values 00202 00203 ================================================================================ 00204 MSG: object_manipulation_msgs/SceneRegion 00205 # Point cloud 00206 sensor_msgs/PointCloud2 cloud 00207 00208 # Indices for the region of interest 00209 int32[] mask 00210 00211 # One of the corresponding 2D images, if applicable 00212 sensor_msgs/Image image 00213 00214 # The disparity image, if applicable 00215 sensor_msgs/Image disparity_image 00216 00217 # Camera info for the camera that took the image 00218 sensor_msgs/CameraInfo cam_info 00219 00220 # a 3D region of interest for grasp planning 00221 geometry_msgs/PoseStamped roi_box_pose 00222 geometry_msgs/Vector3 roi_box_dims 00223 00224 ================================================================================ 00225 MSG: sensor_msgs/PointCloud2 00226 # This message holds a collection of N-dimensional points, which may 00227 # contain additional information such as normals, intensity, etc. The 00228 # point data is stored as a binary blob, its layout described by the 00229 # contents of the "fields" array. 00230 00231 # The point cloud data may be organized 2d (image-like) or 1d 00232 # (unordered). Point clouds organized as 2d images may be produced by 00233 # camera depth sensors such as stereo or time-of-flight. 00234 00235 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00236 # points). 00237 Header header 00238 00239 # 2D structure of the point cloud. If the cloud is unordered, height is 00240 # 1 and width is the length of the point cloud. 00241 uint32 height 00242 uint32 width 00243 00244 # Describes the channels and their layout in the binary data blob. 00245 PointField[] fields 00246 00247 bool is_bigendian # Is this data bigendian? 00248 uint32 point_step # Length of a point in bytes 00249 uint32 row_step # Length of a row in bytes 00250 uint8[] data # Actual point data, size is (row_step*height) 00251 00252 bool is_dense # True if there are no invalid points 00253 00254 ================================================================================ 00255 MSG: sensor_msgs/PointField 00256 # This message holds the description of one point entry in the 00257 # PointCloud2 message format. 00258 uint8 INT8 = 1 00259 uint8 UINT8 = 2 00260 uint8 INT16 = 3 00261 uint8 UINT16 = 4 00262 uint8 INT32 = 5 00263 uint8 UINT32 = 6 00264 uint8 FLOAT32 = 7 00265 uint8 FLOAT64 = 8 00266 00267 string name # Name of field 00268 uint32 offset # Offset from start of point struct 00269 uint8 datatype # Datatype enumeration, see above 00270 uint32 count # How many elements in the field 00271 00272 ================================================================================ 00273 MSG: sensor_msgs/Image 00274 # This message contains an uncompressed image 00275 # (0, 0) is at top-left corner of image 00276 # 00277 00278 Header header # Header timestamp should be acquisition time of image 00279 # Header frame_id should be optical frame of camera 00280 # origin of frame should be optical center of cameara 00281 # +x should point to the right in the image 00282 # +y should point down in the image 00283 # +z should point into to plane of the image 00284 # If the frame_id here and the frame_id of the CameraInfo 00285 # message associated with the image conflict 00286 # the behavior is undefined 00287 00288 uint32 height # image height, that is, number of rows 00289 uint32 width # image width, that is, number of columns 00290 00291 # The legal values for encoding are in file src/image_encodings.cpp 00292 # If you want to standardize a new string format, join 00293 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00294 00295 string encoding # Encoding of pixels -- channel meaning, ordering, size 00296 # taken from the list of strings in src/image_encodings.cpp 00297 00298 uint8 is_bigendian # is this data bigendian? 00299 uint32 step # Full row length in bytes 00300 uint8[] data # actual matrix data, size is (step * rows) 00301 00302 ================================================================================ 00303 MSG: sensor_msgs/CameraInfo 00304 # This message defines meta information for a camera. It should be in a 00305 # camera namespace on topic "camera_info" and accompanied by up to five 00306 # image topics named: 00307 # 00308 # image_raw - raw data from the camera driver, possibly Bayer encoded 00309 # image - monochrome, distorted 00310 # image_color - color, distorted 00311 # image_rect - monochrome, rectified 00312 # image_rect_color - color, rectified 00313 # 00314 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00315 # for producing the four processed image topics from image_raw and 00316 # camera_info. The meaning of the camera parameters are described in 00317 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00318 # 00319 # The image_geometry package provides a user-friendly interface to 00320 # common operations using this meta information. If you want to, e.g., 00321 # project a 3d point into image coordinates, we strongly recommend 00322 # using image_geometry. 00323 # 00324 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00325 # zeroed out. In particular, clients may assume that K[0] == 0.0 00326 # indicates an uncalibrated camera. 00327 00328 ####################################################################### 00329 # Image acquisition info # 00330 ####################################################################### 00331 00332 # Time of image acquisition, camera coordinate frame ID 00333 Header header # Header timestamp should be acquisition time of image 00334 # Header frame_id should be optical frame of camera 00335 # origin of frame should be optical center of camera 00336 # +x should point to the right in the image 00337 # +y should point down in the image 00338 # +z should point into the plane of the image 00339 00340 00341 ####################################################################### 00342 # Calibration Parameters # 00343 ####################################################################### 00344 # These are fixed during camera calibration. Their values will be the # 00345 # same in all messages until the camera is recalibrated. Note that # 00346 # self-calibrating systems may "recalibrate" frequently. # 00347 # # 00348 # The internal parameters can be used to warp a raw (distorted) image # 00349 # to: # 00350 # 1. An undistorted image (requires D and K) # 00351 # 2. A rectified image (requires D, K, R) # 00352 # The projection matrix P projects 3D points into the rectified image.# 00353 ####################################################################### 00354 00355 # The image dimensions with which the camera was calibrated. Normally 00356 # this will be the full camera resolution in pixels. 00357 uint32 height 00358 uint32 width 00359 00360 # The distortion model used. Supported models are listed in 00361 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00362 # simple model of radial and tangential distortion - is sufficent. 00363 string distortion_model 00364 00365 # The distortion parameters, size depending on the distortion model. 00366 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00367 float64[] D 00368 00369 # Intrinsic camera matrix for the raw (distorted) images. 00370 # [fx 0 cx] 00371 # K = [ 0 fy cy] 00372 # [ 0 0 1] 00373 # Projects 3D points in the camera coordinate frame to 2D pixel 00374 # coordinates using the focal lengths (fx, fy) and principal point 00375 # (cx, cy). 00376 float64[9] K # 3x3 row-major matrix 00377 00378 # Rectification matrix (stereo cameras only) 00379 # A rotation matrix aligning the camera coordinate system to the ideal 00380 # stereo image plane so that epipolar lines in both stereo images are 00381 # parallel. 00382 float64[9] R # 3x3 row-major matrix 00383 00384 # Projection/camera matrix 00385 # [fx' 0 cx' Tx] 00386 # P = [ 0 fy' cy' Ty] 00387 # [ 0 0 1 0] 00388 # By convention, this matrix specifies the intrinsic (camera) matrix 00389 # of the processed (rectified) image. That is, the left 3x3 portion 00390 # is the normal camera intrinsic matrix for the rectified image. 00391 # It projects 3D points in the camera coordinate frame to 2D pixel 00392 # coordinates using the focal lengths (fx', fy') and principal point 00393 # (cx', cy') - these may differ from the values in K. 00394 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00395 # also have R = the identity and P[1:3,1:3] = K. 00396 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00397 # position of the optical center of the second camera in the first 00398 # camera's frame. We assume Tz = 0 so both cameras are in the same 00399 # stereo image plane. The first camera always has Tx = Ty = 0. For 00400 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00401 # Tx = -fx' * B, where B is the baseline between the cameras. 00402 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00403 # the rectified image is given by: 00404 # [u v w]' = P * [X Y Z 1]' 00405 # x = u / w 00406 # y = v / w 00407 # This holds for both images of a stereo pair. 00408 float64[12] P # 3x4 row-major matrix 00409 00410 00411 ####################################################################### 00412 # Operational Parameters # 00413 ####################################################################### 00414 # These define the image region actually captured by the camera # 00415 # driver. Although they affect the geometry of the output image, they # 00416 # may be changed freely without recalibrating the camera. # 00417 ####################################################################### 00418 00419 # Binning refers here to any camera setting which combines rectangular 00420 # neighborhoods of pixels into larger "super-pixels." It reduces the 00421 # resolution of the output image to 00422 # (width / binning_x) x (height / binning_y). 00423 # The default values binning_x = binning_y = 0 is considered the same 00424 # as binning_x = binning_y = 1 (no subsampling). 00425 uint32 binning_x 00426 uint32 binning_y 00427 00428 # Region of interest (subwindow of full camera resolution), given in 00429 # full resolution (unbinned) image coordinates. A particular ROI 00430 # always denotes the same window of pixels on the camera sensor, 00431 # regardless of binning settings. 00432 # The default setting of roi (all values 0) is considered the same as 00433 # full resolution (roi.width = width, roi.height = height). 00434 RegionOfInterest roi 00435 00436 ================================================================================ 00437 MSG: sensor_msgs/RegionOfInterest 00438 # This message is used to specify a region of interest within an image. 00439 # 00440 # When used to specify the ROI setting of the camera when the image was 00441 # taken, the height and width fields should either match the height and 00442 # width fields for the associated image; or height = width = 0 00443 # indicates that the full resolution image was captured. 00444 00445 uint32 x_offset # Leftmost pixel of the ROI 00446 # (0 if the ROI includes the left edge of the image) 00447 uint32 y_offset # Topmost pixel of the ROI 00448 # (0 if the ROI includes the top edge of the image) 00449 uint32 height # Height of ROI 00450 uint32 width # Width of ROI 00451 00452 # True if a distinct rectified ROI should be calculated from the "raw" 00453 # ROI in this message. Typically this should be False if the full image 00454 # is captured (ROI not used), and True if a subwindow is captured (ROI 00455 # used). 00456 bool do_rectify 00457 00458 ================================================================================ 00459 MSG: geometry_msgs/Vector3 00460 # This represents a vector in free space. 00461 00462 float64 x 00463 float64 y 00464 float64 z 00465 ================================================================================ 00466 MSG: object_manipulation_msgs/Grasp 00467 00468 # The internal posture of the hand for the pre-grasp 00469 # only positions are used 00470 sensor_msgs/JointState pre_grasp_posture 00471 00472 # The internal posture of the hand for the grasp 00473 # positions and efforts are used 00474 sensor_msgs/JointState grasp_posture 00475 00476 # The position of the end-effector for the grasp relative to a reference frame 00477 # (that is always specified elsewhere, not in this message) 00478 geometry_msgs/Pose grasp_pose 00479 00480 # The estimated probability of success for this grasp 00481 float64 success_probability 00482 00483 # Debug flag to indicate that this grasp would be the best in its cluster 00484 bool cluster_rep 00485 00486 # how far the pre-grasp should ideally be away from the grasp 00487 float32 desired_approach_distance 00488 00489 # how much distance between pre-grasp and grasp must actually be feasible 00490 # for the grasp not to be rejected 00491 float32 min_approach_distance 00492 00493 # an optional list of obstacles that we have semantic information about 00494 # and that we expect might move in the course of executing this grasp 00495 # the grasp planner is expected to make sure they move in an OK way; during 00496 # execution, grasp executors will not check for collisions against these objects 00497 GraspableObject[] moved_obstacles 00498 00499 ================================================================================ 00500 MSG: sensor_msgs/JointState 00501 # This is a message that holds data to describe the state of a set of torque controlled joints. 00502 # 00503 # The state of each joint (revolute or prismatic) is defined by: 00504 # * the position of the joint (rad or m), 00505 # * the velocity of the joint (rad/s or m/s) and 00506 # * the effort that is applied in the joint (Nm or N). 00507 # 00508 # Each joint is uniquely identified by its name 00509 # The header specifies the time at which the joint states were recorded. All the joint states 00510 # in one message have to be recorded at the same time. 00511 # 00512 # This message consists of a multiple arrays, one for each part of the joint state. 00513 # The goal is to make each of the fields optional. When e.g. your joints have no 00514 # effort associated with them, you can leave the effort array empty. 00515 # 00516 # All arrays in this message should have the same size, or be empty. 00517 # This is the only way to uniquely associate the joint name with the correct 00518 # states. 00519 00520 00521 Header header 00522 00523 string[] name 00524 float64[] position 00525 float64[] velocity 00526 float64[] effort 00527 00528 """ 00529 __slots__ = ['header','goal_id','goal'] 00530 _slot_types = ['Header','actionlib_msgs/GoalID','pr2_object_manipulation_msgs/GetGripperPoseGoal'] 00531 00532 def __init__(self, *args, **kwds): 00533 """ 00534 Constructor. Any message fields that are implicitly/explicitly 00535 set to None will be assigned a default value. The recommend 00536 use is keyword arguments as this is more robust to future message 00537 changes. You cannot mix in-order arguments and keyword arguments. 00538 00539 The available fields are: 00540 header,goal_id,goal 00541 00542 @param args: complete set of field values, in .msg order 00543 @param kwds: use keyword arguments corresponding to message field names 00544 to set specific fields. 00545 """ 00546 if args or kwds: 00547 super(GetGripperPoseActionGoal, self).__init__(*args, **kwds) 00548 #message fields cannot be None, assign default values for those that are 00549 if self.header is None: 00550 self.header = std_msgs.msg._Header.Header() 00551 if self.goal_id is None: 00552 self.goal_id = actionlib_msgs.msg.GoalID() 00553 if self.goal is None: 00554 self.goal = pr2_object_manipulation_msgs.msg.GetGripperPoseGoal() 00555 else: 00556 self.header = std_msgs.msg._Header.Header() 00557 self.goal_id = actionlib_msgs.msg.GoalID() 00558 self.goal = pr2_object_manipulation_msgs.msg.GetGripperPoseGoal() 00559 00560 def _get_types(self): 00561 """ 00562 internal API method 00563 """ 00564 return self._slot_types 00565 00566 def serialize(self, buff): 00567 """ 00568 serialize message into buffer 00569 @param buff: buffer 00570 @type buff: StringIO 00571 """ 00572 try: 00573 _x = self 00574 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00575 _x = self.header.frame_id 00576 length = len(_x) 00577 buff.write(struct.pack('<I%ss'%length, length, _x)) 00578 _x = self 00579 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs)) 00580 _x = self.goal_id.id 00581 length = len(_x) 00582 buff.write(struct.pack('<I%ss'%length, length, _x)) 00583 _x = self.goal.arm_name 00584 length = len(_x) 00585 buff.write(struct.pack('<I%ss'%length, length, _x)) 00586 _x = self 00587 buff.write(_struct_3I.pack(_x.goal.gripper_pose.header.seq, _x.goal.gripper_pose.header.stamp.secs, _x.goal.gripper_pose.header.stamp.nsecs)) 00588 _x = self.goal.gripper_pose.header.frame_id 00589 length = len(_x) 00590 buff.write(struct.pack('<I%ss'%length, length, _x)) 00591 _x = self 00592 buff.write(_struct_7df.pack(_x.goal.gripper_pose.pose.position.x, _x.goal.gripper_pose.pose.position.y, _x.goal.gripper_pose.pose.position.z, _x.goal.gripper_pose.pose.orientation.x, _x.goal.gripper_pose.pose.orientation.y, _x.goal.gripper_pose.pose.orientation.z, _x.goal.gripper_pose.pose.orientation.w, _x.goal.gripper_opening)) 00593 _x = self.goal.object.reference_frame_id 00594 length = len(_x) 00595 buff.write(struct.pack('<I%ss'%length, length, _x)) 00596 length = len(self.goal.object.potential_models) 00597 buff.write(_struct_I.pack(length)) 00598 for val1 in self.goal.object.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.object.cluster.header.seq, _x.goal.object.cluster.header.stamp.secs, _x.goal.object.cluster.header.stamp.nsecs)) 00622 _x = self.goal.object.cluster.header.frame_id 00623 length = len(_x) 00624 buff.write(struct.pack('<I%ss'%length, length, _x)) 00625 length = len(self.goal.object.cluster.points) 00626 buff.write(_struct_I.pack(length)) 00627 for val1 in self.goal.object.cluster.points: 00628 _x = val1 00629 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00630 length = len(self.goal.object.cluster.channels) 00631 buff.write(_struct_I.pack(length)) 00632 for val1 in self.goal.object.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.object.region.cloud.header.seq, _x.goal.object.region.cloud.header.stamp.secs, _x.goal.object.region.cloud.header.stamp.nsecs)) 00642 _x = self.goal.object.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.object.region.cloud.height, _x.goal.object.region.cloud.width)) 00647 length = len(self.goal.object.region.cloud.fields) 00648 buff.write(_struct_I.pack(length)) 00649 for val1 in self.goal.object.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.object.region.cloud.is_bigendian, _x.goal.object.region.cloud.point_step, _x.goal.object.region.cloud.row_step)) 00657 _x = self.goal.object.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.object.region.cloud.is_dense)) 00665 length = len(self.goal.object.region.mask) 00666 buff.write(_struct_I.pack(length)) 00667 pattern = '<%si'%length 00668 buff.write(struct.pack(pattern, *self.goal.object.region.mask)) 00669 _x = self 00670 buff.write(_struct_3I.pack(_x.goal.object.region.image.header.seq, _x.goal.object.region.image.header.stamp.secs, _x.goal.object.region.image.header.stamp.nsecs)) 00671 _x = self.goal.object.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.object.region.image.height, _x.goal.object.region.image.width)) 00676 _x = self.goal.object.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.object.region.image.is_bigendian, _x.goal.object.region.image.step)) 00681 _x = self.goal.object.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.object.region.disparity_image.header.seq, _x.goal.object.region.disparity_image.header.stamp.secs, _x.goal.object.region.disparity_image.header.stamp.nsecs)) 00690 _x = self.goal.object.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.object.region.disparity_image.height, _x.goal.object.region.disparity_image.width)) 00695 _x = self.goal.object.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.object.region.disparity_image.is_bigendian, _x.goal.object.region.disparity_image.step)) 00700 _x = self.goal.object.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.object.region.cam_info.header.seq, _x.goal.object.region.cam_info.header.stamp.secs, _x.goal.object.region.cam_info.header.stamp.nsecs)) 00709 _x = self.goal.object.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.object.region.cam_info.height, _x.goal.object.region.cam_info.width)) 00714 _x = self.goal.object.region.cam_info.distortion_model 00715 length = len(_x) 00716 buff.write(struct.pack('<I%ss'%length, length, _x)) 00717 length = len(self.goal.object.region.cam_info.D) 00718 buff.write(_struct_I.pack(length)) 00719 pattern = '<%sd'%length 00720 buff.write(struct.pack(pattern, *self.goal.object.region.cam_info.D)) 00721 buff.write(_struct_9d.pack(*self.goal.object.region.cam_info.K)) 00722 buff.write(_struct_9d.pack(*self.goal.object.region.cam_info.R)) 00723 buff.write(_struct_12d.pack(*self.goal.object.region.cam_info.P)) 00724 _x = self 00725 buff.write(_struct_6IB3I.pack(_x.goal.object.region.cam_info.binning_x, _x.goal.object.region.cam_info.binning_y, _x.goal.object.region.cam_info.roi.x_offset, _x.goal.object.region.cam_info.roi.y_offset, _x.goal.object.region.cam_info.roi.height, _x.goal.object.region.cam_info.roi.width, _x.goal.object.region.cam_info.roi.do_rectify, _x.goal.object.region.roi_box_pose.header.seq, _x.goal.object.region.roi_box_pose.header.stamp.secs, _x.goal.object.region.roi_box_pose.header.stamp.nsecs)) 00726 _x = self.goal.object.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.object.region.roi_box_pose.pose.position.x, _x.goal.object.region.roi_box_pose.pose.position.y, _x.goal.object.region.roi_box_pose.pose.position.z, _x.goal.object.region.roi_box_pose.pose.orientation.x, _x.goal.object.region.roi_box_pose.pose.orientation.y, _x.goal.object.region.roi_box_pose.pose.orientation.z, _x.goal.object.region.roi_box_pose.pose.orientation.w, _x.goal.object.region.roi_box_dims.x, _x.goal.object.region.roi_box_dims.y, _x.goal.object.region.roi_box_dims.z)) 00731 _x = self.goal.object.collision_name 00732 length = len(_x) 00733 buff.write(struct.pack('<I%ss'%length, length, _x)) 00734 _x = self 00735 buff.write(_struct_3I.pack(_x.goal.grasp.pre_grasp_posture.header.seq, _x.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.goal.grasp.pre_grasp_posture.header.stamp.nsecs)) 00736 _x = self.goal.grasp.pre_grasp_posture.header.frame_id 00737 length = len(_x) 00738 buff.write(struct.pack('<I%ss'%length, length, _x)) 00739 length = len(self.goal.grasp.pre_grasp_posture.name) 00740 buff.write(_struct_I.pack(length)) 00741 for val1 in self.goal.grasp.pre_grasp_posture.name: 00742 length = len(val1) 00743 buff.write(struct.pack('<I%ss'%length, length, val1)) 00744 length = len(self.goal.grasp.pre_grasp_posture.position) 00745 buff.write(_struct_I.pack(length)) 00746 pattern = '<%sd'%length 00747 buff.write(struct.pack(pattern, *self.goal.grasp.pre_grasp_posture.position)) 00748 length = len(self.goal.grasp.pre_grasp_posture.velocity) 00749 buff.write(_struct_I.pack(length)) 00750 pattern = '<%sd'%length 00751 buff.write(struct.pack(pattern, *self.goal.grasp.pre_grasp_posture.velocity)) 00752 length = len(self.goal.grasp.pre_grasp_posture.effort) 00753 buff.write(_struct_I.pack(length)) 00754 pattern = '<%sd'%length 00755 buff.write(struct.pack(pattern, *self.goal.grasp.pre_grasp_posture.effort)) 00756 _x = self 00757 buff.write(_struct_3I.pack(_x.goal.grasp.grasp_posture.header.seq, _x.goal.grasp.grasp_posture.header.stamp.secs, _x.goal.grasp.grasp_posture.header.stamp.nsecs)) 00758 _x = self.goal.grasp.grasp_posture.header.frame_id 00759 length = len(_x) 00760 buff.write(struct.pack('<I%ss'%length, length, _x)) 00761 length = len(self.goal.grasp.grasp_posture.name) 00762 buff.write(_struct_I.pack(length)) 00763 for val1 in self.goal.grasp.grasp_posture.name: 00764 length = len(val1) 00765 buff.write(struct.pack('<I%ss'%length, length, val1)) 00766 length = len(self.goal.grasp.grasp_posture.position) 00767 buff.write(_struct_I.pack(length)) 00768 pattern = '<%sd'%length 00769 buff.write(struct.pack(pattern, *self.goal.grasp.grasp_posture.position)) 00770 length = len(self.goal.grasp.grasp_posture.velocity) 00771 buff.write(_struct_I.pack(length)) 00772 pattern = '<%sd'%length 00773 buff.write(struct.pack(pattern, *self.goal.grasp.grasp_posture.velocity)) 00774 length = len(self.goal.grasp.grasp_posture.effort) 00775 buff.write(_struct_I.pack(length)) 00776 pattern = '<%sd'%length 00777 buff.write(struct.pack(pattern, *self.goal.grasp.grasp_posture.effort)) 00778 _x = self 00779 buff.write(_struct_8dB2f.pack(_x.goal.grasp.grasp_pose.position.x, _x.goal.grasp.grasp_pose.position.y, _x.goal.grasp.grasp_pose.position.z, _x.goal.grasp.grasp_pose.orientation.x, _x.goal.grasp.grasp_pose.orientation.y, _x.goal.grasp.grasp_pose.orientation.z, _x.goal.grasp.grasp_pose.orientation.w, _x.goal.grasp.success_probability, _x.goal.grasp.cluster_rep, _x.goal.grasp.desired_approach_distance, _x.goal.grasp.min_approach_distance)) 00780 length = len(self.goal.grasp.moved_obstacles) 00781 buff.write(_struct_I.pack(length)) 00782 for val1 in self.goal.grasp.moved_obstacles: 00783 _x = val1.reference_frame_id 00784 length = len(_x) 00785 buff.write(struct.pack('<I%ss'%length, length, _x)) 00786 length = len(val1.potential_models) 00787 buff.write(_struct_I.pack(length)) 00788 for val2 in val1.potential_models: 00789 buff.write(_struct_i.pack(val2.model_id)) 00790 _v7 = val2.pose 00791 _v8 = _v7.header 00792 buff.write(_struct_I.pack(_v8.seq)) 00793 _v9 = _v8.stamp 00794 _x = _v9 00795 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00796 _x = _v8.frame_id 00797 length = len(_x) 00798 buff.write(struct.pack('<I%ss'%length, length, _x)) 00799 _v10 = _v7.pose 00800 _v11 = _v10.position 00801 _x = _v11 00802 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00803 _v12 = _v10.orientation 00804 _x = _v12 00805 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00806 buff.write(_struct_f.pack(val2.confidence)) 00807 _x = val2.detector_name 00808 length = len(_x) 00809 buff.write(struct.pack('<I%ss'%length, length, _x)) 00810 _v13 = val1.cluster 00811 _v14 = _v13.header 00812 buff.write(_struct_I.pack(_v14.seq)) 00813 _v15 = _v14.stamp 00814 _x = _v15 00815 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00816 _x = _v14.frame_id 00817 length = len(_x) 00818 buff.write(struct.pack('<I%ss'%length, length, _x)) 00819 length = len(_v13.points) 00820 buff.write(_struct_I.pack(length)) 00821 for val3 in _v13.points: 00822 _x = val3 00823 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00824 length = len(_v13.channels) 00825 buff.write(_struct_I.pack(length)) 00826 for val3 in _v13.channels: 00827 _x = val3.name 00828 length = len(_x) 00829 buff.write(struct.pack('<I%ss'%length, length, _x)) 00830 length = len(val3.values) 00831 buff.write(_struct_I.pack(length)) 00832 pattern = '<%sf'%length 00833 buff.write(struct.pack(pattern, *val3.values)) 00834 _v16 = val1.region 00835 _v17 = _v16.cloud 00836 _v18 = _v17.header 00837 buff.write(_struct_I.pack(_v18.seq)) 00838 _v19 = _v18.stamp 00839 _x = _v19 00840 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00841 _x = _v18.frame_id 00842 length = len(_x) 00843 buff.write(struct.pack('<I%ss'%length, length, _x)) 00844 _x = _v17 00845 buff.write(_struct_2I.pack(_x.height, _x.width)) 00846 length = len(_v17.fields) 00847 buff.write(_struct_I.pack(length)) 00848 for val4 in _v17.fields: 00849 _x = val4.name 00850 length = len(_x) 00851 buff.write(struct.pack('<I%ss'%length, length, _x)) 00852 _x = val4 00853 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00854 _x = _v17 00855 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 00856 _x = _v17.data 00857 length = len(_x) 00858 # - if encoded as a list instead, serialize as bytes instead of string 00859 if type(_x) in [list, tuple]: 00860 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00861 else: 00862 buff.write(struct.pack('<I%ss'%length, length, _x)) 00863 buff.write(_struct_B.pack(_v17.is_dense)) 00864 length = len(_v16.mask) 00865 buff.write(_struct_I.pack(length)) 00866 pattern = '<%si'%length 00867 buff.write(struct.pack(pattern, *_v16.mask)) 00868 _v20 = _v16.image 00869 _v21 = _v20.header 00870 buff.write(_struct_I.pack(_v21.seq)) 00871 _v22 = _v21.stamp 00872 _x = _v22 00873 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00874 _x = _v21.frame_id 00875 length = len(_x) 00876 buff.write(struct.pack('<I%ss'%length, length, _x)) 00877 _x = _v20 00878 buff.write(_struct_2I.pack(_x.height, _x.width)) 00879 _x = _v20.encoding 00880 length = len(_x) 00881 buff.write(struct.pack('<I%ss'%length, length, _x)) 00882 _x = _v20 00883 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00884 _x = _v20.data 00885 length = len(_x) 00886 # - if encoded as a list instead, serialize as bytes instead of string 00887 if type(_x) in [list, tuple]: 00888 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00889 else: 00890 buff.write(struct.pack('<I%ss'%length, length, _x)) 00891 _v23 = _v16.disparity_image 00892 _v24 = _v23.header 00893 buff.write(_struct_I.pack(_v24.seq)) 00894 _v25 = _v24.stamp 00895 _x = _v25 00896 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00897 _x = _v24.frame_id 00898 length = len(_x) 00899 buff.write(struct.pack('<I%ss'%length, length, _x)) 00900 _x = _v23 00901 buff.write(_struct_2I.pack(_x.height, _x.width)) 00902 _x = _v23.encoding 00903 length = len(_x) 00904 buff.write(struct.pack('<I%ss'%length, length, _x)) 00905 _x = _v23 00906 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00907 _x = _v23.data 00908 length = len(_x) 00909 # - if encoded as a list instead, serialize as bytes instead of string 00910 if type(_x) in [list, tuple]: 00911 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00912 else: 00913 buff.write(struct.pack('<I%ss'%length, length, _x)) 00914 _v26 = _v16.cam_info 00915 _v27 = _v26.header 00916 buff.write(_struct_I.pack(_v27.seq)) 00917 _v28 = _v27.stamp 00918 _x = _v28 00919 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00920 _x = _v27.frame_id 00921 length = len(_x) 00922 buff.write(struct.pack('<I%ss'%length, length, _x)) 00923 _x = _v26 00924 buff.write(_struct_2I.pack(_x.height, _x.width)) 00925 _x = _v26.distortion_model 00926 length = len(_x) 00927 buff.write(struct.pack('<I%ss'%length, length, _x)) 00928 length = len(_v26.D) 00929 buff.write(_struct_I.pack(length)) 00930 pattern = '<%sd'%length 00931 buff.write(struct.pack(pattern, *_v26.D)) 00932 buff.write(_struct_9d.pack(*_v26.K)) 00933 buff.write(_struct_9d.pack(*_v26.R)) 00934 buff.write(_struct_12d.pack(*_v26.P)) 00935 _x = _v26 00936 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00937 _v29 = _v26.roi 00938 _x = _v29 00939 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00940 _v30 = _v16.roi_box_pose 00941 _v31 = _v30.header 00942 buff.write(_struct_I.pack(_v31.seq)) 00943 _v32 = _v31.stamp 00944 _x = _v32 00945 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00946 _x = _v31.frame_id 00947 length = len(_x) 00948 buff.write(struct.pack('<I%ss'%length, length, _x)) 00949 _v33 = _v30.pose 00950 _v34 = _v33.position 00951 _x = _v34 00952 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00953 _v35 = _v33.orientation 00954 _x = _v35 00955 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00956 _v36 = _v16.roi_box_dims 00957 _x = _v36 00958 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00959 _x = val1.collision_name 00960 length = len(_x) 00961 buff.write(struct.pack('<I%ss'%length, length, _x)) 00962 except struct.error as se: self._check_types(se) 00963 except TypeError as te: self._check_types(te) 00964 00965 def deserialize(self, str): 00966 """ 00967 unpack serialized message in str into this message instance 00968 @param str: byte array of serialized message 00969 @type str: str 00970 """ 00971 try: 00972 if self.header is None: 00973 self.header = std_msgs.msg._Header.Header() 00974 if self.goal_id is None: 00975 self.goal_id = actionlib_msgs.msg.GoalID() 00976 if self.goal is None: 00977 self.goal = pr2_object_manipulation_msgs.msg.GetGripperPoseGoal() 00978 end = 0 00979 _x = self 00980 start = end 00981 end += 12 00982 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00983 start = end 00984 end += 4 00985 (length,) = _struct_I.unpack(str[start:end]) 00986 start = end 00987 end += length 00988 self.header.frame_id = str[start:end] 00989 _x = self 00990 start = end 00991 end += 8 00992 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00993 start = end 00994 end += 4 00995 (length,) = _struct_I.unpack(str[start:end]) 00996 start = end 00997 end += length 00998 self.goal_id.id = str[start:end] 00999 start = end 01000 end += 4 01001 (length,) = _struct_I.unpack(str[start:end]) 01002 start = end 01003 end += length 01004 self.goal.arm_name = str[start:end] 01005 _x = self 01006 start = end 01007 end += 12 01008 (_x.goal.gripper_pose.header.seq, _x.goal.gripper_pose.header.stamp.secs, _x.goal.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01009 start = end 01010 end += 4 01011 (length,) = _struct_I.unpack(str[start:end]) 01012 start = end 01013 end += length 01014 self.goal.gripper_pose.header.frame_id = str[start:end] 01015 _x = self 01016 start = end 01017 end += 60 01018 (_x.goal.gripper_pose.pose.position.x, _x.goal.gripper_pose.pose.position.y, _x.goal.gripper_pose.pose.position.z, _x.goal.gripper_pose.pose.orientation.x, _x.goal.gripper_pose.pose.orientation.y, _x.goal.gripper_pose.pose.orientation.z, _x.goal.gripper_pose.pose.orientation.w, _x.goal.gripper_opening,) = _struct_7df.unpack(str[start:end]) 01019 start = end 01020 end += 4 01021 (length,) = _struct_I.unpack(str[start:end]) 01022 start = end 01023 end += length 01024 self.goal.object.reference_frame_id = str[start:end] 01025 start = end 01026 end += 4 01027 (length,) = _struct_I.unpack(str[start:end]) 01028 self.goal.object.potential_models = [] 01029 for i in range(0, length): 01030 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 01031 start = end 01032 end += 4 01033 (val1.model_id,) = _struct_i.unpack(str[start:end]) 01034 _v37 = val1.pose 01035 _v38 = _v37.header 01036 start = end 01037 end += 4 01038 (_v38.seq,) = _struct_I.unpack(str[start:end]) 01039 _v39 = _v38.stamp 01040 _x = _v39 01041 start = end 01042 end += 8 01043 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01044 start = end 01045 end += 4 01046 (length,) = _struct_I.unpack(str[start:end]) 01047 start = end 01048 end += length 01049 _v38.frame_id = str[start:end] 01050 _v40 = _v37.pose 01051 _v41 = _v40.position 01052 _x = _v41 01053 start = end 01054 end += 24 01055 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01056 _v42 = _v40.orientation 01057 _x = _v42 01058 start = end 01059 end += 32 01060 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01061 start = end 01062 end += 4 01063 (val1.confidence,) = _struct_f.unpack(str[start:end]) 01064 start = end 01065 end += 4 01066 (length,) = _struct_I.unpack(str[start:end]) 01067 start = end 01068 end += length 01069 val1.detector_name = str[start:end] 01070 self.goal.object.potential_models.append(val1) 01071 _x = self 01072 start = end 01073 end += 12 01074 (_x.goal.object.cluster.header.seq, _x.goal.object.cluster.header.stamp.secs, _x.goal.object.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01075 start = end 01076 end += 4 01077 (length,) = _struct_I.unpack(str[start:end]) 01078 start = end 01079 end += length 01080 self.goal.object.cluster.header.frame_id = str[start:end] 01081 start = end 01082 end += 4 01083 (length,) = _struct_I.unpack(str[start:end]) 01084 self.goal.object.cluster.points = [] 01085 for i in range(0, length): 01086 val1 = geometry_msgs.msg.Point32() 01087 _x = val1 01088 start = end 01089 end += 12 01090 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01091 self.goal.object.cluster.points.append(val1) 01092 start = end 01093 end += 4 01094 (length,) = _struct_I.unpack(str[start:end]) 01095 self.goal.object.cluster.channels = [] 01096 for i in range(0, length): 01097 val1 = sensor_msgs.msg.ChannelFloat32() 01098 start = end 01099 end += 4 01100 (length,) = _struct_I.unpack(str[start:end]) 01101 start = end 01102 end += length 01103 val1.name = str[start:end] 01104 start = end 01105 end += 4 01106 (length,) = _struct_I.unpack(str[start:end]) 01107 pattern = '<%sf'%length 01108 start = end 01109 end += struct.calcsize(pattern) 01110 val1.values = struct.unpack(pattern, str[start:end]) 01111 self.goal.object.cluster.channels.append(val1) 01112 _x = self 01113 start = end 01114 end += 12 01115 (_x.goal.object.region.cloud.header.seq, _x.goal.object.region.cloud.header.stamp.secs, _x.goal.object.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01116 start = end 01117 end += 4 01118 (length,) = _struct_I.unpack(str[start:end]) 01119 start = end 01120 end += length 01121 self.goal.object.region.cloud.header.frame_id = str[start:end] 01122 _x = self 01123 start = end 01124 end += 8 01125 (_x.goal.object.region.cloud.height, _x.goal.object.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 01126 start = end 01127 end += 4 01128 (length,) = _struct_I.unpack(str[start:end]) 01129 self.goal.object.region.cloud.fields = [] 01130 for i in range(0, length): 01131 val1 = sensor_msgs.msg.PointField() 01132 start = end 01133 end += 4 01134 (length,) = _struct_I.unpack(str[start:end]) 01135 start = end 01136 end += length 01137 val1.name = str[start:end] 01138 _x = val1 01139 start = end 01140 end += 9 01141 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01142 self.goal.object.region.cloud.fields.append(val1) 01143 _x = self 01144 start = end 01145 end += 9 01146 (_x.goal.object.region.cloud.is_bigendian, _x.goal.object.region.cloud.point_step, _x.goal.object.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 01147 self.goal.object.region.cloud.is_bigendian = bool(self.goal.object.region.cloud.is_bigendian) 01148 start = end 01149 end += 4 01150 (length,) = _struct_I.unpack(str[start:end]) 01151 start = end 01152 end += length 01153 self.goal.object.region.cloud.data = str[start:end] 01154 start = end 01155 end += 1 01156 (self.goal.object.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 01157 self.goal.object.region.cloud.is_dense = bool(self.goal.object.region.cloud.is_dense) 01158 start = end 01159 end += 4 01160 (length,) = _struct_I.unpack(str[start:end]) 01161 pattern = '<%si'%length 01162 start = end 01163 end += struct.calcsize(pattern) 01164 self.goal.object.region.mask = struct.unpack(pattern, str[start:end]) 01165 _x = self 01166 start = end 01167 end += 12 01168 (_x.goal.object.region.image.header.seq, _x.goal.object.region.image.header.stamp.secs, _x.goal.object.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01169 start = end 01170 end += 4 01171 (length,) = _struct_I.unpack(str[start:end]) 01172 start = end 01173 end += length 01174 self.goal.object.region.image.header.frame_id = str[start:end] 01175 _x = self 01176 start = end 01177 end += 8 01178 (_x.goal.object.region.image.height, _x.goal.object.region.image.width,) = _struct_2I.unpack(str[start:end]) 01179 start = end 01180 end += 4 01181 (length,) = _struct_I.unpack(str[start:end]) 01182 start = end 01183 end += length 01184 self.goal.object.region.image.encoding = str[start:end] 01185 _x = self 01186 start = end 01187 end += 5 01188 (_x.goal.object.region.image.is_bigendian, _x.goal.object.region.image.step,) = _struct_BI.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.goal.object.region.image.data = str[start:end] 01195 _x = self 01196 start = end 01197 end += 12 01198 (_x.goal.object.region.disparity_image.header.seq, _x.goal.object.region.disparity_image.header.stamp.secs, _x.goal.object.region.disparity_image.header.stamp.nsecs,) = _struct_3I.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.object.region.disparity_image.header.frame_id = str[start:end] 01205 _x = self 01206 start = end 01207 end += 8 01208 (_x.goal.object.region.disparity_image.height, _x.goal.object.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 01209 start = end 01210 end += 4 01211 (length,) = _struct_I.unpack(str[start:end]) 01212 start = end 01213 end += length 01214 self.goal.object.region.disparity_image.encoding = str[start:end] 01215 _x = self 01216 start = end 01217 end += 5 01218 (_x.goal.object.region.disparity_image.is_bigendian, _x.goal.object.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 01219 start = end 01220 end += 4 01221 (length,) = _struct_I.unpack(str[start:end]) 01222 start = end 01223 end += length 01224 self.goal.object.region.disparity_image.data = str[start:end] 01225 _x = self 01226 start = end 01227 end += 12 01228 (_x.goal.object.region.cam_info.header.seq, _x.goal.object.region.cam_info.header.stamp.secs, _x.goal.object.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01229 start = end 01230 end += 4 01231 (length,) = _struct_I.unpack(str[start:end]) 01232 start = end 01233 end += length 01234 self.goal.object.region.cam_info.header.frame_id = str[start:end] 01235 _x = self 01236 start = end 01237 end += 8 01238 (_x.goal.object.region.cam_info.height, _x.goal.object.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 01239 start = end 01240 end += 4 01241 (length,) = _struct_I.unpack(str[start:end]) 01242 start = end 01243 end += length 01244 self.goal.object.region.cam_info.distortion_model = str[start:end] 01245 start = end 01246 end += 4 01247 (length,) = _struct_I.unpack(str[start:end]) 01248 pattern = '<%sd'%length 01249 start = end 01250 end += struct.calcsize(pattern) 01251 self.goal.object.region.cam_info.D = struct.unpack(pattern, str[start:end]) 01252 start = end 01253 end += 72 01254 self.goal.object.region.cam_info.K = _struct_9d.unpack(str[start:end]) 01255 start = end 01256 end += 72 01257 self.goal.object.region.cam_info.R = _struct_9d.unpack(str[start:end]) 01258 start = end 01259 end += 96 01260 self.goal.object.region.cam_info.P = _struct_12d.unpack(str[start:end]) 01261 _x = self 01262 start = end 01263 end += 37 01264 (_x.goal.object.region.cam_info.binning_x, _x.goal.object.region.cam_info.binning_y, _x.goal.object.region.cam_info.roi.x_offset, _x.goal.object.region.cam_info.roi.y_offset, _x.goal.object.region.cam_info.roi.height, _x.goal.object.region.cam_info.roi.width, _x.goal.object.region.cam_info.roi.do_rectify, _x.goal.object.region.roi_box_pose.header.seq, _x.goal.object.region.roi_box_pose.header.stamp.secs, _x.goal.object.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 01265 self.goal.object.region.cam_info.roi.do_rectify = bool(self.goal.object.region.cam_info.roi.do_rectify) 01266 start = end 01267 end += 4 01268 (length,) = _struct_I.unpack(str[start:end]) 01269 start = end 01270 end += length 01271 self.goal.object.region.roi_box_pose.header.frame_id = str[start:end] 01272 _x = self 01273 start = end 01274 end += 80 01275 (_x.goal.object.region.roi_box_pose.pose.position.x, _x.goal.object.region.roi_box_pose.pose.position.y, _x.goal.object.region.roi_box_pose.pose.position.z, _x.goal.object.region.roi_box_pose.pose.orientation.x, _x.goal.object.region.roi_box_pose.pose.orientation.y, _x.goal.object.region.roi_box_pose.pose.orientation.z, _x.goal.object.region.roi_box_pose.pose.orientation.w, _x.goal.object.region.roi_box_dims.x, _x.goal.object.region.roi_box_dims.y, _x.goal.object.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 01276 start = end 01277 end += 4 01278 (length,) = _struct_I.unpack(str[start:end]) 01279 start = end 01280 end += length 01281 self.goal.object.collision_name = str[start:end] 01282 _x = self 01283 start = end 01284 end += 12 01285 (_x.goal.grasp.pre_grasp_posture.header.seq, _x.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01286 start = end 01287 end += 4 01288 (length,) = _struct_I.unpack(str[start:end]) 01289 start = end 01290 end += length 01291 self.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end] 01292 start = end 01293 end += 4 01294 (length,) = _struct_I.unpack(str[start:end]) 01295 self.goal.grasp.pre_grasp_posture.name = [] 01296 for i in range(0, length): 01297 start = end 01298 end += 4 01299 (length,) = _struct_I.unpack(str[start:end]) 01300 start = end 01301 end += length 01302 val1 = str[start:end] 01303 self.goal.grasp.pre_grasp_posture.name.append(val1) 01304 start = end 01305 end += 4 01306 (length,) = _struct_I.unpack(str[start:end]) 01307 pattern = '<%sd'%length 01308 start = end 01309 end += struct.calcsize(pattern) 01310 self.goal.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end]) 01311 start = end 01312 end += 4 01313 (length,) = _struct_I.unpack(str[start:end]) 01314 pattern = '<%sd'%length 01315 start = end 01316 end += struct.calcsize(pattern) 01317 self.goal.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 01318 start = end 01319 end += 4 01320 (length,) = _struct_I.unpack(str[start:end]) 01321 pattern = '<%sd'%length 01322 start = end 01323 end += struct.calcsize(pattern) 01324 self.goal.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end]) 01325 _x = self 01326 start = end 01327 end += 12 01328 (_x.goal.grasp.grasp_posture.header.seq, _x.goal.grasp.grasp_posture.header.stamp.secs, _x.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01329 start = end 01330 end += 4 01331 (length,) = _struct_I.unpack(str[start:end]) 01332 start = end 01333 end += length 01334 self.goal.grasp.grasp_posture.header.frame_id = str[start:end] 01335 start = end 01336 end += 4 01337 (length,) = _struct_I.unpack(str[start:end]) 01338 self.goal.grasp.grasp_posture.name = [] 01339 for i in range(0, length): 01340 start = end 01341 end += 4 01342 (length,) = _struct_I.unpack(str[start:end]) 01343 start = end 01344 end += length 01345 val1 = str[start:end] 01346 self.goal.grasp.grasp_posture.name.append(val1) 01347 start = end 01348 end += 4 01349 (length,) = _struct_I.unpack(str[start:end]) 01350 pattern = '<%sd'%length 01351 start = end 01352 end += struct.calcsize(pattern) 01353 self.goal.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end]) 01354 start = end 01355 end += 4 01356 (length,) = _struct_I.unpack(str[start:end]) 01357 pattern = '<%sd'%length 01358 start = end 01359 end += struct.calcsize(pattern) 01360 self.goal.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 01361 start = end 01362 end += 4 01363 (length,) = _struct_I.unpack(str[start:end]) 01364 pattern = '<%sd'%length 01365 start = end 01366 end += struct.calcsize(pattern) 01367 self.goal.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end]) 01368 _x = self 01369 start = end 01370 end += 73 01371 (_x.goal.grasp.grasp_pose.position.x, _x.goal.grasp.grasp_pose.position.y, _x.goal.grasp.grasp_pose.position.z, _x.goal.grasp.grasp_pose.orientation.x, _x.goal.grasp.grasp_pose.orientation.y, _x.goal.grasp.grasp_pose.orientation.z, _x.goal.grasp.grasp_pose.orientation.w, _x.goal.grasp.success_probability, _x.goal.grasp.cluster_rep, _x.goal.grasp.desired_approach_distance, _x.goal.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end]) 01372 self.goal.grasp.cluster_rep = bool(self.goal.grasp.cluster_rep) 01373 start = end 01374 end += 4 01375 (length,) = _struct_I.unpack(str[start:end]) 01376 self.goal.grasp.moved_obstacles = [] 01377 for i in range(0, length): 01378 val1 = object_manipulation_msgs.msg.GraspableObject() 01379 start = end 01380 end += 4 01381 (length,) = _struct_I.unpack(str[start:end]) 01382 start = end 01383 end += length 01384 val1.reference_frame_id = str[start:end] 01385 start = end 01386 end += 4 01387 (length,) = _struct_I.unpack(str[start:end]) 01388 val1.potential_models = [] 01389 for i in range(0, length): 01390 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 01391 start = end 01392 end += 4 01393 (val2.model_id,) = _struct_i.unpack(str[start:end]) 01394 _v43 = val2.pose 01395 _v44 = _v43.header 01396 start = end 01397 end += 4 01398 (_v44.seq,) = _struct_I.unpack(str[start:end]) 01399 _v45 = _v44.stamp 01400 _x = _v45 01401 start = end 01402 end += 8 01403 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01404 start = end 01405 end += 4 01406 (length,) = _struct_I.unpack(str[start:end]) 01407 start = end 01408 end += length 01409 _v44.frame_id = str[start:end] 01410 _v46 = _v43.pose 01411 _v47 = _v46.position 01412 _x = _v47 01413 start = end 01414 end += 24 01415 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01416 _v48 = _v46.orientation 01417 _x = _v48 01418 start = end 01419 end += 32 01420 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01421 start = end 01422 end += 4 01423 (val2.confidence,) = _struct_f.unpack(str[start:end]) 01424 start = end 01425 end += 4 01426 (length,) = _struct_I.unpack(str[start:end]) 01427 start = end 01428 end += length 01429 val2.detector_name = str[start:end] 01430 val1.potential_models.append(val2) 01431 _v49 = val1.cluster 01432 _v50 = _v49.header 01433 start = end 01434 end += 4 01435 (_v50.seq,) = _struct_I.unpack(str[start:end]) 01436 _v51 = _v50.stamp 01437 _x = _v51 01438 start = end 01439 end += 8 01440 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01441 start = end 01442 end += 4 01443 (length,) = _struct_I.unpack(str[start:end]) 01444 start = end 01445 end += length 01446 _v50.frame_id = str[start:end] 01447 start = end 01448 end += 4 01449 (length,) = _struct_I.unpack(str[start:end]) 01450 _v49.points = [] 01451 for i in range(0, length): 01452 val3 = geometry_msgs.msg.Point32() 01453 _x = val3 01454 start = end 01455 end += 12 01456 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01457 _v49.points.append(val3) 01458 start = end 01459 end += 4 01460 (length,) = _struct_I.unpack(str[start:end]) 01461 _v49.channels = [] 01462 for i in range(0, length): 01463 val3 = sensor_msgs.msg.ChannelFloat32() 01464 start = end 01465 end += 4 01466 (length,) = _struct_I.unpack(str[start:end]) 01467 start = end 01468 end += length 01469 val3.name = str[start:end] 01470 start = end 01471 end += 4 01472 (length,) = _struct_I.unpack(str[start:end]) 01473 pattern = '<%sf'%length 01474 start = end 01475 end += struct.calcsize(pattern) 01476 val3.values = struct.unpack(pattern, str[start:end]) 01477 _v49.channels.append(val3) 01478 _v52 = val1.region 01479 _v53 = _v52.cloud 01480 _v54 = _v53.header 01481 start = end 01482 end += 4 01483 (_v54.seq,) = _struct_I.unpack(str[start:end]) 01484 _v55 = _v54.stamp 01485 _x = _v55 01486 start = end 01487 end += 8 01488 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01489 start = end 01490 end += 4 01491 (length,) = _struct_I.unpack(str[start:end]) 01492 start = end 01493 end += length 01494 _v54.frame_id = str[start:end] 01495 _x = _v53 01496 start = end 01497 end += 8 01498 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01499 start = end 01500 end += 4 01501 (length,) = _struct_I.unpack(str[start:end]) 01502 _v53.fields = [] 01503 for i in range(0, length): 01504 val4 = sensor_msgs.msg.PointField() 01505 start = end 01506 end += 4 01507 (length,) = _struct_I.unpack(str[start:end]) 01508 start = end 01509 end += length 01510 val4.name = str[start:end] 01511 _x = val4 01512 start = end 01513 end += 9 01514 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01515 _v53.fields.append(val4) 01516 _x = _v53 01517 start = end 01518 end += 9 01519 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01520 _v53.is_bigendian = bool(_v53.is_bigendian) 01521 start = end 01522 end += 4 01523 (length,) = _struct_I.unpack(str[start:end]) 01524 start = end 01525 end += length 01526 _v53.data = str[start:end] 01527 start = end 01528 end += 1 01529 (_v53.is_dense,) = _struct_B.unpack(str[start:end]) 01530 _v53.is_dense = bool(_v53.is_dense) 01531 start = end 01532 end += 4 01533 (length,) = _struct_I.unpack(str[start:end]) 01534 pattern = '<%si'%length 01535 start = end 01536 end += struct.calcsize(pattern) 01537 _v52.mask = struct.unpack(pattern, str[start:end]) 01538 _v56 = _v52.image 01539 _v57 = _v56.header 01540 start = end 01541 end += 4 01542 (_v57.seq,) = _struct_I.unpack(str[start:end]) 01543 _v58 = _v57.stamp 01544 _x = _v58 01545 start = end 01546 end += 8 01547 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01548 start = end 01549 end += 4 01550 (length,) = _struct_I.unpack(str[start:end]) 01551 start = end 01552 end += length 01553 _v57.frame_id = str[start:end] 01554 _x = _v56 01555 start = end 01556 end += 8 01557 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01558 start = end 01559 end += 4 01560 (length,) = _struct_I.unpack(str[start:end]) 01561 start = end 01562 end += length 01563 _v56.encoding = str[start:end] 01564 _x = _v56 01565 start = end 01566 end += 5 01567 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01568 start = end 01569 end += 4 01570 (length,) = _struct_I.unpack(str[start:end]) 01571 start = end 01572 end += length 01573 _v56.data = str[start:end] 01574 _v59 = _v52.disparity_image 01575 _v60 = _v59.header 01576 start = end 01577 end += 4 01578 (_v60.seq,) = _struct_I.unpack(str[start:end]) 01579 _v61 = _v60.stamp 01580 _x = _v61 01581 start = end 01582 end += 8 01583 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01584 start = end 01585 end += 4 01586 (length,) = _struct_I.unpack(str[start:end]) 01587 start = end 01588 end += length 01589 _v60.frame_id = str[start:end] 01590 _x = _v59 01591 start = end 01592 end += 8 01593 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01594 start = end 01595 end += 4 01596 (length,) = _struct_I.unpack(str[start:end]) 01597 start = end 01598 end += length 01599 _v59.encoding = str[start:end] 01600 _x = _v59 01601 start = end 01602 end += 5 01603 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01604 start = end 01605 end += 4 01606 (length,) = _struct_I.unpack(str[start:end]) 01607 start = end 01608 end += length 01609 _v59.data = str[start:end] 01610 _v62 = _v52.cam_info 01611 _v63 = _v62.header 01612 start = end 01613 end += 4 01614 (_v63.seq,) = _struct_I.unpack(str[start:end]) 01615 _v64 = _v63.stamp 01616 _x = _v64 01617 start = end 01618 end += 8 01619 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01620 start = end 01621 end += 4 01622 (length,) = _struct_I.unpack(str[start:end]) 01623 start = end 01624 end += length 01625 _v63.frame_id = str[start:end] 01626 _x = _v62 01627 start = end 01628 end += 8 01629 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01630 start = end 01631 end += 4 01632 (length,) = _struct_I.unpack(str[start:end]) 01633 start = end 01634 end += length 01635 _v62.distortion_model = str[start:end] 01636 start = end 01637 end += 4 01638 (length,) = _struct_I.unpack(str[start:end]) 01639 pattern = '<%sd'%length 01640 start = end 01641 end += struct.calcsize(pattern) 01642 _v62.D = struct.unpack(pattern, str[start:end]) 01643 start = end 01644 end += 72 01645 _v62.K = _struct_9d.unpack(str[start:end]) 01646 start = end 01647 end += 72 01648 _v62.R = _struct_9d.unpack(str[start:end]) 01649 start = end 01650 end += 96 01651 _v62.P = _struct_12d.unpack(str[start:end]) 01652 _x = _v62 01653 start = end 01654 end += 8 01655 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01656 _v65 = _v62.roi 01657 _x = _v65 01658 start = end 01659 end += 17 01660 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01661 _v65.do_rectify = bool(_v65.do_rectify) 01662 _v66 = _v52.roi_box_pose 01663 _v67 = _v66.header 01664 start = end 01665 end += 4 01666 (_v67.seq,) = _struct_I.unpack(str[start:end]) 01667 _v68 = _v67.stamp 01668 _x = _v68 01669 start = end 01670 end += 8 01671 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01672 start = end 01673 end += 4 01674 (length,) = _struct_I.unpack(str[start:end]) 01675 start = end 01676 end += length 01677 _v67.frame_id = str[start:end] 01678 _v69 = _v66.pose 01679 _v70 = _v69.position 01680 _x = _v70 01681 start = end 01682 end += 24 01683 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01684 _v71 = _v69.orientation 01685 _x = _v71 01686 start = end 01687 end += 32 01688 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01689 _v72 = _v52.roi_box_dims 01690 _x = _v72 01691 start = end 01692 end += 24 01693 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01694 start = end 01695 end += 4 01696 (length,) = _struct_I.unpack(str[start:end]) 01697 start = end 01698 end += length 01699 val1.collision_name = str[start:end] 01700 self.goal.grasp.moved_obstacles.append(val1) 01701 return self 01702 except struct.error as e: 01703 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01704 01705 01706 def serialize_numpy(self, buff, numpy): 01707 """ 01708 serialize message with numpy array types into buffer 01709 @param buff: buffer 01710 @type buff: StringIO 01711 @param numpy: numpy python module 01712 @type numpy module 01713 """ 01714 try: 01715 _x = self 01716 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 01717 _x = self.header.frame_id 01718 length = len(_x) 01719 buff.write(struct.pack('<I%ss'%length, length, _x)) 01720 _x = self 01721 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs)) 01722 _x = self.goal_id.id 01723 length = len(_x) 01724 buff.write(struct.pack('<I%ss'%length, length, _x)) 01725 _x = self.goal.arm_name 01726 length = len(_x) 01727 buff.write(struct.pack('<I%ss'%length, length, _x)) 01728 _x = self 01729 buff.write(_struct_3I.pack(_x.goal.gripper_pose.header.seq, _x.goal.gripper_pose.header.stamp.secs, _x.goal.gripper_pose.header.stamp.nsecs)) 01730 _x = self.goal.gripper_pose.header.frame_id 01731 length = len(_x) 01732 buff.write(struct.pack('<I%ss'%length, length, _x)) 01733 _x = self 01734 buff.write(_struct_7df.pack(_x.goal.gripper_pose.pose.position.x, _x.goal.gripper_pose.pose.position.y, _x.goal.gripper_pose.pose.position.z, _x.goal.gripper_pose.pose.orientation.x, _x.goal.gripper_pose.pose.orientation.y, _x.goal.gripper_pose.pose.orientation.z, _x.goal.gripper_pose.pose.orientation.w, _x.goal.gripper_opening)) 01735 _x = self.goal.object.reference_frame_id 01736 length = len(_x) 01737 buff.write(struct.pack('<I%ss'%length, length, _x)) 01738 length = len(self.goal.object.potential_models) 01739 buff.write(_struct_I.pack(length)) 01740 for val1 in self.goal.object.potential_models: 01741 buff.write(_struct_i.pack(val1.model_id)) 01742 _v73 = val1.pose 01743 _v74 = _v73.header 01744 buff.write(_struct_I.pack(_v74.seq)) 01745 _v75 = _v74.stamp 01746 _x = _v75 01747 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01748 _x = _v74.frame_id 01749 length = len(_x) 01750 buff.write(struct.pack('<I%ss'%length, length, _x)) 01751 _v76 = _v73.pose 01752 _v77 = _v76.position 01753 _x = _v77 01754 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01755 _v78 = _v76.orientation 01756 _x = _v78 01757 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01758 buff.write(_struct_f.pack(val1.confidence)) 01759 _x = val1.detector_name 01760 length = len(_x) 01761 buff.write(struct.pack('<I%ss'%length, length, _x)) 01762 _x = self 01763 buff.write(_struct_3I.pack(_x.goal.object.cluster.header.seq, _x.goal.object.cluster.header.stamp.secs, _x.goal.object.cluster.header.stamp.nsecs)) 01764 _x = self.goal.object.cluster.header.frame_id 01765 length = len(_x) 01766 buff.write(struct.pack('<I%ss'%length, length, _x)) 01767 length = len(self.goal.object.cluster.points) 01768 buff.write(_struct_I.pack(length)) 01769 for val1 in self.goal.object.cluster.points: 01770 _x = val1 01771 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01772 length = len(self.goal.object.cluster.channels) 01773 buff.write(_struct_I.pack(length)) 01774 for val1 in self.goal.object.cluster.channels: 01775 _x = val1.name 01776 length = len(_x) 01777 buff.write(struct.pack('<I%ss'%length, length, _x)) 01778 length = len(val1.values) 01779 buff.write(_struct_I.pack(length)) 01780 pattern = '<%sf'%length 01781 buff.write(val1.values.tostring()) 01782 _x = self 01783 buff.write(_struct_3I.pack(_x.goal.object.region.cloud.header.seq, _x.goal.object.region.cloud.header.stamp.secs, _x.goal.object.region.cloud.header.stamp.nsecs)) 01784 _x = self.goal.object.region.cloud.header.frame_id 01785 length = len(_x) 01786 buff.write(struct.pack('<I%ss'%length, length, _x)) 01787 _x = self 01788 buff.write(_struct_2I.pack(_x.goal.object.region.cloud.height, _x.goal.object.region.cloud.width)) 01789 length = len(self.goal.object.region.cloud.fields) 01790 buff.write(_struct_I.pack(length)) 01791 for val1 in self.goal.object.region.cloud.fields: 01792 _x = val1.name 01793 length = len(_x) 01794 buff.write(struct.pack('<I%ss'%length, length, _x)) 01795 _x = val1 01796 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01797 _x = self 01798 buff.write(_struct_B2I.pack(_x.goal.object.region.cloud.is_bigendian, _x.goal.object.region.cloud.point_step, _x.goal.object.region.cloud.row_step)) 01799 _x = self.goal.object.region.cloud.data 01800 length = len(_x) 01801 # - if encoded as a list instead, serialize as bytes instead of string 01802 if type(_x) in [list, tuple]: 01803 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01804 else: 01805 buff.write(struct.pack('<I%ss'%length, length, _x)) 01806 buff.write(_struct_B.pack(self.goal.object.region.cloud.is_dense)) 01807 length = len(self.goal.object.region.mask) 01808 buff.write(_struct_I.pack(length)) 01809 pattern = '<%si'%length 01810 buff.write(self.goal.object.region.mask.tostring()) 01811 _x = self 01812 buff.write(_struct_3I.pack(_x.goal.object.region.image.header.seq, _x.goal.object.region.image.header.stamp.secs, _x.goal.object.region.image.header.stamp.nsecs)) 01813 _x = self.goal.object.region.image.header.frame_id 01814 length = len(_x) 01815 buff.write(struct.pack('<I%ss'%length, length, _x)) 01816 _x = self 01817 buff.write(_struct_2I.pack(_x.goal.object.region.image.height, _x.goal.object.region.image.width)) 01818 _x = self.goal.object.region.image.encoding 01819 length = len(_x) 01820 buff.write(struct.pack('<I%ss'%length, length, _x)) 01821 _x = self 01822 buff.write(_struct_BI.pack(_x.goal.object.region.image.is_bigendian, _x.goal.object.region.image.step)) 01823 _x = self.goal.object.region.image.data 01824 length = len(_x) 01825 # - if encoded as a list instead, serialize as bytes instead of string 01826 if type(_x) in [list, tuple]: 01827 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01828 else: 01829 buff.write(struct.pack('<I%ss'%length, length, _x)) 01830 _x = self 01831 buff.write(_struct_3I.pack(_x.goal.object.region.disparity_image.header.seq, _x.goal.object.region.disparity_image.header.stamp.secs, _x.goal.object.region.disparity_image.header.stamp.nsecs)) 01832 _x = self.goal.object.region.disparity_image.header.frame_id 01833 length = len(_x) 01834 buff.write(struct.pack('<I%ss'%length, length, _x)) 01835 _x = self 01836 buff.write(_struct_2I.pack(_x.goal.object.region.disparity_image.height, _x.goal.object.region.disparity_image.width)) 01837 _x = self.goal.object.region.disparity_image.encoding 01838 length = len(_x) 01839 buff.write(struct.pack('<I%ss'%length, length, _x)) 01840 _x = self 01841 buff.write(_struct_BI.pack(_x.goal.object.region.disparity_image.is_bigendian, _x.goal.object.region.disparity_image.step)) 01842 _x = self.goal.object.region.disparity_image.data 01843 length = len(_x) 01844 # - if encoded as a list instead, serialize as bytes instead of string 01845 if type(_x) in [list, tuple]: 01846 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01847 else: 01848 buff.write(struct.pack('<I%ss'%length, length, _x)) 01849 _x = self 01850 buff.write(_struct_3I.pack(_x.goal.object.region.cam_info.header.seq, _x.goal.object.region.cam_info.header.stamp.secs, _x.goal.object.region.cam_info.header.stamp.nsecs)) 01851 _x = self.goal.object.region.cam_info.header.frame_id 01852 length = len(_x) 01853 buff.write(struct.pack('<I%ss'%length, length, _x)) 01854 _x = self 01855 buff.write(_struct_2I.pack(_x.goal.object.region.cam_info.height, _x.goal.object.region.cam_info.width)) 01856 _x = self.goal.object.region.cam_info.distortion_model 01857 length = len(_x) 01858 buff.write(struct.pack('<I%ss'%length, length, _x)) 01859 length = len(self.goal.object.region.cam_info.D) 01860 buff.write(_struct_I.pack(length)) 01861 pattern = '<%sd'%length 01862 buff.write(self.goal.object.region.cam_info.D.tostring()) 01863 buff.write(self.goal.object.region.cam_info.K.tostring()) 01864 buff.write(self.goal.object.region.cam_info.R.tostring()) 01865 buff.write(self.goal.object.region.cam_info.P.tostring()) 01866 _x = self 01867 buff.write(_struct_6IB3I.pack(_x.goal.object.region.cam_info.binning_x, _x.goal.object.region.cam_info.binning_y, _x.goal.object.region.cam_info.roi.x_offset, _x.goal.object.region.cam_info.roi.y_offset, _x.goal.object.region.cam_info.roi.height, _x.goal.object.region.cam_info.roi.width, _x.goal.object.region.cam_info.roi.do_rectify, _x.goal.object.region.roi_box_pose.header.seq, _x.goal.object.region.roi_box_pose.header.stamp.secs, _x.goal.object.region.roi_box_pose.header.stamp.nsecs)) 01868 _x = self.goal.object.region.roi_box_pose.header.frame_id 01869 length = len(_x) 01870 buff.write(struct.pack('<I%ss'%length, length, _x)) 01871 _x = self 01872 buff.write(_struct_10d.pack(_x.goal.object.region.roi_box_pose.pose.position.x, _x.goal.object.region.roi_box_pose.pose.position.y, _x.goal.object.region.roi_box_pose.pose.position.z, _x.goal.object.region.roi_box_pose.pose.orientation.x, _x.goal.object.region.roi_box_pose.pose.orientation.y, _x.goal.object.region.roi_box_pose.pose.orientation.z, _x.goal.object.region.roi_box_pose.pose.orientation.w, _x.goal.object.region.roi_box_dims.x, _x.goal.object.region.roi_box_dims.y, _x.goal.object.region.roi_box_dims.z)) 01873 _x = self.goal.object.collision_name 01874 length = len(_x) 01875 buff.write(struct.pack('<I%ss'%length, length, _x)) 01876 _x = self 01877 buff.write(_struct_3I.pack(_x.goal.grasp.pre_grasp_posture.header.seq, _x.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.goal.grasp.pre_grasp_posture.header.stamp.nsecs)) 01878 _x = self.goal.grasp.pre_grasp_posture.header.frame_id 01879 length = len(_x) 01880 buff.write(struct.pack('<I%ss'%length, length, _x)) 01881 length = len(self.goal.grasp.pre_grasp_posture.name) 01882 buff.write(_struct_I.pack(length)) 01883 for val1 in self.goal.grasp.pre_grasp_posture.name: 01884 length = len(val1) 01885 buff.write(struct.pack('<I%ss'%length, length, val1)) 01886 length = len(self.goal.grasp.pre_grasp_posture.position) 01887 buff.write(_struct_I.pack(length)) 01888 pattern = '<%sd'%length 01889 buff.write(self.goal.grasp.pre_grasp_posture.position.tostring()) 01890 length = len(self.goal.grasp.pre_grasp_posture.velocity) 01891 buff.write(_struct_I.pack(length)) 01892 pattern = '<%sd'%length 01893 buff.write(self.goal.grasp.pre_grasp_posture.velocity.tostring()) 01894 length = len(self.goal.grasp.pre_grasp_posture.effort) 01895 buff.write(_struct_I.pack(length)) 01896 pattern = '<%sd'%length 01897 buff.write(self.goal.grasp.pre_grasp_posture.effort.tostring()) 01898 _x = self 01899 buff.write(_struct_3I.pack(_x.goal.grasp.grasp_posture.header.seq, _x.goal.grasp.grasp_posture.header.stamp.secs, _x.goal.grasp.grasp_posture.header.stamp.nsecs)) 01900 _x = self.goal.grasp.grasp_posture.header.frame_id 01901 length = len(_x) 01902 buff.write(struct.pack('<I%ss'%length, length, _x)) 01903 length = len(self.goal.grasp.grasp_posture.name) 01904 buff.write(_struct_I.pack(length)) 01905 for val1 in self.goal.grasp.grasp_posture.name: 01906 length = len(val1) 01907 buff.write(struct.pack('<I%ss'%length, length, val1)) 01908 length = len(self.goal.grasp.grasp_posture.position) 01909 buff.write(_struct_I.pack(length)) 01910 pattern = '<%sd'%length 01911 buff.write(self.goal.grasp.grasp_posture.position.tostring()) 01912 length = len(self.goal.grasp.grasp_posture.velocity) 01913 buff.write(_struct_I.pack(length)) 01914 pattern = '<%sd'%length 01915 buff.write(self.goal.grasp.grasp_posture.velocity.tostring()) 01916 length = len(self.goal.grasp.grasp_posture.effort) 01917 buff.write(_struct_I.pack(length)) 01918 pattern = '<%sd'%length 01919 buff.write(self.goal.grasp.grasp_posture.effort.tostring()) 01920 _x = self 01921 buff.write(_struct_8dB2f.pack(_x.goal.grasp.grasp_pose.position.x, _x.goal.grasp.grasp_pose.position.y, _x.goal.grasp.grasp_pose.position.z, _x.goal.grasp.grasp_pose.orientation.x, _x.goal.grasp.grasp_pose.orientation.y, _x.goal.grasp.grasp_pose.orientation.z, _x.goal.grasp.grasp_pose.orientation.w, _x.goal.grasp.success_probability, _x.goal.grasp.cluster_rep, _x.goal.grasp.desired_approach_distance, _x.goal.grasp.min_approach_distance)) 01922 length = len(self.goal.grasp.moved_obstacles) 01923 buff.write(_struct_I.pack(length)) 01924 for val1 in self.goal.grasp.moved_obstacles: 01925 _x = val1.reference_frame_id 01926 length = len(_x) 01927 buff.write(struct.pack('<I%ss'%length, length, _x)) 01928 length = len(val1.potential_models) 01929 buff.write(_struct_I.pack(length)) 01930 for val2 in val1.potential_models: 01931 buff.write(_struct_i.pack(val2.model_id)) 01932 _v79 = val2.pose 01933 _v80 = _v79.header 01934 buff.write(_struct_I.pack(_v80.seq)) 01935 _v81 = _v80.stamp 01936 _x = _v81 01937 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01938 _x = _v80.frame_id 01939 length = len(_x) 01940 buff.write(struct.pack('<I%ss'%length, length, _x)) 01941 _v82 = _v79.pose 01942 _v83 = _v82.position 01943 _x = _v83 01944 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01945 _v84 = _v82.orientation 01946 _x = _v84 01947 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01948 buff.write(_struct_f.pack(val2.confidence)) 01949 _x = val2.detector_name 01950 length = len(_x) 01951 buff.write(struct.pack('<I%ss'%length, length, _x)) 01952 _v85 = val1.cluster 01953 _v86 = _v85.header 01954 buff.write(_struct_I.pack(_v86.seq)) 01955 _v87 = _v86.stamp 01956 _x = _v87 01957 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01958 _x = _v86.frame_id 01959 length = len(_x) 01960 buff.write(struct.pack('<I%ss'%length, length, _x)) 01961 length = len(_v85.points) 01962 buff.write(_struct_I.pack(length)) 01963 for val3 in _v85.points: 01964 _x = val3 01965 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01966 length = len(_v85.channels) 01967 buff.write(_struct_I.pack(length)) 01968 for val3 in _v85.channels: 01969 _x = val3.name 01970 length = len(_x) 01971 buff.write(struct.pack('<I%ss'%length, length, _x)) 01972 length = len(val3.values) 01973 buff.write(_struct_I.pack(length)) 01974 pattern = '<%sf'%length 01975 buff.write(val3.values.tostring()) 01976 _v88 = val1.region 01977 _v89 = _v88.cloud 01978 _v90 = _v89.header 01979 buff.write(_struct_I.pack(_v90.seq)) 01980 _v91 = _v90.stamp 01981 _x = _v91 01982 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01983 _x = _v90.frame_id 01984 length = len(_x) 01985 buff.write(struct.pack('<I%ss'%length, length, _x)) 01986 _x = _v89 01987 buff.write(_struct_2I.pack(_x.height, _x.width)) 01988 length = len(_v89.fields) 01989 buff.write(_struct_I.pack(length)) 01990 for val4 in _v89.fields: 01991 _x = val4.name 01992 length = len(_x) 01993 buff.write(struct.pack('<I%ss'%length, length, _x)) 01994 _x = val4 01995 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01996 _x = _v89 01997 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01998 _x = _v89.data 01999 length = len(_x) 02000 # - if encoded as a list instead, serialize as bytes instead of string 02001 if type(_x) in [list, tuple]: 02002 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02003 else: 02004 buff.write(struct.pack('<I%ss'%length, length, _x)) 02005 buff.write(_struct_B.pack(_v89.is_dense)) 02006 length = len(_v88.mask) 02007 buff.write(_struct_I.pack(length)) 02008 pattern = '<%si'%length 02009 buff.write(_v88.mask.tostring()) 02010 _v92 = _v88.image 02011 _v93 = _v92.header 02012 buff.write(_struct_I.pack(_v93.seq)) 02013 _v94 = _v93.stamp 02014 _x = _v94 02015 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02016 _x = _v93.frame_id 02017 length = len(_x) 02018 buff.write(struct.pack('<I%ss'%length, length, _x)) 02019 _x = _v92 02020 buff.write(_struct_2I.pack(_x.height, _x.width)) 02021 _x = _v92.encoding 02022 length = len(_x) 02023 buff.write(struct.pack('<I%ss'%length, length, _x)) 02024 _x = _v92 02025 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02026 _x = _v92.data 02027 length = len(_x) 02028 # - if encoded as a list instead, serialize as bytes instead of string 02029 if type(_x) in [list, tuple]: 02030 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02031 else: 02032 buff.write(struct.pack('<I%ss'%length, length, _x)) 02033 _v95 = _v88.disparity_image 02034 _v96 = _v95.header 02035 buff.write(_struct_I.pack(_v96.seq)) 02036 _v97 = _v96.stamp 02037 _x = _v97 02038 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02039 _x = _v96.frame_id 02040 length = len(_x) 02041 buff.write(struct.pack('<I%ss'%length, length, _x)) 02042 _x = _v95 02043 buff.write(_struct_2I.pack(_x.height, _x.width)) 02044 _x = _v95.encoding 02045 length = len(_x) 02046 buff.write(struct.pack('<I%ss'%length, length, _x)) 02047 _x = _v95 02048 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02049 _x = _v95.data 02050 length = len(_x) 02051 # - if encoded as a list instead, serialize as bytes instead of string 02052 if type(_x) in [list, tuple]: 02053 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02054 else: 02055 buff.write(struct.pack('<I%ss'%length, length, _x)) 02056 _v98 = _v88.cam_info 02057 _v99 = _v98.header 02058 buff.write(_struct_I.pack(_v99.seq)) 02059 _v100 = _v99.stamp 02060 _x = _v100 02061 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02062 _x = _v99.frame_id 02063 length = len(_x) 02064 buff.write(struct.pack('<I%ss'%length, length, _x)) 02065 _x = _v98 02066 buff.write(_struct_2I.pack(_x.height, _x.width)) 02067 _x = _v98.distortion_model 02068 length = len(_x) 02069 buff.write(struct.pack('<I%ss'%length, length, _x)) 02070 length = len(_v98.D) 02071 buff.write(_struct_I.pack(length)) 02072 pattern = '<%sd'%length 02073 buff.write(_v98.D.tostring()) 02074 buff.write(_v98.K.tostring()) 02075 buff.write(_v98.R.tostring()) 02076 buff.write(_v98.P.tostring()) 02077 _x = _v98 02078 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02079 _v101 = _v98.roi 02080 _x = _v101 02081 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02082 _v102 = _v88.roi_box_pose 02083 _v103 = _v102.header 02084 buff.write(_struct_I.pack(_v103.seq)) 02085 _v104 = _v103.stamp 02086 _x = _v104 02087 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02088 _x = _v103.frame_id 02089 length = len(_x) 02090 buff.write(struct.pack('<I%ss'%length, length, _x)) 02091 _v105 = _v102.pose 02092 _v106 = _v105.position 02093 _x = _v106 02094 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02095 _v107 = _v105.orientation 02096 _x = _v107 02097 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02098 _v108 = _v88.roi_box_dims 02099 _x = _v108 02100 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02101 _x = val1.collision_name 02102 length = len(_x) 02103 buff.write(struct.pack('<I%ss'%length, length, _x)) 02104 except struct.error as se: self._check_types(se) 02105 except TypeError as te: self._check_types(te) 02106 02107 def deserialize_numpy(self, str, numpy): 02108 """ 02109 unpack serialized message in str into this message instance using numpy for array types 02110 @param str: byte array of serialized message 02111 @type str: str 02112 @param numpy: numpy python module 02113 @type numpy: module 02114 """ 02115 try: 02116 if self.header is None: 02117 self.header = std_msgs.msg._Header.Header() 02118 if self.goal_id is None: 02119 self.goal_id = actionlib_msgs.msg.GoalID() 02120 if self.goal is None: 02121 self.goal = pr2_object_manipulation_msgs.msg.GetGripperPoseGoal() 02122 end = 0 02123 _x = self 02124 start = end 02125 end += 12 02126 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02127 start = end 02128 end += 4 02129 (length,) = _struct_I.unpack(str[start:end]) 02130 start = end 02131 end += length 02132 self.header.frame_id = str[start:end] 02133 _x = self 02134 start = end 02135 end += 8 02136 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02137 start = end 02138 end += 4 02139 (length,) = _struct_I.unpack(str[start:end]) 02140 start = end 02141 end += length 02142 self.goal_id.id = str[start:end] 02143 start = end 02144 end += 4 02145 (length,) = _struct_I.unpack(str[start:end]) 02146 start = end 02147 end += length 02148 self.goal.arm_name = str[start:end] 02149 _x = self 02150 start = end 02151 end += 12 02152 (_x.goal.gripper_pose.header.seq, _x.goal.gripper_pose.header.stamp.secs, _x.goal.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02153 start = end 02154 end += 4 02155 (length,) = _struct_I.unpack(str[start:end]) 02156 start = end 02157 end += length 02158 self.goal.gripper_pose.header.frame_id = str[start:end] 02159 _x = self 02160 start = end 02161 end += 60 02162 (_x.goal.gripper_pose.pose.position.x, _x.goal.gripper_pose.pose.position.y, _x.goal.gripper_pose.pose.position.z, _x.goal.gripper_pose.pose.orientation.x, _x.goal.gripper_pose.pose.orientation.y, _x.goal.gripper_pose.pose.orientation.z, _x.goal.gripper_pose.pose.orientation.w, _x.goal.gripper_opening,) = _struct_7df.unpack(str[start:end]) 02163 start = end 02164 end += 4 02165 (length,) = _struct_I.unpack(str[start:end]) 02166 start = end 02167 end += length 02168 self.goal.object.reference_frame_id = str[start:end] 02169 start = end 02170 end += 4 02171 (length,) = _struct_I.unpack(str[start:end]) 02172 self.goal.object.potential_models = [] 02173 for i in range(0, length): 02174 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 02175 start = end 02176 end += 4 02177 (val1.model_id,) = _struct_i.unpack(str[start:end]) 02178 _v109 = val1.pose 02179 _v110 = _v109.header 02180 start = end 02181 end += 4 02182 (_v110.seq,) = _struct_I.unpack(str[start:end]) 02183 _v111 = _v110.stamp 02184 _x = _v111 02185 start = end 02186 end += 8 02187 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02188 start = end 02189 end += 4 02190 (length,) = _struct_I.unpack(str[start:end]) 02191 start = end 02192 end += length 02193 _v110.frame_id = str[start:end] 02194 _v112 = _v109.pose 02195 _v113 = _v112.position 02196 _x = _v113 02197 start = end 02198 end += 24 02199 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02200 _v114 = _v112.orientation 02201 _x = _v114 02202 start = end 02203 end += 32 02204 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02205 start = end 02206 end += 4 02207 (val1.confidence,) = _struct_f.unpack(str[start:end]) 02208 start = end 02209 end += 4 02210 (length,) = _struct_I.unpack(str[start:end]) 02211 start = end 02212 end += length 02213 val1.detector_name = str[start:end] 02214 self.goal.object.potential_models.append(val1) 02215 _x = self 02216 start = end 02217 end += 12 02218 (_x.goal.object.cluster.header.seq, _x.goal.object.cluster.header.stamp.secs, _x.goal.object.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02219 start = end 02220 end += 4 02221 (length,) = _struct_I.unpack(str[start:end]) 02222 start = end 02223 end += length 02224 self.goal.object.cluster.header.frame_id = str[start:end] 02225 start = end 02226 end += 4 02227 (length,) = _struct_I.unpack(str[start:end]) 02228 self.goal.object.cluster.points = [] 02229 for i in range(0, length): 02230 val1 = geometry_msgs.msg.Point32() 02231 _x = val1 02232 start = end 02233 end += 12 02234 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02235 self.goal.object.cluster.points.append(val1) 02236 start = end 02237 end += 4 02238 (length,) = _struct_I.unpack(str[start:end]) 02239 self.goal.object.cluster.channels = [] 02240 for i in range(0, length): 02241 val1 = sensor_msgs.msg.ChannelFloat32() 02242 start = end 02243 end += 4 02244 (length,) = _struct_I.unpack(str[start:end]) 02245 start = end 02246 end += length 02247 val1.name = str[start:end] 02248 start = end 02249 end += 4 02250 (length,) = _struct_I.unpack(str[start:end]) 02251 pattern = '<%sf'%length 02252 start = end 02253 end += struct.calcsize(pattern) 02254 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 02255 self.goal.object.cluster.channels.append(val1) 02256 _x = self 02257 start = end 02258 end += 12 02259 (_x.goal.object.region.cloud.header.seq, _x.goal.object.region.cloud.header.stamp.secs, _x.goal.object.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02260 start = end 02261 end += 4 02262 (length,) = _struct_I.unpack(str[start:end]) 02263 start = end 02264 end += length 02265 self.goal.object.region.cloud.header.frame_id = str[start:end] 02266 _x = self 02267 start = end 02268 end += 8 02269 (_x.goal.object.region.cloud.height, _x.goal.object.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 02270 start = end 02271 end += 4 02272 (length,) = _struct_I.unpack(str[start:end]) 02273 self.goal.object.region.cloud.fields = [] 02274 for i in range(0, length): 02275 val1 = sensor_msgs.msg.PointField() 02276 start = end 02277 end += 4 02278 (length,) = _struct_I.unpack(str[start:end]) 02279 start = end 02280 end += length 02281 val1.name = str[start:end] 02282 _x = val1 02283 start = end 02284 end += 9 02285 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02286 self.goal.object.region.cloud.fields.append(val1) 02287 _x = self 02288 start = end 02289 end += 9 02290 (_x.goal.object.region.cloud.is_bigendian, _x.goal.object.region.cloud.point_step, _x.goal.object.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 02291 self.goal.object.region.cloud.is_bigendian = bool(self.goal.object.region.cloud.is_bigendian) 02292 start = end 02293 end += 4 02294 (length,) = _struct_I.unpack(str[start:end]) 02295 start = end 02296 end += length 02297 self.goal.object.region.cloud.data = str[start:end] 02298 start = end 02299 end += 1 02300 (self.goal.object.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 02301 self.goal.object.region.cloud.is_dense = bool(self.goal.object.region.cloud.is_dense) 02302 start = end 02303 end += 4 02304 (length,) = _struct_I.unpack(str[start:end]) 02305 pattern = '<%si'%length 02306 start = end 02307 end += struct.calcsize(pattern) 02308 self.goal.object.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 02309 _x = self 02310 start = end 02311 end += 12 02312 (_x.goal.object.region.image.header.seq, _x.goal.object.region.image.header.stamp.secs, _x.goal.object.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02313 start = end 02314 end += 4 02315 (length,) = _struct_I.unpack(str[start:end]) 02316 start = end 02317 end += length 02318 self.goal.object.region.image.header.frame_id = str[start:end] 02319 _x = self 02320 start = end 02321 end += 8 02322 (_x.goal.object.region.image.height, _x.goal.object.region.image.width,) = _struct_2I.unpack(str[start:end]) 02323 start = end 02324 end += 4 02325 (length,) = _struct_I.unpack(str[start:end]) 02326 start = end 02327 end += length 02328 self.goal.object.region.image.encoding = str[start:end] 02329 _x = self 02330 start = end 02331 end += 5 02332 (_x.goal.object.region.image.is_bigendian, _x.goal.object.region.image.step,) = _struct_BI.unpack(str[start:end]) 02333 start = end 02334 end += 4 02335 (length,) = _struct_I.unpack(str[start:end]) 02336 start = end 02337 end += length 02338 self.goal.object.region.image.data = str[start:end] 02339 _x = self 02340 start = end 02341 end += 12 02342 (_x.goal.object.region.disparity_image.header.seq, _x.goal.object.region.disparity_image.header.stamp.secs, _x.goal.object.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02343 start = end 02344 end += 4 02345 (length,) = _struct_I.unpack(str[start:end]) 02346 start = end 02347 end += length 02348 self.goal.object.region.disparity_image.header.frame_id = str[start:end] 02349 _x = self 02350 start = end 02351 end += 8 02352 (_x.goal.object.region.disparity_image.height, _x.goal.object.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 02353 start = end 02354 end += 4 02355 (length,) = _struct_I.unpack(str[start:end]) 02356 start = end 02357 end += length 02358 self.goal.object.region.disparity_image.encoding = str[start:end] 02359 _x = self 02360 start = end 02361 end += 5 02362 (_x.goal.object.region.disparity_image.is_bigendian, _x.goal.object.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 02363 start = end 02364 end += 4 02365 (length,) = _struct_I.unpack(str[start:end]) 02366 start = end 02367 end += length 02368 self.goal.object.region.disparity_image.data = str[start:end] 02369 _x = self 02370 start = end 02371 end += 12 02372 (_x.goal.object.region.cam_info.header.seq, _x.goal.object.region.cam_info.header.stamp.secs, _x.goal.object.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02373 start = end 02374 end += 4 02375 (length,) = _struct_I.unpack(str[start:end]) 02376 start = end 02377 end += length 02378 self.goal.object.region.cam_info.header.frame_id = str[start:end] 02379 _x = self 02380 start = end 02381 end += 8 02382 (_x.goal.object.region.cam_info.height, _x.goal.object.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 02383 start = end 02384 end += 4 02385 (length,) = _struct_I.unpack(str[start:end]) 02386 start = end 02387 end += length 02388 self.goal.object.region.cam_info.distortion_model = str[start:end] 02389 start = end 02390 end += 4 02391 (length,) = _struct_I.unpack(str[start:end]) 02392 pattern = '<%sd'%length 02393 start = end 02394 end += struct.calcsize(pattern) 02395 self.goal.object.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02396 start = end 02397 end += 72 02398 self.goal.object.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02399 start = end 02400 end += 72 02401 self.goal.object.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02402 start = end 02403 end += 96 02404 self.goal.object.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 02405 _x = self 02406 start = end 02407 end += 37 02408 (_x.goal.object.region.cam_info.binning_x, _x.goal.object.region.cam_info.binning_y, _x.goal.object.region.cam_info.roi.x_offset, _x.goal.object.region.cam_info.roi.y_offset, _x.goal.object.region.cam_info.roi.height, _x.goal.object.region.cam_info.roi.width, _x.goal.object.region.cam_info.roi.do_rectify, _x.goal.object.region.roi_box_pose.header.seq, _x.goal.object.region.roi_box_pose.header.stamp.secs, _x.goal.object.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 02409 self.goal.object.region.cam_info.roi.do_rectify = bool(self.goal.object.region.cam_info.roi.do_rectify) 02410 start = end 02411 end += 4 02412 (length,) = _struct_I.unpack(str[start:end]) 02413 start = end 02414 end += length 02415 self.goal.object.region.roi_box_pose.header.frame_id = str[start:end] 02416 _x = self 02417 start = end 02418 end += 80 02419 (_x.goal.object.region.roi_box_pose.pose.position.x, _x.goal.object.region.roi_box_pose.pose.position.y, _x.goal.object.region.roi_box_pose.pose.position.z, _x.goal.object.region.roi_box_pose.pose.orientation.x, _x.goal.object.region.roi_box_pose.pose.orientation.y, _x.goal.object.region.roi_box_pose.pose.orientation.z, _x.goal.object.region.roi_box_pose.pose.orientation.w, _x.goal.object.region.roi_box_dims.x, _x.goal.object.region.roi_box_dims.y, _x.goal.object.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 02420 start = end 02421 end += 4 02422 (length,) = _struct_I.unpack(str[start:end]) 02423 start = end 02424 end += length 02425 self.goal.object.collision_name = str[start:end] 02426 _x = self 02427 start = end 02428 end += 12 02429 (_x.goal.grasp.pre_grasp_posture.header.seq, _x.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02430 start = end 02431 end += 4 02432 (length,) = _struct_I.unpack(str[start:end]) 02433 start = end 02434 end += length 02435 self.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end] 02436 start = end 02437 end += 4 02438 (length,) = _struct_I.unpack(str[start:end]) 02439 self.goal.grasp.pre_grasp_posture.name = [] 02440 for i in range(0, length): 02441 start = end 02442 end += 4 02443 (length,) = _struct_I.unpack(str[start:end]) 02444 start = end 02445 end += length 02446 val1 = str[start:end] 02447 self.goal.grasp.pre_grasp_posture.name.append(val1) 02448 start = end 02449 end += 4 02450 (length,) = _struct_I.unpack(str[start:end]) 02451 pattern = '<%sd'%length 02452 start = end 02453 end += struct.calcsize(pattern) 02454 self.goal.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02455 start = end 02456 end += 4 02457 (length,) = _struct_I.unpack(str[start:end]) 02458 pattern = '<%sd'%length 02459 start = end 02460 end += struct.calcsize(pattern) 02461 self.goal.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02462 start = end 02463 end += 4 02464 (length,) = _struct_I.unpack(str[start:end]) 02465 pattern = '<%sd'%length 02466 start = end 02467 end += struct.calcsize(pattern) 02468 self.goal.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02469 _x = self 02470 start = end 02471 end += 12 02472 (_x.goal.grasp.grasp_posture.header.seq, _x.goal.grasp.grasp_posture.header.stamp.secs, _x.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02473 start = end 02474 end += 4 02475 (length,) = _struct_I.unpack(str[start:end]) 02476 start = end 02477 end += length 02478 self.goal.grasp.grasp_posture.header.frame_id = str[start:end] 02479 start = end 02480 end += 4 02481 (length,) = _struct_I.unpack(str[start:end]) 02482 self.goal.grasp.grasp_posture.name = [] 02483 for i in range(0, length): 02484 start = end 02485 end += 4 02486 (length,) = _struct_I.unpack(str[start:end]) 02487 start = end 02488 end += length 02489 val1 = str[start:end] 02490 self.goal.grasp.grasp_posture.name.append(val1) 02491 start = end 02492 end += 4 02493 (length,) = _struct_I.unpack(str[start:end]) 02494 pattern = '<%sd'%length 02495 start = end 02496 end += struct.calcsize(pattern) 02497 self.goal.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02498 start = end 02499 end += 4 02500 (length,) = _struct_I.unpack(str[start:end]) 02501 pattern = '<%sd'%length 02502 start = end 02503 end += struct.calcsize(pattern) 02504 self.goal.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02505 start = end 02506 end += 4 02507 (length,) = _struct_I.unpack(str[start:end]) 02508 pattern = '<%sd'%length 02509 start = end 02510 end += struct.calcsize(pattern) 02511 self.goal.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02512 _x = self 02513 start = end 02514 end += 73 02515 (_x.goal.grasp.grasp_pose.position.x, _x.goal.grasp.grasp_pose.position.y, _x.goal.grasp.grasp_pose.position.z, _x.goal.grasp.grasp_pose.orientation.x, _x.goal.grasp.grasp_pose.orientation.y, _x.goal.grasp.grasp_pose.orientation.z, _x.goal.grasp.grasp_pose.orientation.w, _x.goal.grasp.success_probability, _x.goal.grasp.cluster_rep, _x.goal.grasp.desired_approach_distance, _x.goal.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end]) 02516 self.goal.grasp.cluster_rep = bool(self.goal.grasp.cluster_rep) 02517 start = end 02518 end += 4 02519 (length,) = _struct_I.unpack(str[start:end]) 02520 self.goal.grasp.moved_obstacles = [] 02521 for i in range(0, length): 02522 val1 = object_manipulation_msgs.msg.GraspableObject() 02523 start = end 02524 end += 4 02525 (length,) = _struct_I.unpack(str[start:end]) 02526 start = end 02527 end += length 02528 val1.reference_frame_id = str[start:end] 02529 start = end 02530 end += 4 02531 (length,) = _struct_I.unpack(str[start:end]) 02532 val1.potential_models = [] 02533 for i in range(0, length): 02534 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 02535 start = end 02536 end += 4 02537 (val2.model_id,) = _struct_i.unpack(str[start:end]) 02538 _v115 = val2.pose 02539 _v116 = _v115.header 02540 start = end 02541 end += 4 02542 (_v116.seq,) = _struct_I.unpack(str[start:end]) 02543 _v117 = _v116.stamp 02544 _x = _v117 02545 start = end 02546 end += 8 02547 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02548 start = end 02549 end += 4 02550 (length,) = _struct_I.unpack(str[start:end]) 02551 start = end 02552 end += length 02553 _v116.frame_id = str[start:end] 02554 _v118 = _v115.pose 02555 _v119 = _v118.position 02556 _x = _v119 02557 start = end 02558 end += 24 02559 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02560 _v120 = _v118.orientation 02561 _x = _v120 02562 start = end 02563 end += 32 02564 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02565 start = end 02566 end += 4 02567 (val2.confidence,) = _struct_f.unpack(str[start:end]) 02568 start = end 02569 end += 4 02570 (length,) = _struct_I.unpack(str[start:end]) 02571 start = end 02572 end += length 02573 val2.detector_name = str[start:end] 02574 val1.potential_models.append(val2) 02575 _v121 = val1.cluster 02576 _v122 = _v121.header 02577 start = end 02578 end += 4 02579 (_v122.seq,) = _struct_I.unpack(str[start:end]) 02580 _v123 = _v122.stamp 02581 _x = _v123 02582 start = end 02583 end += 8 02584 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02585 start = end 02586 end += 4 02587 (length,) = _struct_I.unpack(str[start:end]) 02588 start = end 02589 end += length 02590 _v122.frame_id = str[start:end] 02591 start = end 02592 end += 4 02593 (length,) = _struct_I.unpack(str[start:end]) 02594 _v121.points = [] 02595 for i in range(0, length): 02596 val3 = geometry_msgs.msg.Point32() 02597 _x = val3 02598 start = end 02599 end += 12 02600 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02601 _v121.points.append(val3) 02602 start = end 02603 end += 4 02604 (length,) = _struct_I.unpack(str[start:end]) 02605 _v121.channels = [] 02606 for i in range(0, length): 02607 val3 = sensor_msgs.msg.ChannelFloat32() 02608 start = end 02609 end += 4 02610 (length,) = _struct_I.unpack(str[start:end]) 02611 start = end 02612 end += length 02613 val3.name = str[start:end] 02614 start = end 02615 end += 4 02616 (length,) = _struct_I.unpack(str[start:end]) 02617 pattern = '<%sf'%length 02618 start = end 02619 end += struct.calcsize(pattern) 02620 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 02621 _v121.channels.append(val3) 02622 _v124 = val1.region 02623 _v125 = _v124.cloud 02624 _v126 = _v125.header 02625 start = end 02626 end += 4 02627 (_v126.seq,) = _struct_I.unpack(str[start:end]) 02628 _v127 = _v126.stamp 02629 _x = _v127 02630 start = end 02631 end += 8 02632 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02633 start = end 02634 end += 4 02635 (length,) = _struct_I.unpack(str[start:end]) 02636 start = end 02637 end += length 02638 _v126.frame_id = str[start:end] 02639 _x = _v125 02640 start = end 02641 end += 8 02642 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02643 start = end 02644 end += 4 02645 (length,) = _struct_I.unpack(str[start:end]) 02646 _v125.fields = [] 02647 for i in range(0, length): 02648 val4 = sensor_msgs.msg.PointField() 02649 start = end 02650 end += 4 02651 (length,) = _struct_I.unpack(str[start:end]) 02652 start = end 02653 end += length 02654 val4.name = str[start:end] 02655 _x = val4 02656 start = end 02657 end += 9 02658 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02659 _v125.fields.append(val4) 02660 _x = _v125 02661 start = end 02662 end += 9 02663 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 02664 _v125.is_bigendian = bool(_v125.is_bigendian) 02665 start = end 02666 end += 4 02667 (length,) = _struct_I.unpack(str[start:end]) 02668 start = end 02669 end += length 02670 _v125.data = str[start:end] 02671 start = end 02672 end += 1 02673 (_v125.is_dense,) = _struct_B.unpack(str[start:end]) 02674 _v125.is_dense = bool(_v125.is_dense) 02675 start = end 02676 end += 4 02677 (length,) = _struct_I.unpack(str[start:end]) 02678 pattern = '<%si'%length 02679 start = end 02680 end += struct.calcsize(pattern) 02681 _v124.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 02682 _v128 = _v124.image 02683 _v129 = _v128.header 02684 start = end 02685 end += 4 02686 (_v129.seq,) = _struct_I.unpack(str[start:end]) 02687 _v130 = _v129.stamp 02688 _x = _v130 02689 start = end 02690 end += 8 02691 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02692 start = end 02693 end += 4 02694 (length,) = _struct_I.unpack(str[start:end]) 02695 start = end 02696 end += length 02697 _v129.frame_id = str[start:end] 02698 _x = _v128 02699 start = end 02700 end += 8 02701 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02702 start = end 02703 end += 4 02704 (length,) = _struct_I.unpack(str[start:end]) 02705 start = end 02706 end += length 02707 _v128.encoding = str[start:end] 02708 _x = _v128 02709 start = end 02710 end += 5 02711 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02712 start = end 02713 end += 4 02714 (length,) = _struct_I.unpack(str[start:end]) 02715 start = end 02716 end += length 02717 _v128.data = str[start:end] 02718 _v131 = _v124.disparity_image 02719 _v132 = _v131.header 02720 start = end 02721 end += 4 02722 (_v132.seq,) = _struct_I.unpack(str[start:end]) 02723 _v133 = _v132.stamp 02724 _x = _v133 02725 start = end 02726 end += 8 02727 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02728 start = end 02729 end += 4 02730 (length,) = _struct_I.unpack(str[start:end]) 02731 start = end 02732 end += length 02733 _v132.frame_id = str[start:end] 02734 _x = _v131 02735 start = end 02736 end += 8 02737 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02738 start = end 02739 end += 4 02740 (length,) = _struct_I.unpack(str[start:end]) 02741 start = end 02742 end += length 02743 _v131.encoding = str[start:end] 02744 _x = _v131 02745 start = end 02746 end += 5 02747 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02748 start = end 02749 end += 4 02750 (length,) = _struct_I.unpack(str[start:end]) 02751 start = end 02752 end += length 02753 _v131.data = str[start:end] 02754 _v134 = _v124.cam_info 02755 _v135 = _v134.header 02756 start = end 02757 end += 4 02758 (_v135.seq,) = _struct_I.unpack(str[start:end]) 02759 _v136 = _v135.stamp 02760 _x = _v136 02761 start = end 02762 end += 8 02763 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02764 start = end 02765 end += 4 02766 (length,) = _struct_I.unpack(str[start:end]) 02767 start = end 02768 end += length 02769 _v135.frame_id = str[start:end] 02770 _x = _v134 02771 start = end 02772 end += 8 02773 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02774 start = end 02775 end += 4 02776 (length,) = _struct_I.unpack(str[start:end]) 02777 start = end 02778 end += length 02779 _v134.distortion_model = str[start:end] 02780 start = end 02781 end += 4 02782 (length,) = _struct_I.unpack(str[start:end]) 02783 pattern = '<%sd'%length 02784 start = end 02785 end += struct.calcsize(pattern) 02786 _v134.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02787 start = end 02788 end += 72 02789 _v134.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02790 start = end 02791 end += 72 02792 _v134.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 02793 start = end 02794 end += 96 02795 _v134.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 02796 _x = _v134 02797 start = end 02798 end += 8 02799 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02800 _v137 = _v134.roi 02801 _x = _v137 02802 start = end 02803 end += 17 02804 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02805 _v137.do_rectify = bool(_v137.do_rectify) 02806 _v138 = _v124.roi_box_pose 02807 _v139 = _v138.header 02808 start = end 02809 end += 4 02810 (_v139.seq,) = _struct_I.unpack(str[start:end]) 02811 _v140 = _v139.stamp 02812 _x = _v140 02813 start = end 02814 end += 8 02815 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02816 start = end 02817 end += 4 02818 (length,) = _struct_I.unpack(str[start:end]) 02819 start = end 02820 end += length 02821 _v139.frame_id = str[start:end] 02822 _v141 = _v138.pose 02823 _v142 = _v141.position 02824 _x = _v142 02825 start = end 02826 end += 24 02827 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02828 _v143 = _v141.orientation 02829 _x = _v143 02830 start = end 02831 end += 32 02832 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02833 _v144 = _v124.roi_box_dims 02834 _x = _v144 02835 start = end 02836 end += 24 02837 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02838 start = end 02839 end += 4 02840 (length,) = _struct_I.unpack(str[start:end]) 02841 start = end 02842 end += length 02843 val1.collision_name = str[start:end] 02844 self.goal.grasp.moved_obstacles.append(val1) 02845 return self 02846 except struct.error as e: 02847 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02848 02849 _struct_I = roslib.message.struct_I 02850 _struct_IBI = struct.Struct("<IBI") 02851 _struct_6IB3I = struct.Struct("<6IB3I") 02852 _struct_B = struct.Struct("<B") 02853 _struct_12d = struct.Struct("<12d") 02854 _struct_7df = struct.Struct("<7df") 02855 _struct_f = struct.Struct("<f") 02856 _struct_i = struct.Struct("<i") 02857 _struct_BI = struct.Struct("<BI") 02858 _struct_10d = struct.Struct("<10d") 02859 _struct_3f = struct.Struct("<3f") 02860 _struct_3I = struct.Struct("<3I") 02861 _struct_8dB2f = struct.Struct("<8dB2f") 02862 _struct_9d = struct.Struct("<9d") 02863 _struct_B2I = struct.Struct("<B2I") 02864 _struct_4d = struct.Struct("<4d") 02865 _struct_2I = struct.Struct("<2I") 02866 _struct_4IB = struct.Struct("<4IB") 02867 _struct_3d = struct.Struct("<3d")