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