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