$search
00001 """autogenerated by genmsg_py from PickupAction.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import arm_navigation_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 PickupAction(roslib.message.Message): 00015 _md5sum = "09990218855ede96047d48c436f6201d" 00016 _type = "object_manipulation_msgs/PickupAction" 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 PickupActionGoal action_goal 00021 PickupActionResult action_result 00022 PickupActionFeedback action_feedback 00023 00024 ================================================================================ 00025 MSG: object_manipulation_msgs/PickupActionGoal 00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00027 00028 Header header 00029 actionlib_msgs/GoalID goal_id 00030 PickupGoal 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: object_manipulation_msgs/PickupGoal 00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00066 # An action for picking up an object 00067 00068 # which arm to be used for grasping 00069 string arm_name 00070 00071 # the object to be grasped 00072 GraspableObject target 00073 00074 # a list of grasps to be used 00075 # if empty, the grasp executive will call one of its own planners 00076 Grasp[] desired_grasps 00077 00078 # how the object should be lifted after the grasp 00079 # the frame_id that this lift is specified in MUST be either the robot_frame 00080 # or the gripper_frame specified in your hand description file 00081 GripperTranslation lift 00082 00083 # the name that the target object has in the collision map 00084 # can be left empty if no name is available 00085 string collision_object_name 00086 00087 # the name that the support surface (e.g. table) has in the collision map 00088 # can be left empty if no name is available 00089 string collision_support_surface_name 00090 00091 # whether collisions between the gripper and the support surface should be acceptable 00092 # during move from pre-grasp to grasp and during lift. Collisions when moving to the 00093 # pre-grasp location are still not allowed even if this is set to true. 00094 bool allow_gripper_support_collision 00095 00096 # whether reactive grasp execution using tactile sensors should be used 00097 bool use_reactive_execution 00098 00099 # whether reactive object lifting based on tactile sensors should be used 00100 bool use_reactive_lift 00101 00102 # set this to true if you only want to query the manipulation pipeline as to what 00103 # grasps it thinks are feasible, without actually executing them. If this is set to 00104 # true, the atempted_grasp_results field of the result will be populated, but no arm 00105 # movement will be attempted 00106 bool only_perform_feasibility_test 00107 00108 # set this to true if you want to ignore all collisions throughout the pickup 00109 # and also move directly to the pre-grasp using Cartesian controllers 00110 bool ignore_collisions 00111 00112 # OPTIONAL (These will not have to be filled out most of the time) 00113 # constraints to be imposed on every point in the motion of the arm 00114 arm_navigation_msgs/Constraints path_constraints 00115 00116 # OPTIONAL (These will not have to be filled out most of the time) 00117 # additional collision operations to be used for every arm movement performed 00118 # during grasping. Note that these will be added on top of (and thus overide) other 00119 # collision operations that the grasping pipeline deems necessary. Should be used 00120 # with care and only if special behaviors are desired 00121 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations 00122 00123 # OPTIONAL (These will not have to be filled out most of the time) 00124 # additional link paddings to be used for every arm movement performed 00125 # during grasping. Note that these will be added on top of (and thus overide) other 00126 # link paddings that the grasping pipeline deems necessary. Should be used 00127 # with care and only if special behaviors are desired 00128 arm_navigation_msgs/LinkPadding[] additional_link_padding 00129 00130 # an optional list of obstacles that we have semantic information about 00131 # and that can be moved in the course of grasping 00132 GraspableObject[] movable_obstacles 00133 00134 # the maximum contact force to use while grasping (<=0 to disable) 00135 float32 max_contact_force 00136 00137 00138 ================================================================================ 00139 MSG: object_manipulation_msgs/GraspableObject 00140 # an object that the object_manipulator can work on 00141 00142 # a graspable object can be represented in multiple ways. This message 00143 # can contain all of them. Which one is actually used is up to the receiver 00144 # of this message. When adding new representations, one must be careful that 00145 # they have reasonable lightweight defaults indicating that that particular 00146 # representation is not available. 00147 00148 # the tf frame to be used as a reference frame when combining information from 00149 # the different representations below 00150 string reference_frame_id 00151 00152 # potential recognition results from a database of models 00153 # all poses are relative to the object reference pose 00154 household_objects_database_msgs/DatabaseModelPose[] potential_models 00155 00156 # the point cloud itself 00157 sensor_msgs/PointCloud cluster 00158 00159 # a region of a PointCloud2 of interest 00160 object_manipulation_msgs/SceneRegion region 00161 00162 # the name that this object has in the collision environment 00163 string collision_name 00164 ================================================================================ 00165 MSG: household_objects_database_msgs/DatabaseModelPose 00166 # Informs that a specific model from the Model Database has been 00167 # identified at a certain location 00168 00169 # the database id of the model 00170 int32 model_id 00171 00172 # the pose that it can be found in 00173 geometry_msgs/PoseStamped pose 00174 00175 # a measure of the confidence level in this detection result 00176 float32 confidence 00177 00178 # the name of the object detector that generated this detection result 00179 string detector_name 00180 00181 ================================================================================ 00182 MSG: geometry_msgs/PoseStamped 00183 # A Pose with reference coordinate frame and timestamp 00184 Header header 00185 Pose pose 00186 00187 ================================================================================ 00188 MSG: geometry_msgs/Pose 00189 # A representation of pose in free space, composed of postion and orientation. 00190 Point position 00191 Quaternion orientation 00192 00193 ================================================================================ 00194 MSG: geometry_msgs/Point 00195 # This contains the position of a point in free space 00196 float64 x 00197 float64 y 00198 float64 z 00199 00200 ================================================================================ 00201 MSG: geometry_msgs/Quaternion 00202 # This represents an orientation in free space in quaternion form. 00203 00204 float64 x 00205 float64 y 00206 float64 z 00207 float64 w 00208 00209 ================================================================================ 00210 MSG: sensor_msgs/PointCloud 00211 # This message holds a collection of 3d points, plus optional additional 00212 # information about each point. 00213 00214 # Time of sensor data acquisition, coordinate frame ID. 00215 Header header 00216 00217 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00218 # in the frame given in the header. 00219 geometry_msgs/Point32[] points 00220 00221 # Each channel should have the same number of elements as points array, 00222 # and the data in each channel should correspond 1:1 with each point. 00223 # Channel names in common practice are listed in ChannelFloat32.msg. 00224 ChannelFloat32[] channels 00225 00226 ================================================================================ 00227 MSG: geometry_msgs/Point32 00228 # This contains the position of a point in free space(with 32 bits of precision). 00229 # It is recommeded to use Point wherever possible instead of Point32. 00230 # 00231 # This recommendation is to promote interoperability. 00232 # 00233 # This message is designed to take up less space when sending 00234 # lots of points at once, as in the case of a PointCloud. 00235 00236 float32 x 00237 float32 y 00238 float32 z 00239 ================================================================================ 00240 MSG: sensor_msgs/ChannelFloat32 00241 # This message is used by the PointCloud message to hold optional data 00242 # associated with each point in the cloud. The length of the values 00243 # array should be the same as the length of the points array in the 00244 # PointCloud, and each value should be associated with the corresponding 00245 # point. 00246 00247 # Channel names in existing practice include: 00248 # "u", "v" - row and column (respectively) in the left stereo image. 00249 # This is opposite to usual conventions but remains for 00250 # historical reasons. The newer PointCloud2 message has no 00251 # such problem. 00252 # "rgb" - For point clouds produced by color stereo cameras. uint8 00253 # (R,G,B) values packed into the least significant 24 bits, 00254 # in order. 00255 # "intensity" - laser or pixel intensity. 00256 # "distance" 00257 00258 # The channel name should give semantics of the channel (e.g. 00259 # "intensity" instead of "value"). 00260 string name 00261 00262 # The values array should be 1-1 with the elements of the associated 00263 # PointCloud. 00264 float32[] values 00265 00266 ================================================================================ 00267 MSG: object_manipulation_msgs/SceneRegion 00268 # Point cloud 00269 sensor_msgs/PointCloud2 cloud 00270 00271 # Indices for the region of interest 00272 int32[] mask 00273 00274 # One of the corresponding 2D images, if applicable 00275 sensor_msgs/Image image 00276 00277 # The disparity image, if applicable 00278 sensor_msgs/Image disparity_image 00279 00280 # Camera info for the camera that took the image 00281 sensor_msgs/CameraInfo cam_info 00282 00283 # a 3D region of interest for grasp planning 00284 geometry_msgs/PoseStamped roi_box_pose 00285 geometry_msgs/Vector3 roi_box_dims 00286 00287 ================================================================================ 00288 MSG: sensor_msgs/PointCloud2 00289 # This message holds a collection of N-dimensional points, which may 00290 # contain additional information such as normals, intensity, etc. The 00291 # point data is stored as a binary blob, its layout described by the 00292 # contents of the "fields" array. 00293 00294 # The point cloud data may be organized 2d (image-like) or 1d 00295 # (unordered). Point clouds organized as 2d images may be produced by 00296 # camera depth sensors such as stereo or time-of-flight. 00297 00298 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00299 # points). 00300 Header header 00301 00302 # 2D structure of the point cloud. If the cloud is unordered, height is 00303 # 1 and width is the length of the point cloud. 00304 uint32 height 00305 uint32 width 00306 00307 # Describes the channels and their layout in the binary data blob. 00308 PointField[] fields 00309 00310 bool is_bigendian # Is this data bigendian? 00311 uint32 point_step # Length of a point in bytes 00312 uint32 row_step # Length of a row in bytes 00313 uint8[] data # Actual point data, size is (row_step*height) 00314 00315 bool is_dense # True if there are no invalid points 00316 00317 ================================================================================ 00318 MSG: sensor_msgs/PointField 00319 # This message holds the description of one point entry in the 00320 # PointCloud2 message format. 00321 uint8 INT8 = 1 00322 uint8 UINT8 = 2 00323 uint8 INT16 = 3 00324 uint8 UINT16 = 4 00325 uint8 INT32 = 5 00326 uint8 UINT32 = 6 00327 uint8 FLOAT32 = 7 00328 uint8 FLOAT64 = 8 00329 00330 string name # Name of field 00331 uint32 offset # Offset from start of point struct 00332 uint8 datatype # Datatype enumeration, see above 00333 uint32 count # How many elements in the field 00334 00335 ================================================================================ 00336 MSG: sensor_msgs/Image 00337 # This message contains an uncompressed image 00338 # (0, 0) is at top-left corner of image 00339 # 00340 00341 Header header # Header timestamp should be acquisition time of image 00342 # Header frame_id should be optical frame of camera 00343 # origin of frame should be optical center of cameara 00344 # +x should point to the right in the image 00345 # +y should point down in the image 00346 # +z should point into to plane of the image 00347 # If the frame_id here and the frame_id of the CameraInfo 00348 # message associated with the image conflict 00349 # the behavior is undefined 00350 00351 uint32 height # image height, that is, number of rows 00352 uint32 width # image width, that is, number of columns 00353 00354 # The legal values for encoding are in file src/image_encodings.cpp 00355 # If you want to standardize a new string format, join 00356 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00357 00358 string encoding # Encoding of pixels -- channel meaning, ordering, size 00359 # taken from the list of strings in src/image_encodings.cpp 00360 00361 uint8 is_bigendian # is this data bigendian? 00362 uint32 step # Full row length in bytes 00363 uint8[] data # actual matrix data, size is (step * rows) 00364 00365 ================================================================================ 00366 MSG: sensor_msgs/CameraInfo 00367 # This message defines meta information for a camera. It should be in a 00368 # camera namespace on topic "camera_info" and accompanied by up to five 00369 # image topics named: 00370 # 00371 # image_raw - raw data from the camera driver, possibly Bayer encoded 00372 # image - monochrome, distorted 00373 # image_color - color, distorted 00374 # image_rect - monochrome, rectified 00375 # image_rect_color - color, rectified 00376 # 00377 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00378 # for producing the four processed image topics from image_raw and 00379 # camera_info. The meaning of the camera parameters are described in 00380 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00381 # 00382 # The image_geometry package provides a user-friendly interface to 00383 # common operations using this meta information. If you want to, e.g., 00384 # project a 3d point into image coordinates, we strongly recommend 00385 # using image_geometry. 00386 # 00387 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00388 # zeroed out. In particular, clients may assume that K[0] == 0.0 00389 # indicates an uncalibrated camera. 00390 00391 ####################################################################### 00392 # Image acquisition info # 00393 ####################################################################### 00394 00395 # Time of image acquisition, camera coordinate frame ID 00396 Header header # Header timestamp should be acquisition time of image 00397 # Header frame_id should be optical frame of camera 00398 # origin of frame should be optical center of camera 00399 # +x should point to the right in the image 00400 # +y should point down in the image 00401 # +z should point into the plane of the image 00402 00403 00404 ####################################################################### 00405 # Calibration Parameters # 00406 ####################################################################### 00407 # These are fixed during camera calibration. Their values will be the # 00408 # same in all messages until the camera is recalibrated. Note that # 00409 # self-calibrating systems may "recalibrate" frequently. # 00410 # # 00411 # The internal parameters can be used to warp a raw (distorted) image # 00412 # to: # 00413 # 1. An undistorted image (requires D and K) # 00414 # 2. A rectified image (requires D, K, R) # 00415 # The projection matrix P projects 3D points into the rectified image.# 00416 ####################################################################### 00417 00418 # The image dimensions with which the camera was calibrated. Normally 00419 # this will be the full camera resolution in pixels. 00420 uint32 height 00421 uint32 width 00422 00423 # The distortion model used. Supported models are listed in 00424 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00425 # simple model of radial and tangential distortion - is sufficent. 00426 string distortion_model 00427 00428 # The distortion parameters, size depending on the distortion model. 00429 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00430 float64[] D 00431 00432 # Intrinsic camera matrix for the raw (distorted) images. 00433 # [fx 0 cx] 00434 # K = [ 0 fy cy] 00435 # [ 0 0 1] 00436 # Projects 3D points in the camera coordinate frame to 2D pixel 00437 # coordinates using the focal lengths (fx, fy) and principal point 00438 # (cx, cy). 00439 float64[9] K # 3x3 row-major matrix 00440 00441 # Rectification matrix (stereo cameras only) 00442 # A rotation matrix aligning the camera coordinate system to the ideal 00443 # stereo image plane so that epipolar lines in both stereo images are 00444 # parallel. 00445 float64[9] R # 3x3 row-major matrix 00446 00447 # Projection/camera matrix 00448 # [fx' 0 cx' Tx] 00449 # P = [ 0 fy' cy' Ty] 00450 # [ 0 0 1 0] 00451 # By convention, this matrix specifies the intrinsic (camera) matrix 00452 # of the processed (rectified) image. That is, the left 3x3 portion 00453 # is the normal camera intrinsic matrix for the rectified image. 00454 # It projects 3D points in the camera coordinate frame to 2D pixel 00455 # coordinates using the focal lengths (fx', fy') and principal point 00456 # (cx', cy') - these may differ from the values in K. 00457 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00458 # also have R = the identity and P[1:3,1:3] = K. 00459 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00460 # position of the optical center of the second camera in the first 00461 # camera's frame. We assume Tz = 0 so both cameras are in the same 00462 # stereo image plane. The first camera always has Tx = Ty = 0. For 00463 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00464 # Tx = -fx' * B, where B is the baseline between the cameras. 00465 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00466 # the rectified image is given by: 00467 # [u v w]' = P * [X Y Z 1]' 00468 # x = u / w 00469 # y = v / w 00470 # This holds for both images of a stereo pair. 00471 float64[12] P # 3x4 row-major matrix 00472 00473 00474 ####################################################################### 00475 # Operational Parameters # 00476 ####################################################################### 00477 # These define the image region actually captured by the camera # 00478 # driver. Although they affect the geometry of the output image, they # 00479 # may be changed freely without recalibrating the camera. # 00480 ####################################################################### 00481 00482 # Binning refers here to any camera setting which combines rectangular 00483 # neighborhoods of pixels into larger "super-pixels." It reduces the 00484 # resolution of the output image to 00485 # (width / binning_x) x (height / binning_y). 00486 # The default values binning_x = binning_y = 0 is considered the same 00487 # as binning_x = binning_y = 1 (no subsampling). 00488 uint32 binning_x 00489 uint32 binning_y 00490 00491 # Region of interest (subwindow of full camera resolution), given in 00492 # full resolution (unbinned) image coordinates. A particular ROI 00493 # always denotes the same window of pixels on the camera sensor, 00494 # regardless of binning settings. 00495 # The default setting of roi (all values 0) is considered the same as 00496 # full resolution (roi.width = width, roi.height = height). 00497 RegionOfInterest roi 00498 00499 ================================================================================ 00500 MSG: sensor_msgs/RegionOfInterest 00501 # This message is used to specify a region of interest within an image. 00502 # 00503 # When used to specify the ROI setting of the camera when the image was 00504 # taken, the height and width fields should either match the height and 00505 # width fields for the associated image; or height = width = 0 00506 # indicates that the full resolution image was captured. 00507 00508 uint32 x_offset # Leftmost pixel of the ROI 00509 # (0 if the ROI includes the left edge of the image) 00510 uint32 y_offset # Topmost pixel of the ROI 00511 # (0 if the ROI includes the top edge of the image) 00512 uint32 height # Height of ROI 00513 uint32 width # Width of ROI 00514 00515 # True if a distinct rectified ROI should be calculated from the "raw" 00516 # ROI in this message. Typically this should be False if the full image 00517 # is captured (ROI not used), and True if a subwindow is captured (ROI 00518 # used). 00519 bool do_rectify 00520 00521 ================================================================================ 00522 MSG: geometry_msgs/Vector3 00523 # This represents a vector in free space. 00524 00525 float64 x 00526 float64 y 00527 float64 z 00528 ================================================================================ 00529 MSG: object_manipulation_msgs/Grasp 00530 00531 # The internal posture of the hand for the pre-grasp 00532 # only positions are used 00533 sensor_msgs/JointState pre_grasp_posture 00534 00535 # The internal posture of the hand for the grasp 00536 # positions and efforts are used 00537 sensor_msgs/JointState grasp_posture 00538 00539 # The position of the end-effector for the grasp relative to a reference frame 00540 # (that is always specified elsewhere, not in this message) 00541 geometry_msgs/Pose grasp_pose 00542 00543 # The estimated probability of success for this grasp 00544 float64 success_probability 00545 00546 # Debug flag to indicate that this grasp would be the best in its cluster 00547 bool cluster_rep 00548 00549 # how far the pre-grasp should ideally be away from the grasp 00550 float32 desired_approach_distance 00551 00552 # how much distance between pre-grasp and grasp must actually be feasible 00553 # for the grasp not to be rejected 00554 float32 min_approach_distance 00555 00556 # an optional list of obstacles that we have semantic information about 00557 # and that we expect might move in the course of executing this grasp 00558 # the grasp planner is expected to make sure they move in an OK way; during 00559 # execution, grasp executors will not check for collisions against these objects 00560 GraspableObject[] moved_obstacles 00561 00562 ================================================================================ 00563 MSG: sensor_msgs/JointState 00564 # This is a message that holds data to describe the state of a set of torque controlled joints. 00565 # 00566 # The state of each joint (revolute or prismatic) is defined by: 00567 # * the position of the joint (rad or m), 00568 # * the velocity of the joint (rad/s or m/s) and 00569 # * the effort that is applied in the joint (Nm or N). 00570 # 00571 # Each joint is uniquely identified by its name 00572 # The header specifies the time at which the joint states were recorded. All the joint states 00573 # in one message have to be recorded at the same time. 00574 # 00575 # This message consists of a multiple arrays, one for each part of the joint state. 00576 # The goal is to make each of the fields optional. When e.g. your joints have no 00577 # effort associated with them, you can leave the effort array empty. 00578 # 00579 # All arrays in this message should have the same size, or be empty. 00580 # This is the only way to uniquely associate the joint name with the correct 00581 # states. 00582 00583 00584 Header header 00585 00586 string[] name 00587 float64[] position 00588 float64[] velocity 00589 float64[] effort 00590 00591 ================================================================================ 00592 MSG: object_manipulation_msgs/GripperTranslation 00593 # defines a translation for the gripper, used in pickup or place tasks 00594 # for example for lifting an object off a table or approaching the table for placing 00595 00596 # the direction of the translation 00597 geometry_msgs/Vector3Stamped direction 00598 00599 # the desired translation distance 00600 float32 desired_distance 00601 00602 # the min distance that must be considered feasible before the 00603 # grasp is even attempted 00604 float32 min_distance 00605 ================================================================================ 00606 MSG: geometry_msgs/Vector3Stamped 00607 # This represents a Vector3 with reference coordinate frame and timestamp 00608 Header header 00609 Vector3 vector 00610 00611 ================================================================================ 00612 MSG: arm_navigation_msgs/Constraints 00613 # This message contains a list of motion planning constraints. 00614 00615 arm_navigation_msgs/JointConstraint[] joint_constraints 00616 arm_navigation_msgs/PositionConstraint[] position_constraints 00617 arm_navigation_msgs/OrientationConstraint[] orientation_constraints 00618 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints 00619 00620 ================================================================================ 00621 MSG: arm_navigation_msgs/JointConstraint 00622 # Constrain the position of a joint to be within a certain bound 00623 string joint_name 00624 00625 # the bound to be achieved is [position - tolerance_below, position + tolerance_above] 00626 float64 position 00627 float64 tolerance_above 00628 float64 tolerance_below 00629 00630 # A weighting factor for this constraint 00631 float64 weight 00632 ================================================================================ 00633 MSG: arm_navigation_msgs/PositionConstraint 00634 # This message contains the definition of a position constraint. 00635 Header header 00636 00637 # The robot link this constraint refers to 00638 string link_name 00639 00640 # The offset (in the link frame) for the target point on the link we are planning for 00641 geometry_msgs/Point target_point_offset 00642 00643 # The nominal/target position for the point we are planning for 00644 geometry_msgs/Point position 00645 00646 # The shape of the bounded region that constrains the position of the end-effector 00647 # This region is always centered at the position defined above 00648 arm_navigation_msgs/Shape constraint_region_shape 00649 00650 # The orientation of the bounded region that constrains the position of the end-effector. 00651 # This allows the specification of non-axis aligned constraints 00652 geometry_msgs/Quaternion constraint_region_orientation 00653 00654 # Constraint weighting factor - a weight for this constraint 00655 float64 weight 00656 00657 ================================================================================ 00658 MSG: arm_navigation_msgs/Shape 00659 byte SPHERE=0 00660 byte BOX=1 00661 byte CYLINDER=2 00662 byte MESH=3 00663 00664 byte type 00665 00666 00667 #### define sphere, box, cylinder #### 00668 # the origin of each shape is considered at the shape's center 00669 00670 # for sphere 00671 # radius := dimensions[0] 00672 00673 # for cylinder 00674 # radius := dimensions[0] 00675 # length := dimensions[1] 00676 # the length is along the Z axis 00677 00678 # for box 00679 # size_x := dimensions[0] 00680 # size_y := dimensions[1] 00681 # size_z := dimensions[2] 00682 float64[] dimensions 00683 00684 00685 #### define mesh #### 00686 00687 # list of triangles; triangle k is defined by tre vertices located 00688 # at indices triangles[3k], triangles[3k+1], triangles[3k+2] 00689 int32[] triangles 00690 geometry_msgs/Point[] vertices 00691 00692 ================================================================================ 00693 MSG: arm_navigation_msgs/OrientationConstraint 00694 # This message contains the definition of an orientation constraint. 00695 Header header 00696 00697 # The robot link this constraint refers to 00698 string link_name 00699 00700 # The type of the constraint 00701 int32 type 00702 int32 LINK_FRAME=0 00703 int32 HEADER_FRAME=1 00704 00705 # The desired orientation of the robot link specified as a quaternion 00706 geometry_msgs/Quaternion orientation 00707 00708 # optional RPY error tolerances specified if 00709 float64 absolute_roll_tolerance 00710 float64 absolute_pitch_tolerance 00711 float64 absolute_yaw_tolerance 00712 00713 # Constraint weighting factor - a weight for this constraint 00714 float64 weight 00715 00716 ================================================================================ 00717 MSG: arm_navigation_msgs/VisibilityConstraint 00718 # This message contains the definition of a visibility constraint. 00719 Header header 00720 00721 # The point stamped target that needs to be kept within view of the sensor 00722 geometry_msgs/PointStamped target 00723 00724 # The local pose of the frame in which visibility is to be maintained 00725 # The frame id should represent the robot link to which the sensor is attached 00726 # The visual axis of the sensor is assumed to be along the X axis of this frame 00727 geometry_msgs/PoseStamped sensor_pose 00728 00729 # The deviation (in radians) that will be tolerated 00730 # Constraint error will be measured as the solid angle between the 00731 # X axis of the frame defined above and the vector between the origin 00732 # of the frame defined above and the target location 00733 float64 absolute_tolerance 00734 00735 00736 ================================================================================ 00737 MSG: geometry_msgs/PointStamped 00738 # This represents a Point with reference coordinate frame and timestamp 00739 Header header 00740 Point point 00741 00742 ================================================================================ 00743 MSG: arm_navigation_msgs/OrderedCollisionOperations 00744 # A set of collision operations that will be performed in the order they are specified 00745 CollisionOperation[] collision_operations 00746 ================================================================================ 00747 MSG: arm_navigation_msgs/CollisionOperation 00748 # A definition of a collision operation 00749 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions 00750 # between the gripper and all objects in the collision space 00751 00752 string object1 00753 string object2 00754 string COLLISION_SET_ALL="all" 00755 string COLLISION_SET_OBJECTS="objects" 00756 string COLLISION_SET_ATTACHED_OBJECTS="attached" 00757 00758 # The penetration distance to which collisions are allowed. This is 0.0 by default. 00759 float64 penetration_distance 00760 00761 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above 00762 int32 operation 00763 int32 DISABLE=0 00764 int32 ENABLE=1 00765 00766 ================================================================================ 00767 MSG: arm_navigation_msgs/LinkPadding 00768 #name for the link 00769 string link_name 00770 00771 # padding to apply to the link 00772 float64 padding 00773 00774 ================================================================================ 00775 MSG: object_manipulation_msgs/PickupActionResult 00776 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00777 00778 Header header 00779 actionlib_msgs/GoalStatus status 00780 PickupResult result 00781 00782 ================================================================================ 00783 MSG: actionlib_msgs/GoalStatus 00784 GoalID goal_id 00785 uint8 status 00786 uint8 PENDING = 0 # The goal has yet to be processed by the action server 00787 uint8 ACTIVE = 1 # The goal is currently being processed by the action server 00788 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing 00789 # and has since completed its execution (Terminal State) 00790 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State) 00791 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due 00792 # to some failure (Terminal State) 00793 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, 00794 # because the goal was unattainable or invalid (Terminal State) 00795 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing 00796 # and has not yet completed execution 00797 uint8 RECALLING = 7 # The goal received a cancel request before it started executing, 00798 # but the action server has not yet confirmed that the goal is canceled 00799 uint8 RECALLED = 8 # The goal received a cancel request before it started executing 00800 # and was successfully cancelled (Terminal State) 00801 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be 00802 # sent over the wire by an action server 00803 00804 #Allow for the user to associate a string with GoalStatus for debugging 00805 string text 00806 00807 00808 ================================================================================ 00809 MSG: object_manipulation_msgs/PickupResult 00810 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00811 00812 # The overall result of the pickup attempt 00813 ManipulationResult manipulation_result 00814 00815 # The performed grasp, if attempt was successful 00816 Grasp grasp 00817 00818 # the complete list of attempted grasp, in the order in which they have been attempted 00819 # the successful one should be the last one in this list 00820 Grasp[] attempted_grasps 00821 00822 # the outcomes of the attempted grasps, in the same order as attempted_grasps 00823 GraspResult[] attempted_grasp_results 00824 00825 00826 ================================================================================ 00827 MSG: object_manipulation_msgs/ManipulationResult 00828 # Result codes for manipulation tasks 00829 00830 # task completed as expected 00831 # generally means you can proceed as planned 00832 int32 SUCCESS = 1 00833 00834 # task not possible (e.g. out of reach or obstacles in the way) 00835 # generally means that the world was not disturbed, so you can try another task 00836 int32 UNFEASIBLE = -1 00837 00838 # task was thought possible, but failed due to unexpected events during execution 00839 # it is likely that the world was disturbed, so you are encouraged to refresh 00840 # your sensed world model before proceeding to another task 00841 int32 FAILED = -2 00842 00843 # a lower level error prevented task completion (e.g. joint controller not responding) 00844 # generally requires human attention 00845 int32 ERROR = -3 00846 00847 # means that at some point during execution we ended up in a state that the collision-aware 00848 # arm navigation module will not move out of. The world was likely not disturbed, but you 00849 # probably need a new collision map to move the arm out of the stuck position 00850 int32 ARM_MOVEMENT_PREVENTED = -4 00851 00852 # specific to grasp actions 00853 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested 00854 # it is likely that the collision environment will see collisions between the hand/object and the support surface 00855 int32 LIFT_FAILED = -5 00856 00857 # specific to place actions 00858 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested 00859 # it is likely that the collision environment will see collisions between the hand and the object 00860 int32 RETREAT_FAILED = -6 00861 00862 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else" 00863 int32 CANCELLED = -7 00864 00865 # the actual value of this error code 00866 int32 value 00867 00868 ================================================================================ 00869 MSG: object_manipulation_msgs/GraspResult 00870 int32 SUCCESS = 1 00871 int32 GRASP_OUT_OF_REACH = 2 00872 int32 GRASP_IN_COLLISION = 3 00873 int32 GRASP_UNFEASIBLE = 4 00874 int32 PREGRASP_OUT_OF_REACH = 5 00875 int32 PREGRASP_IN_COLLISION = 6 00876 int32 PREGRASP_UNFEASIBLE = 7 00877 int32 LIFT_OUT_OF_REACH = 8 00878 int32 LIFT_IN_COLLISION = 9 00879 int32 LIFT_UNFEASIBLE = 10 00880 int32 MOVE_ARM_FAILED = 11 00881 int32 GRASP_FAILED = 12 00882 int32 LIFT_FAILED = 13 00883 int32 RETREAT_FAILED = 14 00884 int32 result_code 00885 00886 # whether the state of the world was disturbed by this attempt. generally, this flag 00887 # shows if another task can be attempted, or a new sensed world model is recommeded 00888 # before proceeding 00889 bool continuation_possible 00890 00891 ================================================================================ 00892 MSG: object_manipulation_msgs/PickupActionFeedback 00893 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00894 00895 Header header 00896 actionlib_msgs/GoalStatus status 00897 PickupFeedback feedback 00898 00899 ================================================================================ 00900 MSG: object_manipulation_msgs/PickupFeedback 00901 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00902 00903 # The number of the grasp currently being attempted 00904 int32 current_grasp 00905 00906 # The total number of grasps that will be attempted 00907 int32 total_grasps 00908 00909 00910 """ 00911 __slots__ = ['action_goal','action_result','action_feedback'] 00912 _slot_types = ['object_manipulation_msgs/PickupActionGoal','object_manipulation_msgs/PickupActionResult','object_manipulation_msgs/PickupActionFeedback'] 00913 00914 def __init__(self, *args, **kwds): 00915 """ 00916 Constructor. Any message fields that are implicitly/explicitly 00917 set to None will be assigned a default value. The recommend 00918 use is keyword arguments as this is more robust to future message 00919 changes. You cannot mix in-order arguments and keyword arguments. 00920 00921 The available fields are: 00922 action_goal,action_result,action_feedback 00923 00924 @param args: complete set of field values, in .msg order 00925 @param kwds: use keyword arguments corresponding to message field names 00926 to set specific fields. 00927 """ 00928 if args or kwds: 00929 super(PickupAction, self).__init__(*args, **kwds) 00930 #message fields cannot be None, assign default values for those that are 00931 if self.action_goal is None: 00932 self.action_goal = object_manipulation_msgs.msg.PickupActionGoal() 00933 if self.action_result is None: 00934 self.action_result = object_manipulation_msgs.msg.PickupActionResult() 00935 if self.action_feedback is None: 00936 self.action_feedback = object_manipulation_msgs.msg.PickupActionFeedback() 00937 else: 00938 self.action_goal = object_manipulation_msgs.msg.PickupActionGoal() 00939 self.action_result = object_manipulation_msgs.msg.PickupActionResult() 00940 self.action_feedback = object_manipulation_msgs.msg.PickupActionFeedback() 00941 00942 def _get_types(self): 00943 """ 00944 internal API method 00945 """ 00946 return self._slot_types 00947 00948 def serialize(self, buff): 00949 """ 00950 serialize message into buffer 00951 @param buff: buffer 00952 @type buff: StringIO 00953 """ 00954 try: 00955 _x = self 00956 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 00957 _x = self.action_goal.header.frame_id 00958 length = len(_x) 00959 buff.write(struct.pack('<I%ss'%length, length, _x)) 00960 _x = self 00961 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 00962 _x = self.action_goal.goal_id.id 00963 length = len(_x) 00964 buff.write(struct.pack('<I%ss'%length, length, _x)) 00965 _x = self.action_goal.goal.arm_name 00966 length = len(_x) 00967 buff.write(struct.pack('<I%ss'%length, length, _x)) 00968 _x = self.action_goal.goal.target.reference_frame_id 00969 length = len(_x) 00970 buff.write(struct.pack('<I%ss'%length, length, _x)) 00971 length = len(self.action_goal.goal.target.potential_models) 00972 buff.write(_struct_I.pack(length)) 00973 for val1 in self.action_goal.goal.target.potential_models: 00974 buff.write(_struct_i.pack(val1.model_id)) 00975 _v1 = val1.pose 00976 _v2 = _v1.header 00977 buff.write(_struct_I.pack(_v2.seq)) 00978 _v3 = _v2.stamp 00979 _x = _v3 00980 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00981 _x = _v2.frame_id 00982 length = len(_x) 00983 buff.write(struct.pack('<I%ss'%length, length, _x)) 00984 _v4 = _v1.pose 00985 _v5 = _v4.position 00986 _x = _v5 00987 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00988 _v6 = _v4.orientation 00989 _x = _v6 00990 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00991 buff.write(_struct_f.pack(val1.confidence)) 00992 _x = val1.detector_name 00993 length = len(_x) 00994 buff.write(struct.pack('<I%ss'%length, length, _x)) 00995 _x = self 00996 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs)) 00997 _x = self.action_goal.goal.target.cluster.header.frame_id 00998 length = len(_x) 00999 buff.write(struct.pack('<I%ss'%length, length, _x)) 01000 length = len(self.action_goal.goal.target.cluster.points) 01001 buff.write(_struct_I.pack(length)) 01002 for val1 in self.action_goal.goal.target.cluster.points: 01003 _x = val1 01004 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01005 length = len(self.action_goal.goal.target.cluster.channels) 01006 buff.write(_struct_I.pack(length)) 01007 for val1 in self.action_goal.goal.target.cluster.channels: 01008 _x = val1.name 01009 length = len(_x) 01010 buff.write(struct.pack('<I%ss'%length, length, _x)) 01011 length = len(val1.values) 01012 buff.write(_struct_I.pack(length)) 01013 pattern = '<%sf'%length 01014 buff.write(struct.pack(pattern, *val1.values)) 01015 _x = self 01016 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs)) 01017 _x = self.action_goal.goal.target.region.cloud.header.frame_id 01018 length = len(_x) 01019 buff.write(struct.pack('<I%ss'%length, length, _x)) 01020 _x = self 01021 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width)) 01022 length = len(self.action_goal.goal.target.region.cloud.fields) 01023 buff.write(_struct_I.pack(length)) 01024 for val1 in self.action_goal.goal.target.region.cloud.fields: 01025 _x = val1.name 01026 length = len(_x) 01027 buff.write(struct.pack('<I%ss'%length, length, _x)) 01028 _x = val1 01029 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01030 _x = self 01031 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step)) 01032 _x = self.action_goal.goal.target.region.cloud.data 01033 length = len(_x) 01034 # - if encoded as a list instead, serialize as bytes instead of string 01035 if type(_x) in [list, tuple]: 01036 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01037 else: 01038 buff.write(struct.pack('<I%ss'%length, length, _x)) 01039 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense)) 01040 length = len(self.action_goal.goal.target.region.mask) 01041 buff.write(_struct_I.pack(length)) 01042 pattern = '<%si'%length 01043 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.mask)) 01044 _x = self 01045 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs)) 01046 _x = self.action_goal.goal.target.region.image.header.frame_id 01047 length = len(_x) 01048 buff.write(struct.pack('<I%ss'%length, length, _x)) 01049 _x = self 01050 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width)) 01051 _x = self.action_goal.goal.target.region.image.encoding 01052 length = len(_x) 01053 buff.write(struct.pack('<I%ss'%length, length, _x)) 01054 _x = self 01055 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step)) 01056 _x = self.action_goal.goal.target.region.image.data 01057 length = len(_x) 01058 # - if encoded as a list instead, serialize as bytes instead of string 01059 if type(_x) in [list, tuple]: 01060 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01061 else: 01062 buff.write(struct.pack('<I%ss'%length, length, _x)) 01063 _x = self 01064 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs)) 01065 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id 01066 length = len(_x) 01067 buff.write(struct.pack('<I%ss'%length, length, _x)) 01068 _x = self 01069 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width)) 01070 _x = self.action_goal.goal.target.region.disparity_image.encoding 01071 length = len(_x) 01072 buff.write(struct.pack('<I%ss'%length, length, _x)) 01073 _x = self 01074 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step)) 01075 _x = self.action_goal.goal.target.region.disparity_image.data 01076 length = len(_x) 01077 # - if encoded as a list instead, serialize as bytes instead of string 01078 if type(_x) in [list, tuple]: 01079 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01080 else: 01081 buff.write(struct.pack('<I%ss'%length, length, _x)) 01082 _x = self 01083 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs)) 01084 _x = self.action_goal.goal.target.region.cam_info.header.frame_id 01085 length = len(_x) 01086 buff.write(struct.pack('<I%ss'%length, length, _x)) 01087 _x = self 01088 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width)) 01089 _x = self.action_goal.goal.target.region.cam_info.distortion_model 01090 length = len(_x) 01091 buff.write(struct.pack('<I%ss'%length, length, _x)) 01092 length = len(self.action_goal.goal.target.region.cam_info.D) 01093 buff.write(_struct_I.pack(length)) 01094 pattern = '<%sd'%length 01095 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.cam_info.D)) 01096 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.K)) 01097 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.R)) 01098 buff.write(_struct_12d.pack(*self.action_goal.goal.target.region.cam_info.P)) 01099 _x = self 01100 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs)) 01101 _x = self.action_goal.goal.target.region.roi_box_pose.header.frame_id 01102 length = len(_x) 01103 buff.write(struct.pack('<I%ss'%length, length, _x)) 01104 _x = self 01105 buff.write(_struct_10d.pack(_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z)) 01106 _x = self.action_goal.goal.target.collision_name 01107 length = len(_x) 01108 buff.write(struct.pack('<I%ss'%length, length, _x)) 01109 length = len(self.action_goal.goal.desired_grasps) 01110 buff.write(_struct_I.pack(length)) 01111 for val1 in self.action_goal.goal.desired_grasps: 01112 _v7 = val1.pre_grasp_posture 01113 _v8 = _v7.header 01114 buff.write(_struct_I.pack(_v8.seq)) 01115 _v9 = _v8.stamp 01116 _x = _v9 01117 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01118 _x = _v8.frame_id 01119 length = len(_x) 01120 buff.write(struct.pack('<I%ss'%length, length, _x)) 01121 length = len(_v7.name) 01122 buff.write(_struct_I.pack(length)) 01123 for val3 in _v7.name: 01124 length = len(val3) 01125 buff.write(struct.pack('<I%ss'%length, length, val3)) 01126 length = len(_v7.position) 01127 buff.write(_struct_I.pack(length)) 01128 pattern = '<%sd'%length 01129 buff.write(struct.pack(pattern, *_v7.position)) 01130 length = len(_v7.velocity) 01131 buff.write(_struct_I.pack(length)) 01132 pattern = '<%sd'%length 01133 buff.write(struct.pack(pattern, *_v7.velocity)) 01134 length = len(_v7.effort) 01135 buff.write(_struct_I.pack(length)) 01136 pattern = '<%sd'%length 01137 buff.write(struct.pack(pattern, *_v7.effort)) 01138 _v10 = val1.grasp_posture 01139 _v11 = _v10.header 01140 buff.write(_struct_I.pack(_v11.seq)) 01141 _v12 = _v11.stamp 01142 _x = _v12 01143 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01144 _x = _v11.frame_id 01145 length = len(_x) 01146 buff.write(struct.pack('<I%ss'%length, length, _x)) 01147 length = len(_v10.name) 01148 buff.write(_struct_I.pack(length)) 01149 for val3 in _v10.name: 01150 length = len(val3) 01151 buff.write(struct.pack('<I%ss'%length, length, val3)) 01152 length = len(_v10.position) 01153 buff.write(_struct_I.pack(length)) 01154 pattern = '<%sd'%length 01155 buff.write(struct.pack(pattern, *_v10.position)) 01156 length = len(_v10.velocity) 01157 buff.write(_struct_I.pack(length)) 01158 pattern = '<%sd'%length 01159 buff.write(struct.pack(pattern, *_v10.velocity)) 01160 length = len(_v10.effort) 01161 buff.write(_struct_I.pack(length)) 01162 pattern = '<%sd'%length 01163 buff.write(struct.pack(pattern, *_v10.effort)) 01164 _v13 = val1.grasp_pose 01165 _v14 = _v13.position 01166 _x = _v14 01167 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01168 _v15 = _v13.orientation 01169 _x = _v15 01170 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01171 _x = val1 01172 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 01173 length = len(val1.moved_obstacles) 01174 buff.write(_struct_I.pack(length)) 01175 for val2 in val1.moved_obstacles: 01176 _x = val2.reference_frame_id 01177 length = len(_x) 01178 buff.write(struct.pack('<I%ss'%length, length, _x)) 01179 length = len(val2.potential_models) 01180 buff.write(_struct_I.pack(length)) 01181 for val3 in val2.potential_models: 01182 buff.write(_struct_i.pack(val3.model_id)) 01183 _v16 = val3.pose 01184 _v17 = _v16.header 01185 buff.write(_struct_I.pack(_v17.seq)) 01186 _v18 = _v17.stamp 01187 _x = _v18 01188 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01189 _x = _v17.frame_id 01190 length = len(_x) 01191 buff.write(struct.pack('<I%ss'%length, length, _x)) 01192 _v19 = _v16.pose 01193 _v20 = _v19.position 01194 _x = _v20 01195 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01196 _v21 = _v19.orientation 01197 _x = _v21 01198 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01199 buff.write(_struct_f.pack(val3.confidence)) 01200 _x = val3.detector_name 01201 length = len(_x) 01202 buff.write(struct.pack('<I%ss'%length, length, _x)) 01203 _v22 = val2.cluster 01204 _v23 = _v22.header 01205 buff.write(_struct_I.pack(_v23.seq)) 01206 _v24 = _v23.stamp 01207 _x = _v24 01208 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01209 _x = _v23.frame_id 01210 length = len(_x) 01211 buff.write(struct.pack('<I%ss'%length, length, _x)) 01212 length = len(_v22.points) 01213 buff.write(_struct_I.pack(length)) 01214 for val4 in _v22.points: 01215 _x = val4 01216 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01217 length = len(_v22.channels) 01218 buff.write(_struct_I.pack(length)) 01219 for val4 in _v22.channels: 01220 _x = val4.name 01221 length = len(_x) 01222 buff.write(struct.pack('<I%ss'%length, length, _x)) 01223 length = len(val4.values) 01224 buff.write(_struct_I.pack(length)) 01225 pattern = '<%sf'%length 01226 buff.write(struct.pack(pattern, *val4.values)) 01227 _v25 = val2.region 01228 _v26 = _v25.cloud 01229 _v27 = _v26.header 01230 buff.write(_struct_I.pack(_v27.seq)) 01231 _v28 = _v27.stamp 01232 _x = _v28 01233 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01234 _x = _v27.frame_id 01235 length = len(_x) 01236 buff.write(struct.pack('<I%ss'%length, length, _x)) 01237 _x = _v26 01238 buff.write(_struct_2I.pack(_x.height, _x.width)) 01239 length = len(_v26.fields) 01240 buff.write(_struct_I.pack(length)) 01241 for val5 in _v26.fields: 01242 _x = val5.name 01243 length = len(_x) 01244 buff.write(struct.pack('<I%ss'%length, length, _x)) 01245 _x = val5 01246 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01247 _x = _v26 01248 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01249 _x = _v26.data 01250 length = len(_x) 01251 # - if encoded as a list instead, serialize as bytes instead of string 01252 if type(_x) in [list, tuple]: 01253 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01254 else: 01255 buff.write(struct.pack('<I%ss'%length, length, _x)) 01256 buff.write(_struct_B.pack(_v26.is_dense)) 01257 length = len(_v25.mask) 01258 buff.write(_struct_I.pack(length)) 01259 pattern = '<%si'%length 01260 buff.write(struct.pack(pattern, *_v25.mask)) 01261 _v29 = _v25.image 01262 _v30 = _v29.header 01263 buff.write(_struct_I.pack(_v30.seq)) 01264 _v31 = _v30.stamp 01265 _x = _v31 01266 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01267 _x = _v30.frame_id 01268 length = len(_x) 01269 buff.write(struct.pack('<I%ss'%length, length, _x)) 01270 _x = _v29 01271 buff.write(_struct_2I.pack(_x.height, _x.width)) 01272 _x = _v29.encoding 01273 length = len(_x) 01274 buff.write(struct.pack('<I%ss'%length, length, _x)) 01275 _x = _v29 01276 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01277 _x = _v29.data 01278 length = len(_x) 01279 # - if encoded as a list instead, serialize as bytes instead of string 01280 if type(_x) in [list, tuple]: 01281 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01282 else: 01283 buff.write(struct.pack('<I%ss'%length, length, _x)) 01284 _v32 = _v25.disparity_image 01285 _v33 = _v32.header 01286 buff.write(_struct_I.pack(_v33.seq)) 01287 _v34 = _v33.stamp 01288 _x = _v34 01289 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01290 _x = _v33.frame_id 01291 length = len(_x) 01292 buff.write(struct.pack('<I%ss'%length, length, _x)) 01293 _x = _v32 01294 buff.write(_struct_2I.pack(_x.height, _x.width)) 01295 _x = _v32.encoding 01296 length = len(_x) 01297 buff.write(struct.pack('<I%ss'%length, length, _x)) 01298 _x = _v32 01299 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01300 _x = _v32.data 01301 length = len(_x) 01302 # - if encoded as a list instead, serialize as bytes instead of string 01303 if type(_x) in [list, tuple]: 01304 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01305 else: 01306 buff.write(struct.pack('<I%ss'%length, length, _x)) 01307 _v35 = _v25.cam_info 01308 _v36 = _v35.header 01309 buff.write(_struct_I.pack(_v36.seq)) 01310 _v37 = _v36.stamp 01311 _x = _v37 01312 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01313 _x = _v36.frame_id 01314 length = len(_x) 01315 buff.write(struct.pack('<I%ss'%length, length, _x)) 01316 _x = _v35 01317 buff.write(_struct_2I.pack(_x.height, _x.width)) 01318 _x = _v35.distortion_model 01319 length = len(_x) 01320 buff.write(struct.pack('<I%ss'%length, length, _x)) 01321 length = len(_v35.D) 01322 buff.write(_struct_I.pack(length)) 01323 pattern = '<%sd'%length 01324 buff.write(struct.pack(pattern, *_v35.D)) 01325 buff.write(_struct_9d.pack(*_v35.K)) 01326 buff.write(_struct_9d.pack(*_v35.R)) 01327 buff.write(_struct_12d.pack(*_v35.P)) 01328 _x = _v35 01329 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01330 _v38 = _v35.roi 01331 _x = _v38 01332 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01333 _v39 = _v25.roi_box_pose 01334 _v40 = _v39.header 01335 buff.write(_struct_I.pack(_v40.seq)) 01336 _v41 = _v40.stamp 01337 _x = _v41 01338 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01339 _x = _v40.frame_id 01340 length = len(_x) 01341 buff.write(struct.pack('<I%ss'%length, length, _x)) 01342 _v42 = _v39.pose 01343 _v43 = _v42.position 01344 _x = _v43 01345 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01346 _v44 = _v42.orientation 01347 _x = _v44 01348 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01349 _v45 = _v25.roi_box_dims 01350 _x = _v45 01351 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01352 _x = val2.collision_name 01353 length = len(_x) 01354 buff.write(struct.pack('<I%ss'%length, length, _x)) 01355 _x = self 01356 buff.write(_struct_3I.pack(_x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs)) 01357 _x = self.action_goal.goal.lift.direction.header.frame_id 01358 length = len(_x) 01359 buff.write(struct.pack('<I%ss'%length, length, _x)) 01360 _x = self 01361 buff.write(_struct_3d2f.pack(_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance)) 01362 _x = self.action_goal.goal.collision_object_name 01363 length = len(_x) 01364 buff.write(struct.pack('<I%ss'%length, length, _x)) 01365 _x = self.action_goal.goal.collision_support_surface_name 01366 length = len(_x) 01367 buff.write(struct.pack('<I%ss'%length, length, _x)) 01368 _x = self 01369 buff.write(_struct_5B.pack(_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_execution, _x.action_goal.goal.use_reactive_lift, _x.action_goal.goal.only_perform_feasibility_test, _x.action_goal.goal.ignore_collisions)) 01370 length = len(self.action_goal.goal.path_constraints.joint_constraints) 01371 buff.write(_struct_I.pack(length)) 01372 for val1 in self.action_goal.goal.path_constraints.joint_constraints: 01373 _x = val1.joint_name 01374 length = len(_x) 01375 buff.write(struct.pack('<I%ss'%length, length, _x)) 01376 _x = val1 01377 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 01378 length = len(self.action_goal.goal.path_constraints.position_constraints) 01379 buff.write(_struct_I.pack(length)) 01380 for val1 in self.action_goal.goal.path_constraints.position_constraints: 01381 _v46 = val1.header 01382 buff.write(_struct_I.pack(_v46.seq)) 01383 _v47 = _v46.stamp 01384 _x = _v47 01385 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01386 _x = _v46.frame_id 01387 length = len(_x) 01388 buff.write(struct.pack('<I%ss'%length, length, _x)) 01389 _x = val1.link_name 01390 length = len(_x) 01391 buff.write(struct.pack('<I%ss'%length, length, _x)) 01392 _v48 = val1.target_point_offset 01393 _x = _v48 01394 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01395 _v49 = val1.position 01396 _x = _v49 01397 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01398 _v50 = val1.constraint_region_shape 01399 buff.write(_struct_b.pack(_v50.type)) 01400 length = len(_v50.dimensions) 01401 buff.write(_struct_I.pack(length)) 01402 pattern = '<%sd'%length 01403 buff.write(struct.pack(pattern, *_v50.dimensions)) 01404 length = len(_v50.triangles) 01405 buff.write(_struct_I.pack(length)) 01406 pattern = '<%si'%length 01407 buff.write(struct.pack(pattern, *_v50.triangles)) 01408 length = len(_v50.vertices) 01409 buff.write(_struct_I.pack(length)) 01410 for val3 in _v50.vertices: 01411 _x = val3 01412 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01413 _v51 = val1.constraint_region_orientation 01414 _x = _v51 01415 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01416 buff.write(_struct_d.pack(val1.weight)) 01417 length = len(self.action_goal.goal.path_constraints.orientation_constraints) 01418 buff.write(_struct_I.pack(length)) 01419 for val1 in self.action_goal.goal.path_constraints.orientation_constraints: 01420 _v52 = val1.header 01421 buff.write(_struct_I.pack(_v52.seq)) 01422 _v53 = _v52.stamp 01423 _x = _v53 01424 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01425 _x = _v52.frame_id 01426 length = len(_x) 01427 buff.write(struct.pack('<I%ss'%length, length, _x)) 01428 _x = val1.link_name 01429 length = len(_x) 01430 buff.write(struct.pack('<I%ss'%length, length, _x)) 01431 buff.write(_struct_i.pack(val1.type)) 01432 _v54 = val1.orientation 01433 _x = _v54 01434 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01435 _x = val1 01436 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 01437 length = len(self.action_goal.goal.path_constraints.visibility_constraints) 01438 buff.write(_struct_I.pack(length)) 01439 for val1 in self.action_goal.goal.path_constraints.visibility_constraints: 01440 _v55 = val1.header 01441 buff.write(_struct_I.pack(_v55.seq)) 01442 _v56 = _v55.stamp 01443 _x = _v56 01444 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01445 _x = _v55.frame_id 01446 length = len(_x) 01447 buff.write(struct.pack('<I%ss'%length, length, _x)) 01448 _v57 = val1.target 01449 _v58 = _v57.header 01450 buff.write(_struct_I.pack(_v58.seq)) 01451 _v59 = _v58.stamp 01452 _x = _v59 01453 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01454 _x = _v58.frame_id 01455 length = len(_x) 01456 buff.write(struct.pack('<I%ss'%length, length, _x)) 01457 _v60 = _v57.point 01458 _x = _v60 01459 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01460 _v61 = val1.sensor_pose 01461 _v62 = _v61.header 01462 buff.write(_struct_I.pack(_v62.seq)) 01463 _v63 = _v62.stamp 01464 _x = _v63 01465 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01466 _x = _v62.frame_id 01467 length = len(_x) 01468 buff.write(struct.pack('<I%ss'%length, length, _x)) 01469 _v64 = _v61.pose 01470 _v65 = _v64.position 01471 _x = _v65 01472 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01473 _v66 = _v64.orientation 01474 _x = _v66 01475 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01476 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01477 length = len(self.action_goal.goal.additional_collision_operations.collision_operations) 01478 buff.write(_struct_I.pack(length)) 01479 for val1 in self.action_goal.goal.additional_collision_operations.collision_operations: 01480 _x = val1.object1 01481 length = len(_x) 01482 buff.write(struct.pack('<I%ss'%length, length, _x)) 01483 _x = val1.object2 01484 length = len(_x) 01485 buff.write(struct.pack('<I%ss'%length, length, _x)) 01486 _x = val1 01487 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 01488 length = len(self.action_goal.goal.additional_link_padding) 01489 buff.write(_struct_I.pack(length)) 01490 for val1 in self.action_goal.goal.additional_link_padding: 01491 _x = val1.link_name 01492 length = len(_x) 01493 buff.write(struct.pack('<I%ss'%length, length, _x)) 01494 buff.write(_struct_d.pack(val1.padding)) 01495 length = len(self.action_goal.goal.movable_obstacles) 01496 buff.write(_struct_I.pack(length)) 01497 for val1 in self.action_goal.goal.movable_obstacles: 01498 _x = val1.reference_frame_id 01499 length = len(_x) 01500 buff.write(struct.pack('<I%ss'%length, length, _x)) 01501 length = len(val1.potential_models) 01502 buff.write(_struct_I.pack(length)) 01503 for val2 in val1.potential_models: 01504 buff.write(_struct_i.pack(val2.model_id)) 01505 _v67 = val2.pose 01506 _v68 = _v67.header 01507 buff.write(_struct_I.pack(_v68.seq)) 01508 _v69 = _v68.stamp 01509 _x = _v69 01510 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01511 _x = _v68.frame_id 01512 length = len(_x) 01513 buff.write(struct.pack('<I%ss'%length, length, _x)) 01514 _v70 = _v67.pose 01515 _v71 = _v70.position 01516 _x = _v71 01517 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01518 _v72 = _v70.orientation 01519 _x = _v72 01520 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01521 buff.write(_struct_f.pack(val2.confidence)) 01522 _x = val2.detector_name 01523 length = len(_x) 01524 buff.write(struct.pack('<I%ss'%length, length, _x)) 01525 _v73 = val1.cluster 01526 _v74 = _v73.header 01527 buff.write(_struct_I.pack(_v74.seq)) 01528 _v75 = _v74.stamp 01529 _x = _v75 01530 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01531 _x = _v74.frame_id 01532 length = len(_x) 01533 buff.write(struct.pack('<I%ss'%length, length, _x)) 01534 length = len(_v73.points) 01535 buff.write(_struct_I.pack(length)) 01536 for val3 in _v73.points: 01537 _x = val3 01538 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01539 length = len(_v73.channels) 01540 buff.write(_struct_I.pack(length)) 01541 for val3 in _v73.channels: 01542 _x = val3.name 01543 length = len(_x) 01544 buff.write(struct.pack('<I%ss'%length, length, _x)) 01545 length = len(val3.values) 01546 buff.write(_struct_I.pack(length)) 01547 pattern = '<%sf'%length 01548 buff.write(struct.pack(pattern, *val3.values)) 01549 _v76 = val1.region 01550 _v77 = _v76.cloud 01551 _v78 = _v77.header 01552 buff.write(_struct_I.pack(_v78.seq)) 01553 _v79 = _v78.stamp 01554 _x = _v79 01555 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01556 _x = _v78.frame_id 01557 length = len(_x) 01558 buff.write(struct.pack('<I%ss'%length, length, _x)) 01559 _x = _v77 01560 buff.write(_struct_2I.pack(_x.height, _x.width)) 01561 length = len(_v77.fields) 01562 buff.write(_struct_I.pack(length)) 01563 for val4 in _v77.fields: 01564 _x = val4.name 01565 length = len(_x) 01566 buff.write(struct.pack('<I%ss'%length, length, _x)) 01567 _x = val4 01568 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01569 _x = _v77 01570 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01571 _x = _v77.data 01572 length = len(_x) 01573 # - if encoded as a list instead, serialize as bytes instead of string 01574 if type(_x) in [list, tuple]: 01575 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01576 else: 01577 buff.write(struct.pack('<I%ss'%length, length, _x)) 01578 buff.write(_struct_B.pack(_v77.is_dense)) 01579 length = len(_v76.mask) 01580 buff.write(_struct_I.pack(length)) 01581 pattern = '<%si'%length 01582 buff.write(struct.pack(pattern, *_v76.mask)) 01583 _v80 = _v76.image 01584 _v81 = _v80.header 01585 buff.write(_struct_I.pack(_v81.seq)) 01586 _v82 = _v81.stamp 01587 _x = _v82 01588 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01589 _x = _v81.frame_id 01590 length = len(_x) 01591 buff.write(struct.pack('<I%ss'%length, length, _x)) 01592 _x = _v80 01593 buff.write(_struct_2I.pack(_x.height, _x.width)) 01594 _x = _v80.encoding 01595 length = len(_x) 01596 buff.write(struct.pack('<I%ss'%length, length, _x)) 01597 _x = _v80 01598 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01599 _x = _v80.data 01600 length = len(_x) 01601 # - if encoded as a list instead, serialize as bytes instead of string 01602 if type(_x) in [list, tuple]: 01603 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01604 else: 01605 buff.write(struct.pack('<I%ss'%length, length, _x)) 01606 _v83 = _v76.disparity_image 01607 _v84 = _v83.header 01608 buff.write(_struct_I.pack(_v84.seq)) 01609 _v85 = _v84.stamp 01610 _x = _v85 01611 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01612 _x = _v84.frame_id 01613 length = len(_x) 01614 buff.write(struct.pack('<I%ss'%length, length, _x)) 01615 _x = _v83 01616 buff.write(_struct_2I.pack(_x.height, _x.width)) 01617 _x = _v83.encoding 01618 length = len(_x) 01619 buff.write(struct.pack('<I%ss'%length, length, _x)) 01620 _x = _v83 01621 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01622 _x = _v83.data 01623 length = len(_x) 01624 # - if encoded as a list instead, serialize as bytes instead of string 01625 if type(_x) in [list, tuple]: 01626 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01627 else: 01628 buff.write(struct.pack('<I%ss'%length, length, _x)) 01629 _v86 = _v76.cam_info 01630 _v87 = _v86.header 01631 buff.write(_struct_I.pack(_v87.seq)) 01632 _v88 = _v87.stamp 01633 _x = _v88 01634 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01635 _x = _v87.frame_id 01636 length = len(_x) 01637 buff.write(struct.pack('<I%ss'%length, length, _x)) 01638 _x = _v86 01639 buff.write(_struct_2I.pack(_x.height, _x.width)) 01640 _x = _v86.distortion_model 01641 length = len(_x) 01642 buff.write(struct.pack('<I%ss'%length, length, _x)) 01643 length = len(_v86.D) 01644 buff.write(_struct_I.pack(length)) 01645 pattern = '<%sd'%length 01646 buff.write(struct.pack(pattern, *_v86.D)) 01647 buff.write(_struct_9d.pack(*_v86.K)) 01648 buff.write(_struct_9d.pack(*_v86.R)) 01649 buff.write(_struct_12d.pack(*_v86.P)) 01650 _x = _v86 01651 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01652 _v89 = _v86.roi 01653 _x = _v89 01654 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01655 _v90 = _v76.roi_box_pose 01656 _v91 = _v90.header 01657 buff.write(_struct_I.pack(_v91.seq)) 01658 _v92 = _v91.stamp 01659 _x = _v92 01660 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01661 _x = _v91.frame_id 01662 length = len(_x) 01663 buff.write(struct.pack('<I%ss'%length, length, _x)) 01664 _v93 = _v90.pose 01665 _v94 = _v93.position 01666 _x = _v94 01667 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01668 _v95 = _v93.orientation 01669 _x = _v95 01670 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01671 _v96 = _v76.roi_box_dims 01672 _x = _v96 01673 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01674 _x = val1.collision_name 01675 length = len(_x) 01676 buff.write(struct.pack('<I%ss'%length, length, _x)) 01677 _x = self 01678 buff.write(_struct_f3I.pack(_x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 01679 _x = self.action_result.header.frame_id 01680 length = len(_x) 01681 buff.write(struct.pack('<I%ss'%length, length, _x)) 01682 _x = self 01683 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 01684 _x = self.action_result.status.goal_id.id 01685 length = len(_x) 01686 buff.write(struct.pack('<I%ss'%length, length, _x)) 01687 buff.write(_struct_B.pack(self.action_result.status.status)) 01688 _x = self.action_result.status.text 01689 length = len(_x) 01690 buff.write(struct.pack('<I%ss'%length, length, _x)) 01691 _x = self 01692 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_result.result.grasp.pre_grasp_posture.header.seq, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.secs, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.nsecs)) 01693 _x = self.action_result.result.grasp.pre_grasp_posture.header.frame_id 01694 length = len(_x) 01695 buff.write(struct.pack('<I%ss'%length, length, _x)) 01696 length = len(self.action_result.result.grasp.pre_grasp_posture.name) 01697 buff.write(_struct_I.pack(length)) 01698 for val1 in self.action_result.result.grasp.pre_grasp_posture.name: 01699 length = len(val1) 01700 buff.write(struct.pack('<I%ss'%length, length, val1)) 01701 length = len(self.action_result.result.grasp.pre_grasp_posture.position) 01702 buff.write(_struct_I.pack(length)) 01703 pattern = '<%sd'%length 01704 buff.write(struct.pack(pattern, *self.action_result.result.grasp.pre_grasp_posture.position)) 01705 length = len(self.action_result.result.grasp.pre_grasp_posture.velocity) 01706 buff.write(_struct_I.pack(length)) 01707 pattern = '<%sd'%length 01708 buff.write(struct.pack(pattern, *self.action_result.result.grasp.pre_grasp_posture.velocity)) 01709 length = len(self.action_result.result.grasp.pre_grasp_posture.effort) 01710 buff.write(_struct_I.pack(length)) 01711 pattern = '<%sd'%length 01712 buff.write(struct.pack(pattern, *self.action_result.result.grasp.pre_grasp_posture.effort)) 01713 _x = self 01714 buff.write(_struct_3I.pack(_x.action_result.result.grasp.grasp_posture.header.seq, _x.action_result.result.grasp.grasp_posture.header.stamp.secs, _x.action_result.result.grasp.grasp_posture.header.stamp.nsecs)) 01715 _x = self.action_result.result.grasp.grasp_posture.header.frame_id 01716 length = len(_x) 01717 buff.write(struct.pack('<I%ss'%length, length, _x)) 01718 length = len(self.action_result.result.grasp.grasp_posture.name) 01719 buff.write(_struct_I.pack(length)) 01720 for val1 in self.action_result.result.grasp.grasp_posture.name: 01721 length = len(val1) 01722 buff.write(struct.pack('<I%ss'%length, length, val1)) 01723 length = len(self.action_result.result.grasp.grasp_posture.position) 01724 buff.write(_struct_I.pack(length)) 01725 pattern = '<%sd'%length 01726 buff.write(struct.pack(pattern, *self.action_result.result.grasp.grasp_posture.position)) 01727 length = len(self.action_result.result.grasp.grasp_posture.velocity) 01728 buff.write(_struct_I.pack(length)) 01729 pattern = '<%sd'%length 01730 buff.write(struct.pack(pattern, *self.action_result.result.grasp.grasp_posture.velocity)) 01731 length = len(self.action_result.result.grasp.grasp_posture.effort) 01732 buff.write(_struct_I.pack(length)) 01733 pattern = '<%sd'%length 01734 buff.write(struct.pack(pattern, *self.action_result.result.grasp.grasp_posture.effort)) 01735 _x = self 01736 buff.write(_struct_8dB2f.pack(_x.action_result.result.grasp.grasp_pose.position.x, _x.action_result.result.grasp.grasp_pose.position.y, _x.action_result.result.grasp.grasp_pose.position.z, _x.action_result.result.grasp.grasp_pose.orientation.x, _x.action_result.result.grasp.grasp_pose.orientation.y, _x.action_result.result.grasp.grasp_pose.orientation.z, _x.action_result.result.grasp.grasp_pose.orientation.w, _x.action_result.result.grasp.success_probability, _x.action_result.result.grasp.cluster_rep, _x.action_result.result.grasp.desired_approach_distance, _x.action_result.result.grasp.min_approach_distance)) 01737 length = len(self.action_result.result.grasp.moved_obstacles) 01738 buff.write(_struct_I.pack(length)) 01739 for val1 in self.action_result.result.grasp.moved_obstacles: 01740 _x = val1.reference_frame_id 01741 length = len(_x) 01742 buff.write(struct.pack('<I%ss'%length, length, _x)) 01743 length = len(val1.potential_models) 01744 buff.write(_struct_I.pack(length)) 01745 for val2 in val1.potential_models: 01746 buff.write(_struct_i.pack(val2.model_id)) 01747 _v97 = val2.pose 01748 _v98 = _v97.header 01749 buff.write(_struct_I.pack(_v98.seq)) 01750 _v99 = _v98.stamp 01751 _x = _v99 01752 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01753 _x = _v98.frame_id 01754 length = len(_x) 01755 buff.write(struct.pack('<I%ss'%length, length, _x)) 01756 _v100 = _v97.pose 01757 _v101 = _v100.position 01758 _x = _v101 01759 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01760 _v102 = _v100.orientation 01761 _x = _v102 01762 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01763 buff.write(_struct_f.pack(val2.confidence)) 01764 _x = val2.detector_name 01765 length = len(_x) 01766 buff.write(struct.pack('<I%ss'%length, length, _x)) 01767 _v103 = val1.cluster 01768 _v104 = _v103.header 01769 buff.write(_struct_I.pack(_v104.seq)) 01770 _v105 = _v104.stamp 01771 _x = _v105 01772 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01773 _x = _v104.frame_id 01774 length = len(_x) 01775 buff.write(struct.pack('<I%ss'%length, length, _x)) 01776 length = len(_v103.points) 01777 buff.write(_struct_I.pack(length)) 01778 for val3 in _v103.points: 01779 _x = val3 01780 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01781 length = len(_v103.channels) 01782 buff.write(_struct_I.pack(length)) 01783 for val3 in _v103.channels: 01784 _x = val3.name 01785 length = len(_x) 01786 buff.write(struct.pack('<I%ss'%length, length, _x)) 01787 length = len(val3.values) 01788 buff.write(_struct_I.pack(length)) 01789 pattern = '<%sf'%length 01790 buff.write(struct.pack(pattern, *val3.values)) 01791 _v106 = val1.region 01792 _v107 = _v106.cloud 01793 _v108 = _v107.header 01794 buff.write(_struct_I.pack(_v108.seq)) 01795 _v109 = _v108.stamp 01796 _x = _v109 01797 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01798 _x = _v108.frame_id 01799 length = len(_x) 01800 buff.write(struct.pack('<I%ss'%length, length, _x)) 01801 _x = _v107 01802 buff.write(_struct_2I.pack(_x.height, _x.width)) 01803 length = len(_v107.fields) 01804 buff.write(_struct_I.pack(length)) 01805 for val4 in _v107.fields: 01806 _x = val4.name 01807 length = len(_x) 01808 buff.write(struct.pack('<I%ss'%length, length, _x)) 01809 _x = val4 01810 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01811 _x = _v107 01812 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01813 _x = _v107.data 01814 length = len(_x) 01815 # - if encoded as a list instead, serialize as bytes instead of string 01816 if type(_x) in [list, tuple]: 01817 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01818 else: 01819 buff.write(struct.pack('<I%ss'%length, length, _x)) 01820 buff.write(_struct_B.pack(_v107.is_dense)) 01821 length = len(_v106.mask) 01822 buff.write(_struct_I.pack(length)) 01823 pattern = '<%si'%length 01824 buff.write(struct.pack(pattern, *_v106.mask)) 01825 _v110 = _v106.image 01826 _v111 = _v110.header 01827 buff.write(_struct_I.pack(_v111.seq)) 01828 _v112 = _v111.stamp 01829 _x = _v112 01830 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01831 _x = _v111.frame_id 01832 length = len(_x) 01833 buff.write(struct.pack('<I%ss'%length, length, _x)) 01834 _x = _v110 01835 buff.write(_struct_2I.pack(_x.height, _x.width)) 01836 _x = _v110.encoding 01837 length = len(_x) 01838 buff.write(struct.pack('<I%ss'%length, length, _x)) 01839 _x = _v110 01840 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01841 _x = _v110.data 01842 length = len(_x) 01843 # - if encoded as a list instead, serialize as bytes instead of string 01844 if type(_x) in [list, tuple]: 01845 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01846 else: 01847 buff.write(struct.pack('<I%ss'%length, length, _x)) 01848 _v113 = _v106.disparity_image 01849 _v114 = _v113.header 01850 buff.write(_struct_I.pack(_v114.seq)) 01851 _v115 = _v114.stamp 01852 _x = _v115 01853 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01854 _x = _v114.frame_id 01855 length = len(_x) 01856 buff.write(struct.pack('<I%ss'%length, length, _x)) 01857 _x = _v113 01858 buff.write(_struct_2I.pack(_x.height, _x.width)) 01859 _x = _v113.encoding 01860 length = len(_x) 01861 buff.write(struct.pack('<I%ss'%length, length, _x)) 01862 _x = _v113 01863 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01864 _x = _v113.data 01865 length = len(_x) 01866 # - if encoded as a list instead, serialize as bytes instead of string 01867 if type(_x) in [list, tuple]: 01868 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01869 else: 01870 buff.write(struct.pack('<I%ss'%length, length, _x)) 01871 _v116 = _v106.cam_info 01872 _v117 = _v116.header 01873 buff.write(_struct_I.pack(_v117.seq)) 01874 _v118 = _v117.stamp 01875 _x = _v118 01876 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01877 _x = _v117.frame_id 01878 length = len(_x) 01879 buff.write(struct.pack('<I%ss'%length, length, _x)) 01880 _x = _v116 01881 buff.write(_struct_2I.pack(_x.height, _x.width)) 01882 _x = _v116.distortion_model 01883 length = len(_x) 01884 buff.write(struct.pack('<I%ss'%length, length, _x)) 01885 length = len(_v116.D) 01886 buff.write(_struct_I.pack(length)) 01887 pattern = '<%sd'%length 01888 buff.write(struct.pack(pattern, *_v116.D)) 01889 buff.write(_struct_9d.pack(*_v116.K)) 01890 buff.write(_struct_9d.pack(*_v116.R)) 01891 buff.write(_struct_12d.pack(*_v116.P)) 01892 _x = _v116 01893 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01894 _v119 = _v116.roi 01895 _x = _v119 01896 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01897 _v120 = _v106.roi_box_pose 01898 _v121 = _v120.header 01899 buff.write(_struct_I.pack(_v121.seq)) 01900 _v122 = _v121.stamp 01901 _x = _v122 01902 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01903 _x = _v121.frame_id 01904 length = len(_x) 01905 buff.write(struct.pack('<I%ss'%length, length, _x)) 01906 _v123 = _v120.pose 01907 _v124 = _v123.position 01908 _x = _v124 01909 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01910 _v125 = _v123.orientation 01911 _x = _v125 01912 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01913 _v126 = _v106.roi_box_dims 01914 _x = _v126 01915 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01916 _x = val1.collision_name 01917 length = len(_x) 01918 buff.write(struct.pack('<I%ss'%length, length, _x)) 01919 length = len(self.action_result.result.attempted_grasps) 01920 buff.write(_struct_I.pack(length)) 01921 for val1 in self.action_result.result.attempted_grasps: 01922 _v127 = val1.pre_grasp_posture 01923 _v128 = _v127.header 01924 buff.write(_struct_I.pack(_v128.seq)) 01925 _v129 = _v128.stamp 01926 _x = _v129 01927 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01928 _x = _v128.frame_id 01929 length = len(_x) 01930 buff.write(struct.pack('<I%ss'%length, length, _x)) 01931 length = len(_v127.name) 01932 buff.write(_struct_I.pack(length)) 01933 for val3 in _v127.name: 01934 length = len(val3) 01935 buff.write(struct.pack('<I%ss'%length, length, val3)) 01936 length = len(_v127.position) 01937 buff.write(_struct_I.pack(length)) 01938 pattern = '<%sd'%length 01939 buff.write(struct.pack(pattern, *_v127.position)) 01940 length = len(_v127.velocity) 01941 buff.write(_struct_I.pack(length)) 01942 pattern = '<%sd'%length 01943 buff.write(struct.pack(pattern, *_v127.velocity)) 01944 length = len(_v127.effort) 01945 buff.write(_struct_I.pack(length)) 01946 pattern = '<%sd'%length 01947 buff.write(struct.pack(pattern, *_v127.effort)) 01948 _v130 = val1.grasp_posture 01949 _v131 = _v130.header 01950 buff.write(_struct_I.pack(_v131.seq)) 01951 _v132 = _v131.stamp 01952 _x = _v132 01953 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01954 _x = _v131.frame_id 01955 length = len(_x) 01956 buff.write(struct.pack('<I%ss'%length, length, _x)) 01957 length = len(_v130.name) 01958 buff.write(_struct_I.pack(length)) 01959 for val3 in _v130.name: 01960 length = len(val3) 01961 buff.write(struct.pack('<I%ss'%length, length, val3)) 01962 length = len(_v130.position) 01963 buff.write(_struct_I.pack(length)) 01964 pattern = '<%sd'%length 01965 buff.write(struct.pack(pattern, *_v130.position)) 01966 length = len(_v130.velocity) 01967 buff.write(_struct_I.pack(length)) 01968 pattern = '<%sd'%length 01969 buff.write(struct.pack(pattern, *_v130.velocity)) 01970 length = len(_v130.effort) 01971 buff.write(_struct_I.pack(length)) 01972 pattern = '<%sd'%length 01973 buff.write(struct.pack(pattern, *_v130.effort)) 01974 _v133 = val1.grasp_pose 01975 _v134 = _v133.position 01976 _x = _v134 01977 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01978 _v135 = _v133.orientation 01979 _x = _v135 01980 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01981 _x = val1 01982 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 01983 length = len(val1.moved_obstacles) 01984 buff.write(_struct_I.pack(length)) 01985 for val2 in val1.moved_obstacles: 01986 _x = val2.reference_frame_id 01987 length = len(_x) 01988 buff.write(struct.pack('<I%ss'%length, length, _x)) 01989 length = len(val2.potential_models) 01990 buff.write(_struct_I.pack(length)) 01991 for val3 in val2.potential_models: 01992 buff.write(_struct_i.pack(val3.model_id)) 01993 _v136 = val3.pose 01994 _v137 = _v136.header 01995 buff.write(_struct_I.pack(_v137.seq)) 01996 _v138 = _v137.stamp 01997 _x = _v138 01998 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01999 _x = _v137.frame_id 02000 length = len(_x) 02001 buff.write(struct.pack('<I%ss'%length, length, _x)) 02002 _v139 = _v136.pose 02003 _v140 = _v139.position 02004 _x = _v140 02005 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02006 _v141 = _v139.orientation 02007 _x = _v141 02008 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02009 buff.write(_struct_f.pack(val3.confidence)) 02010 _x = val3.detector_name 02011 length = len(_x) 02012 buff.write(struct.pack('<I%ss'%length, length, _x)) 02013 _v142 = val2.cluster 02014 _v143 = _v142.header 02015 buff.write(_struct_I.pack(_v143.seq)) 02016 _v144 = _v143.stamp 02017 _x = _v144 02018 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02019 _x = _v143.frame_id 02020 length = len(_x) 02021 buff.write(struct.pack('<I%ss'%length, length, _x)) 02022 length = len(_v142.points) 02023 buff.write(_struct_I.pack(length)) 02024 for val4 in _v142.points: 02025 _x = val4 02026 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02027 length = len(_v142.channels) 02028 buff.write(_struct_I.pack(length)) 02029 for val4 in _v142.channels: 02030 _x = val4.name 02031 length = len(_x) 02032 buff.write(struct.pack('<I%ss'%length, length, _x)) 02033 length = len(val4.values) 02034 buff.write(_struct_I.pack(length)) 02035 pattern = '<%sf'%length 02036 buff.write(struct.pack(pattern, *val4.values)) 02037 _v145 = val2.region 02038 _v146 = _v145.cloud 02039 _v147 = _v146.header 02040 buff.write(_struct_I.pack(_v147.seq)) 02041 _v148 = _v147.stamp 02042 _x = _v148 02043 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02044 _x = _v147.frame_id 02045 length = len(_x) 02046 buff.write(struct.pack('<I%ss'%length, length, _x)) 02047 _x = _v146 02048 buff.write(_struct_2I.pack(_x.height, _x.width)) 02049 length = len(_v146.fields) 02050 buff.write(_struct_I.pack(length)) 02051 for val5 in _v146.fields: 02052 _x = val5.name 02053 length = len(_x) 02054 buff.write(struct.pack('<I%ss'%length, length, _x)) 02055 _x = val5 02056 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02057 _x = _v146 02058 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 02059 _x = _v146.data 02060 length = len(_x) 02061 # - if encoded as a list instead, serialize as bytes instead of string 02062 if type(_x) in [list, tuple]: 02063 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02064 else: 02065 buff.write(struct.pack('<I%ss'%length, length, _x)) 02066 buff.write(_struct_B.pack(_v146.is_dense)) 02067 length = len(_v145.mask) 02068 buff.write(_struct_I.pack(length)) 02069 pattern = '<%si'%length 02070 buff.write(struct.pack(pattern, *_v145.mask)) 02071 _v149 = _v145.image 02072 _v150 = _v149.header 02073 buff.write(_struct_I.pack(_v150.seq)) 02074 _v151 = _v150.stamp 02075 _x = _v151 02076 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02077 _x = _v150.frame_id 02078 length = len(_x) 02079 buff.write(struct.pack('<I%ss'%length, length, _x)) 02080 _x = _v149 02081 buff.write(_struct_2I.pack(_x.height, _x.width)) 02082 _x = _v149.encoding 02083 length = len(_x) 02084 buff.write(struct.pack('<I%ss'%length, length, _x)) 02085 _x = _v149 02086 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02087 _x = _v149.data 02088 length = len(_x) 02089 # - if encoded as a list instead, serialize as bytes instead of string 02090 if type(_x) in [list, tuple]: 02091 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02092 else: 02093 buff.write(struct.pack('<I%ss'%length, length, _x)) 02094 _v152 = _v145.disparity_image 02095 _v153 = _v152.header 02096 buff.write(_struct_I.pack(_v153.seq)) 02097 _v154 = _v153.stamp 02098 _x = _v154 02099 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02100 _x = _v153.frame_id 02101 length = len(_x) 02102 buff.write(struct.pack('<I%ss'%length, length, _x)) 02103 _x = _v152 02104 buff.write(_struct_2I.pack(_x.height, _x.width)) 02105 _x = _v152.encoding 02106 length = len(_x) 02107 buff.write(struct.pack('<I%ss'%length, length, _x)) 02108 _x = _v152 02109 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02110 _x = _v152.data 02111 length = len(_x) 02112 # - if encoded as a list instead, serialize as bytes instead of string 02113 if type(_x) in [list, tuple]: 02114 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02115 else: 02116 buff.write(struct.pack('<I%ss'%length, length, _x)) 02117 _v155 = _v145.cam_info 02118 _v156 = _v155.header 02119 buff.write(_struct_I.pack(_v156.seq)) 02120 _v157 = _v156.stamp 02121 _x = _v157 02122 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02123 _x = _v156.frame_id 02124 length = len(_x) 02125 buff.write(struct.pack('<I%ss'%length, length, _x)) 02126 _x = _v155 02127 buff.write(_struct_2I.pack(_x.height, _x.width)) 02128 _x = _v155.distortion_model 02129 length = len(_x) 02130 buff.write(struct.pack('<I%ss'%length, length, _x)) 02131 length = len(_v155.D) 02132 buff.write(_struct_I.pack(length)) 02133 pattern = '<%sd'%length 02134 buff.write(struct.pack(pattern, *_v155.D)) 02135 buff.write(_struct_9d.pack(*_v155.K)) 02136 buff.write(_struct_9d.pack(*_v155.R)) 02137 buff.write(_struct_12d.pack(*_v155.P)) 02138 _x = _v155 02139 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02140 _v158 = _v155.roi 02141 _x = _v158 02142 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02143 _v159 = _v145.roi_box_pose 02144 _v160 = _v159.header 02145 buff.write(_struct_I.pack(_v160.seq)) 02146 _v161 = _v160.stamp 02147 _x = _v161 02148 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02149 _x = _v160.frame_id 02150 length = len(_x) 02151 buff.write(struct.pack('<I%ss'%length, length, _x)) 02152 _v162 = _v159.pose 02153 _v163 = _v162.position 02154 _x = _v163 02155 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02156 _v164 = _v162.orientation 02157 _x = _v164 02158 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02159 _v165 = _v145.roi_box_dims 02160 _x = _v165 02161 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02162 _x = val2.collision_name 02163 length = len(_x) 02164 buff.write(struct.pack('<I%ss'%length, length, _x)) 02165 length = len(self.action_result.result.attempted_grasp_results) 02166 buff.write(_struct_I.pack(length)) 02167 for val1 in self.action_result.result.attempted_grasp_results: 02168 _x = val1 02169 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible)) 02170 _x = self 02171 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 02172 _x = self.action_feedback.header.frame_id 02173 length = len(_x) 02174 buff.write(struct.pack('<I%ss'%length, length, _x)) 02175 _x = self 02176 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 02177 _x = self.action_feedback.status.goal_id.id 02178 length = len(_x) 02179 buff.write(struct.pack('<I%ss'%length, length, _x)) 02180 buff.write(_struct_B.pack(self.action_feedback.status.status)) 02181 _x = self.action_feedback.status.text 02182 length = len(_x) 02183 buff.write(struct.pack('<I%ss'%length, length, _x)) 02184 _x = self 02185 buff.write(_struct_2i.pack(_x.action_feedback.feedback.current_grasp, _x.action_feedback.feedback.total_grasps)) 02186 except struct.error as se: self._check_types(se) 02187 except TypeError as te: self._check_types(te) 02188 02189 def deserialize(self, str): 02190 """ 02191 unpack serialized message in str into this message instance 02192 @param str: byte array of serialized message 02193 @type str: str 02194 """ 02195 try: 02196 if self.action_goal is None: 02197 self.action_goal = object_manipulation_msgs.msg.PickupActionGoal() 02198 if self.action_result is None: 02199 self.action_result = object_manipulation_msgs.msg.PickupActionResult() 02200 if self.action_feedback is None: 02201 self.action_feedback = object_manipulation_msgs.msg.PickupActionFeedback() 02202 end = 0 02203 _x = self 02204 start = end 02205 end += 12 02206 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02207 start = end 02208 end += 4 02209 (length,) = _struct_I.unpack(str[start:end]) 02210 start = end 02211 end += length 02212 self.action_goal.header.frame_id = str[start:end] 02213 _x = self 02214 start = end 02215 end += 8 02216 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02217 start = end 02218 end += 4 02219 (length,) = _struct_I.unpack(str[start:end]) 02220 start = end 02221 end += length 02222 self.action_goal.goal_id.id = str[start:end] 02223 start = end 02224 end += 4 02225 (length,) = _struct_I.unpack(str[start:end]) 02226 start = end 02227 end += length 02228 self.action_goal.goal.arm_name = str[start:end] 02229 start = end 02230 end += 4 02231 (length,) = _struct_I.unpack(str[start:end]) 02232 start = end 02233 end += length 02234 self.action_goal.goal.target.reference_frame_id = str[start:end] 02235 start = end 02236 end += 4 02237 (length,) = _struct_I.unpack(str[start:end]) 02238 self.action_goal.goal.target.potential_models = [] 02239 for i in range(0, length): 02240 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 02241 start = end 02242 end += 4 02243 (val1.model_id,) = _struct_i.unpack(str[start:end]) 02244 _v166 = val1.pose 02245 _v167 = _v166.header 02246 start = end 02247 end += 4 02248 (_v167.seq,) = _struct_I.unpack(str[start:end]) 02249 _v168 = _v167.stamp 02250 _x = _v168 02251 start = end 02252 end += 8 02253 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02254 start = end 02255 end += 4 02256 (length,) = _struct_I.unpack(str[start:end]) 02257 start = end 02258 end += length 02259 _v167.frame_id = str[start:end] 02260 _v169 = _v166.pose 02261 _v170 = _v169.position 02262 _x = _v170 02263 start = end 02264 end += 24 02265 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02266 _v171 = _v169.orientation 02267 _x = _v171 02268 start = end 02269 end += 32 02270 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02271 start = end 02272 end += 4 02273 (val1.confidence,) = _struct_f.unpack(str[start:end]) 02274 start = end 02275 end += 4 02276 (length,) = _struct_I.unpack(str[start:end]) 02277 start = end 02278 end += length 02279 val1.detector_name = str[start:end] 02280 self.action_goal.goal.target.potential_models.append(val1) 02281 _x = self 02282 start = end 02283 end += 12 02284 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02285 start = end 02286 end += 4 02287 (length,) = _struct_I.unpack(str[start:end]) 02288 start = end 02289 end += length 02290 self.action_goal.goal.target.cluster.header.frame_id = str[start:end] 02291 start = end 02292 end += 4 02293 (length,) = _struct_I.unpack(str[start:end]) 02294 self.action_goal.goal.target.cluster.points = [] 02295 for i in range(0, length): 02296 val1 = geometry_msgs.msg.Point32() 02297 _x = val1 02298 start = end 02299 end += 12 02300 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02301 self.action_goal.goal.target.cluster.points.append(val1) 02302 start = end 02303 end += 4 02304 (length,) = _struct_I.unpack(str[start:end]) 02305 self.action_goal.goal.target.cluster.channels = [] 02306 for i in range(0, length): 02307 val1 = sensor_msgs.msg.ChannelFloat32() 02308 start = end 02309 end += 4 02310 (length,) = _struct_I.unpack(str[start:end]) 02311 start = end 02312 end += length 02313 val1.name = str[start:end] 02314 start = end 02315 end += 4 02316 (length,) = _struct_I.unpack(str[start:end]) 02317 pattern = '<%sf'%length 02318 start = end 02319 end += struct.calcsize(pattern) 02320 val1.values = struct.unpack(pattern, str[start:end]) 02321 self.action_goal.goal.target.cluster.channels.append(val1) 02322 _x = self 02323 start = end 02324 end += 12 02325 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02326 start = end 02327 end += 4 02328 (length,) = _struct_I.unpack(str[start:end]) 02329 start = end 02330 end += length 02331 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end] 02332 _x = self 02333 start = end 02334 end += 8 02335 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 02336 start = end 02337 end += 4 02338 (length,) = _struct_I.unpack(str[start:end]) 02339 self.action_goal.goal.target.region.cloud.fields = [] 02340 for i in range(0, length): 02341 val1 = sensor_msgs.msg.PointField() 02342 start = end 02343 end += 4 02344 (length,) = _struct_I.unpack(str[start:end]) 02345 start = end 02346 end += length 02347 val1.name = str[start:end] 02348 _x = val1 02349 start = end 02350 end += 9 02351 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02352 self.action_goal.goal.target.region.cloud.fields.append(val1) 02353 _x = self 02354 start = end 02355 end += 9 02356 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 02357 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian) 02358 start = end 02359 end += 4 02360 (length,) = _struct_I.unpack(str[start:end]) 02361 start = end 02362 end += length 02363 self.action_goal.goal.target.region.cloud.data = str[start:end] 02364 start = end 02365 end += 1 02366 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 02367 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense) 02368 start = end 02369 end += 4 02370 (length,) = _struct_I.unpack(str[start:end]) 02371 pattern = '<%si'%length 02372 start = end 02373 end += struct.calcsize(pattern) 02374 self.action_goal.goal.target.region.mask = struct.unpack(pattern, str[start:end]) 02375 _x = self 02376 start = end 02377 end += 12 02378 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02379 start = end 02380 end += 4 02381 (length,) = _struct_I.unpack(str[start:end]) 02382 start = end 02383 end += length 02384 self.action_goal.goal.target.region.image.header.frame_id = str[start:end] 02385 _x = self 02386 start = end 02387 end += 8 02388 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 02389 start = end 02390 end += 4 02391 (length,) = _struct_I.unpack(str[start:end]) 02392 start = end 02393 end += length 02394 self.action_goal.goal.target.region.image.encoding = str[start:end] 02395 _x = self 02396 start = end 02397 end += 5 02398 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 02399 start = end 02400 end += 4 02401 (length,) = _struct_I.unpack(str[start:end]) 02402 start = end 02403 end += length 02404 self.action_goal.goal.target.region.image.data = str[start:end] 02405 _x = self 02406 start = end 02407 end += 12 02408 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02409 start = end 02410 end += 4 02411 (length,) = _struct_I.unpack(str[start:end]) 02412 start = end 02413 end += length 02414 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end] 02415 _x = self 02416 start = end 02417 end += 8 02418 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 02419 start = end 02420 end += 4 02421 (length,) = _struct_I.unpack(str[start:end]) 02422 start = end 02423 end += length 02424 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end] 02425 _x = self 02426 start = end 02427 end += 5 02428 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 02429 start = end 02430 end += 4 02431 (length,) = _struct_I.unpack(str[start:end]) 02432 start = end 02433 end += length 02434 self.action_goal.goal.target.region.disparity_image.data = str[start:end] 02435 _x = self 02436 start = end 02437 end += 12 02438 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02439 start = end 02440 end += 4 02441 (length,) = _struct_I.unpack(str[start:end]) 02442 start = end 02443 end += length 02444 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end] 02445 _x = self 02446 start = end 02447 end += 8 02448 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 02449 start = end 02450 end += 4 02451 (length,) = _struct_I.unpack(str[start:end]) 02452 start = end 02453 end += length 02454 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end] 02455 start = end 02456 end += 4 02457 (length,) = _struct_I.unpack(str[start:end]) 02458 pattern = '<%sd'%length 02459 start = end 02460 end += struct.calcsize(pattern) 02461 self.action_goal.goal.target.region.cam_info.D = struct.unpack(pattern, str[start:end]) 02462 start = end 02463 end += 72 02464 self.action_goal.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end]) 02465 start = end 02466 end += 72 02467 self.action_goal.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end]) 02468 start = end 02469 end += 96 02470 self.action_goal.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end]) 02471 _x = self 02472 start = end 02473 end += 37 02474 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 02475 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify) 02476 start = end 02477 end += 4 02478 (length,) = _struct_I.unpack(str[start:end]) 02479 start = end 02480 end += length 02481 self.action_goal.goal.target.region.roi_box_pose.header.frame_id = str[start:end] 02482 _x = self 02483 start = end 02484 end += 80 02485 (_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 02486 start = end 02487 end += 4 02488 (length,) = _struct_I.unpack(str[start:end]) 02489 start = end 02490 end += length 02491 self.action_goal.goal.target.collision_name = str[start:end] 02492 start = end 02493 end += 4 02494 (length,) = _struct_I.unpack(str[start:end]) 02495 self.action_goal.goal.desired_grasps = [] 02496 for i in range(0, length): 02497 val1 = object_manipulation_msgs.msg.Grasp() 02498 _v172 = val1.pre_grasp_posture 02499 _v173 = _v172.header 02500 start = end 02501 end += 4 02502 (_v173.seq,) = _struct_I.unpack(str[start:end]) 02503 _v174 = _v173.stamp 02504 _x = _v174 02505 start = end 02506 end += 8 02507 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02508 start = end 02509 end += 4 02510 (length,) = _struct_I.unpack(str[start:end]) 02511 start = end 02512 end += length 02513 _v173.frame_id = str[start:end] 02514 start = end 02515 end += 4 02516 (length,) = _struct_I.unpack(str[start:end]) 02517 _v172.name = [] 02518 for i in range(0, length): 02519 start = end 02520 end += 4 02521 (length,) = _struct_I.unpack(str[start:end]) 02522 start = end 02523 end += length 02524 val3 = str[start:end] 02525 _v172.name.append(val3) 02526 start = end 02527 end += 4 02528 (length,) = _struct_I.unpack(str[start:end]) 02529 pattern = '<%sd'%length 02530 start = end 02531 end += struct.calcsize(pattern) 02532 _v172.position = struct.unpack(pattern, str[start:end]) 02533 start = end 02534 end += 4 02535 (length,) = _struct_I.unpack(str[start:end]) 02536 pattern = '<%sd'%length 02537 start = end 02538 end += struct.calcsize(pattern) 02539 _v172.velocity = struct.unpack(pattern, str[start:end]) 02540 start = end 02541 end += 4 02542 (length,) = _struct_I.unpack(str[start:end]) 02543 pattern = '<%sd'%length 02544 start = end 02545 end += struct.calcsize(pattern) 02546 _v172.effort = struct.unpack(pattern, str[start:end]) 02547 _v175 = val1.grasp_posture 02548 _v176 = _v175.header 02549 start = end 02550 end += 4 02551 (_v176.seq,) = _struct_I.unpack(str[start:end]) 02552 _v177 = _v176.stamp 02553 _x = _v177 02554 start = end 02555 end += 8 02556 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02557 start = end 02558 end += 4 02559 (length,) = _struct_I.unpack(str[start:end]) 02560 start = end 02561 end += length 02562 _v176.frame_id = str[start:end] 02563 start = end 02564 end += 4 02565 (length,) = _struct_I.unpack(str[start:end]) 02566 _v175.name = [] 02567 for i in range(0, length): 02568 start = end 02569 end += 4 02570 (length,) = _struct_I.unpack(str[start:end]) 02571 start = end 02572 end += length 02573 val3 = str[start:end] 02574 _v175.name.append(val3) 02575 start = end 02576 end += 4 02577 (length,) = _struct_I.unpack(str[start:end]) 02578 pattern = '<%sd'%length 02579 start = end 02580 end += struct.calcsize(pattern) 02581 _v175.position = struct.unpack(pattern, str[start:end]) 02582 start = end 02583 end += 4 02584 (length,) = _struct_I.unpack(str[start:end]) 02585 pattern = '<%sd'%length 02586 start = end 02587 end += struct.calcsize(pattern) 02588 _v175.velocity = struct.unpack(pattern, str[start:end]) 02589 start = end 02590 end += 4 02591 (length,) = _struct_I.unpack(str[start:end]) 02592 pattern = '<%sd'%length 02593 start = end 02594 end += struct.calcsize(pattern) 02595 _v175.effort = struct.unpack(pattern, str[start:end]) 02596 _v178 = val1.grasp_pose 02597 _v179 = _v178.position 02598 _x = _v179 02599 start = end 02600 end += 24 02601 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02602 _v180 = _v178.orientation 02603 _x = _v180 02604 start = end 02605 end += 32 02606 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02607 _x = val1 02608 start = end 02609 end += 17 02610 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 02611 val1.cluster_rep = bool(val1.cluster_rep) 02612 start = end 02613 end += 4 02614 (length,) = _struct_I.unpack(str[start:end]) 02615 val1.moved_obstacles = [] 02616 for i in range(0, length): 02617 val2 = object_manipulation_msgs.msg.GraspableObject() 02618 start = end 02619 end += 4 02620 (length,) = _struct_I.unpack(str[start:end]) 02621 start = end 02622 end += length 02623 val2.reference_frame_id = str[start:end] 02624 start = end 02625 end += 4 02626 (length,) = _struct_I.unpack(str[start:end]) 02627 val2.potential_models = [] 02628 for i in range(0, length): 02629 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 02630 start = end 02631 end += 4 02632 (val3.model_id,) = _struct_i.unpack(str[start:end]) 02633 _v181 = val3.pose 02634 _v182 = _v181.header 02635 start = end 02636 end += 4 02637 (_v182.seq,) = _struct_I.unpack(str[start:end]) 02638 _v183 = _v182.stamp 02639 _x = _v183 02640 start = end 02641 end += 8 02642 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02643 start = end 02644 end += 4 02645 (length,) = _struct_I.unpack(str[start:end]) 02646 start = end 02647 end += length 02648 _v182.frame_id = str[start:end] 02649 _v184 = _v181.pose 02650 _v185 = _v184.position 02651 _x = _v185 02652 start = end 02653 end += 24 02654 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02655 _v186 = _v184.orientation 02656 _x = _v186 02657 start = end 02658 end += 32 02659 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02660 start = end 02661 end += 4 02662 (val3.confidence,) = _struct_f.unpack(str[start:end]) 02663 start = end 02664 end += 4 02665 (length,) = _struct_I.unpack(str[start:end]) 02666 start = end 02667 end += length 02668 val3.detector_name = str[start:end] 02669 val2.potential_models.append(val3) 02670 _v187 = val2.cluster 02671 _v188 = _v187.header 02672 start = end 02673 end += 4 02674 (_v188.seq,) = _struct_I.unpack(str[start:end]) 02675 _v189 = _v188.stamp 02676 _x = _v189 02677 start = end 02678 end += 8 02679 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02680 start = end 02681 end += 4 02682 (length,) = _struct_I.unpack(str[start:end]) 02683 start = end 02684 end += length 02685 _v188.frame_id = str[start:end] 02686 start = end 02687 end += 4 02688 (length,) = _struct_I.unpack(str[start:end]) 02689 _v187.points = [] 02690 for i in range(0, length): 02691 val4 = geometry_msgs.msg.Point32() 02692 _x = val4 02693 start = end 02694 end += 12 02695 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02696 _v187.points.append(val4) 02697 start = end 02698 end += 4 02699 (length,) = _struct_I.unpack(str[start:end]) 02700 _v187.channels = [] 02701 for i in range(0, length): 02702 val4 = sensor_msgs.msg.ChannelFloat32() 02703 start = end 02704 end += 4 02705 (length,) = _struct_I.unpack(str[start:end]) 02706 start = end 02707 end += length 02708 val4.name = str[start:end] 02709 start = end 02710 end += 4 02711 (length,) = _struct_I.unpack(str[start:end]) 02712 pattern = '<%sf'%length 02713 start = end 02714 end += struct.calcsize(pattern) 02715 val4.values = struct.unpack(pattern, str[start:end]) 02716 _v187.channels.append(val4) 02717 _v190 = val2.region 02718 _v191 = _v190.cloud 02719 _v192 = _v191.header 02720 start = end 02721 end += 4 02722 (_v192.seq,) = _struct_I.unpack(str[start:end]) 02723 _v193 = _v192.stamp 02724 _x = _v193 02725 start = end 02726 end += 8 02727 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02728 start = end 02729 end += 4 02730 (length,) = _struct_I.unpack(str[start:end]) 02731 start = end 02732 end += length 02733 _v192.frame_id = str[start:end] 02734 _x = _v191 02735 start = end 02736 end += 8 02737 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02738 start = end 02739 end += 4 02740 (length,) = _struct_I.unpack(str[start:end]) 02741 _v191.fields = [] 02742 for i in range(0, length): 02743 val5 = sensor_msgs.msg.PointField() 02744 start = end 02745 end += 4 02746 (length,) = _struct_I.unpack(str[start:end]) 02747 start = end 02748 end += length 02749 val5.name = str[start:end] 02750 _x = val5 02751 start = end 02752 end += 9 02753 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02754 _v191.fields.append(val5) 02755 _x = _v191 02756 start = end 02757 end += 9 02758 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 02759 _v191.is_bigendian = bool(_v191.is_bigendian) 02760 start = end 02761 end += 4 02762 (length,) = _struct_I.unpack(str[start:end]) 02763 start = end 02764 end += length 02765 _v191.data = str[start:end] 02766 start = end 02767 end += 1 02768 (_v191.is_dense,) = _struct_B.unpack(str[start:end]) 02769 _v191.is_dense = bool(_v191.is_dense) 02770 start = end 02771 end += 4 02772 (length,) = _struct_I.unpack(str[start:end]) 02773 pattern = '<%si'%length 02774 start = end 02775 end += struct.calcsize(pattern) 02776 _v190.mask = struct.unpack(pattern, str[start:end]) 02777 _v194 = _v190.image 02778 _v195 = _v194.header 02779 start = end 02780 end += 4 02781 (_v195.seq,) = _struct_I.unpack(str[start:end]) 02782 _v196 = _v195.stamp 02783 _x = _v196 02784 start = end 02785 end += 8 02786 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02787 start = end 02788 end += 4 02789 (length,) = _struct_I.unpack(str[start:end]) 02790 start = end 02791 end += length 02792 _v195.frame_id = str[start:end] 02793 _x = _v194 02794 start = end 02795 end += 8 02796 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02797 start = end 02798 end += 4 02799 (length,) = _struct_I.unpack(str[start:end]) 02800 start = end 02801 end += length 02802 _v194.encoding = str[start:end] 02803 _x = _v194 02804 start = end 02805 end += 5 02806 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02807 start = end 02808 end += 4 02809 (length,) = _struct_I.unpack(str[start:end]) 02810 start = end 02811 end += length 02812 _v194.data = str[start:end] 02813 _v197 = _v190.disparity_image 02814 _v198 = _v197.header 02815 start = end 02816 end += 4 02817 (_v198.seq,) = _struct_I.unpack(str[start:end]) 02818 _v199 = _v198.stamp 02819 _x = _v199 02820 start = end 02821 end += 8 02822 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02823 start = end 02824 end += 4 02825 (length,) = _struct_I.unpack(str[start:end]) 02826 start = end 02827 end += length 02828 _v198.frame_id = str[start:end] 02829 _x = _v197 02830 start = end 02831 end += 8 02832 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02833 start = end 02834 end += 4 02835 (length,) = _struct_I.unpack(str[start:end]) 02836 start = end 02837 end += length 02838 _v197.encoding = str[start:end] 02839 _x = _v197 02840 start = end 02841 end += 5 02842 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02843 start = end 02844 end += 4 02845 (length,) = _struct_I.unpack(str[start:end]) 02846 start = end 02847 end += length 02848 _v197.data = str[start:end] 02849 _v200 = _v190.cam_info 02850 _v201 = _v200.header 02851 start = end 02852 end += 4 02853 (_v201.seq,) = _struct_I.unpack(str[start:end]) 02854 _v202 = _v201.stamp 02855 _x = _v202 02856 start = end 02857 end += 8 02858 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02859 start = end 02860 end += 4 02861 (length,) = _struct_I.unpack(str[start:end]) 02862 start = end 02863 end += length 02864 _v201.frame_id = str[start:end] 02865 _x = _v200 02866 start = end 02867 end += 8 02868 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02869 start = end 02870 end += 4 02871 (length,) = _struct_I.unpack(str[start:end]) 02872 start = end 02873 end += length 02874 _v200.distortion_model = str[start:end] 02875 start = end 02876 end += 4 02877 (length,) = _struct_I.unpack(str[start:end]) 02878 pattern = '<%sd'%length 02879 start = end 02880 end += struct.calcsize(pattern) 02881 _v200.D = struct.unpack(pattern, str[start:end]) 02882 start = end 02883 end += 72 02884 _v200.K = _struct_9d.unpack(str[start:end]) 02885 start = end 02886 end += 72 02887 _v200.R = _struct_9d.unpack(str[start:end]) 02888 start = end 02889 end += 96 02890 _v200.P = _struct_12d.unpack(str[start:end]) 02891 _x = _v200 02892 start = end 02893 end += 8 02894 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02895 _v203 = _v200.roi 02896 _x = _v203 02897 start = end 02898 end += 17 02899 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02900 _v203.do_rectify = bool(_v203.do_rectify) 02901 _v204 = _v190.roi_box_pose 02902 _v205 = _v204.header 02903 start = end 02904 end += 4 02905 (_v205.seq,) = _struct_I.unpack(str[start:end]) 02906 _v206 = _v205.stamp 02907 _x = _v206 02908 start = end 02909 end += 8 02910 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02911 start = end 02912 end += 4 02913 (length,) = _struct_I.unpack(str[start:end]) 02914 start = end 02915 end += length 02916 _v205.frame_id = str[start:end] 02917 _v207 = _v204.pose 02918 _v208 = _v207.position 02919 _x = _v208 02920 start = end 02921 end += 24 02922 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02923 _v209 = _v207.orientation 02924 _x = _v209 02925 start = end 02926 end += 32 02927 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02928 _v210 = _v190.roi_box_dims 02929 _x = _v210 02930 start = end 02931 end += 24 02932 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02933 start = end 02934 end += 4 02935 (length,) = _struct_I.unpack(str[start:end]) 02936 start = end 02937 end += length 02938 val2.collision_name = str[start:end] 02939 val1.moved_obstacles.append(val2) 02940 self.action_goal.goal.desired_grasps.append(val1) 02941 _x = self 02942 start = end 02943 end += 12 02944 (_x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02945 start = end 02946 end += 4 02947 (length,) = _struct_I.unpack(str[start:end]) 02948 start = end 02949 end += length 02950 self.action_goal.goal.lift.direction.header.frame_id = str[start:end] 02951 _x = self 02952 start = end 02953 end += 32 02954 (_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance,) = _struct_3d2f.unpack(str[start:end]) 02955 start = end 02956 end += 4 02957 (length,) = _struct_I.unpack(str[start:end]) 02958 start = end 02959 end += length 02960 self.action_goal.goal.collision_object_name = str[start:end] 02961 start = end 02962 end += 4 02963 (length,) = _struct_I.unpack(str[start:end]) 02964 start = end 02965 end += length 02966 self.action_goal.goal.collision_support_surface_name = str[start:end] 02967 _x = self 02968 start = end 02969 end += 5 02970 (_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_execution, _x.action_goal.goal.use_reactive_lift, _x.action_goal.goal.only_perform_feasibility_test, _x.action_goal.goal.ignore_collisions,) = _struct_5B.unpack(str[start:end]) 02971 self.action_goal.goal.allow_gripper_support_collision = bool(self.action_goal.goal.allow_gripper_support_collision) 02972 self.action_goal.goal.use_reactive_execution = bool(self.action_goal.goal.use_reactive_execution) 02973 self.action_goal.goal.use_reactive_lift = bool(self.action_goal.goal.use_reactive_lift) 02974 self.action_goal.goal.only_perform_feasibility_test = bool(self.action_goal.goal.only_perform_feasibility_test) 02975 self.action_goal.goal.ignore_collisions = bool(self.action_goal.goal.ignore_collisions) 02976 start = end 02977 end += 4 02978 (length,) = _struct_I.unpack(str[start:end]) 02979 self.action_goal.goal.path_constraints.joint_constraints = [] 02980 for i in range(0, length): 02981 val1 = arm_navigation_msgs.msg.JointConstraint() 02982 start = end 02983 end += 4 02984 (length,) = _struct_I.unpack(str[start:end]) 02985 start = end 02986 end += length 02987 val1.joint_name = str[start:end] 02988 _x = val1 02989 start = end 02990 end += 32 02991 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 02992 self.action_goal.goal.path_constraints.joint_constraints.append(val1) 02993 start = end 02994 end += 4 02995 (length,) = _struct_I.unpack(str[start:end]) 02996 self.action_goal.goal.path_constraints.position_constraints = [] 02997 for i in range(0, length): 02998 val1 = arm_navigation_msgs.msg.PositionConstraint() 02999 _v211 = val1.header 03000 start = end 03001 end += 4 03002 (_v211.seq,) = _struct_I.unpack(str[start:end]) 03003 _v212 = _v211.stamp 03004 _x = _v212 03005 start = end 03006 end += 8 03007 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03008 start = end 03009 end += 4 03010 (length,) = _struct_I.unpack(str[start:end]) 03011 start = end 03012 end += length 03013 _v211.frame_id = str[start:end] 03014 start = end 03015 end += 4 03016 (length,) = _struct_I.unpack(str[start:end]) 03017 start = end 03018 end += length 03019 val1.link_name = str[start:end] 03020 _v213 = val1.target_point_offset 03021 _x = _v213 03022 start = end 03023 end += 24 03024 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03025 _v214 = val1.position 03026 _x = _v214 03027 start = end 03028 end += 24 03029 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03030 _v215 = val1.constraint_region_shape 03031 start = end 03032 end += 1 03033 (_v215.type,) = _struct_b.unpack(str[start:end]) 03034 start = end 03035 end += 4 03036 (length,) = _struct_I.unpack(str[start:end]) 03037 pattern = '<%sd'%length 03038 start = end 03039 end += struct.calcsize(pattern) 03040 _v215.dimensions = struct.unpack(pattern, str[start:end]) 03041 start = end 03042 end += 4 03043 (length,) = _struct_I.unpack(str[start:end]) 03044 pattern = '<%si'%length 03045 start = end 03046 end += struct.calcsize(pattern) 03047 _v215.triangles = struct.unpack(pattern, str[start:end]) 03048 start = end 03049 end += 4 03050 (length,) = _struct_I.unpack(str[start:end]) 03051 _v215.vertices = [] 03052 for i in range(0, length): 03053 val3 = geometry_msgs.msg.Point() 03054 _x = val3 03055 start = end 03056 end += 24 03057 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03058 _v215.vertices.append(val3) 03059 _v216 = val1.constraint_region_orientation 03060 _x = _v216 03061 start = end 03062 end += 32 03063 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03064 start = end 03065 end += 8 03066 (val1.weight,) = _struct_d.unpack(str[start:end]) 03067 self.action_goal.goal.path_constraints.position_constraints.append(val1) 03068 start = end 03069 end += 4 03070 (length,) = _struct_I.unpack(str[start:end]) 03071 self.action_goal.goal.path_constraints.orientation_constraints = [] 03072 for i in range(0, length): 03073 val1 = arm_navigation_msgs.msg.OrientationConstraint() 03074 _v217 = val1.header 03075 start = end 03076 end += 4 03077 (_v217.seq,) = _struct_I.unpack(str[start:end]) 03078 _v218 = _v217.stamp 03079 _x = _v218 03080 start = end 03081 end += 8 03082 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03083 start = end 03084 end += 4 03085 (length,) = _struct_I.unpack(str[start:end]) 03086 start = end 03087 end += length 03088 _v217.frame_id = str[start:end] 03089 start = end 03090 end += 4 03091 (length,) = _struct_I.unpack(str[start:end]) 03092 start = end 03093 end += length 03094 val1.link_name = str[start:end] 03095 start = end 03096 end += 4 03097 (val1.type,) = _struct_i.unpack(str[start:end]) 03098 _v219 = val1.orientation 03099 _x = _v219 03100 start = end 03101 end += 32 03102 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03103 _x = val1 03104 start = end 03105 end += 32 03106 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 03107 self.action_goal.goal.path_constraints.orientation_constraints.append(val1) 03108 start = end 03109 end += 4 03110 (length,) = _struct_I.unpack(str[start:end]) 03111 self.action_goal.goal.path_constraints.visibility_constraints = [] 03112 for i in range(0, length): 03113 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 03114 _v220 = val1.header 03115 start = end 03116 end += 4 03117 (_v220.seq,) = _struct_I.unpack(str[start:end]) 03118 _v221 = _v220.stamp 03119 _x = _v221 03120 start = end 03121 end += 8 03122 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03123 start = end 03124 end += 4 03125 (length,) = _struct_I.unpack(str[start:end]) 03126 start = end 03127 end += length 03128 _v220.frame_id = str[start:end] 03129 _v222 = val1.target 03130 _v223 = _v222.header 03131 start = end 03132 end += 4 03133 (_v223.seq,) = _struct_I.unpack(str[start:end]) 03134 _v224 = _v223.stamp 03135 _x = _v224 03136 start = end 03137 end += 8 03138 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03139 start = end 03140 end += 4 03141 (length,) = _struct_I.unpack(str[start:end]) 03142 start = end 03143 end += length 03144 _v223.frame_id = str[start:end] 03145 _v225 = _v222.point 03146 _x = _v225 03147 start = end 03148 end += 24 03149 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03150 _v226 = val1.sensor_pose 03151 _v227 = _v226.header 03152 start = end 03153 end += 4 03154 (_v227.seq,) = _struct_I.unpack(str[start:end]) 03155 _v228 = _v227.stamp 03156 _x = _v228 03157 start = end 03158 end += 8 03159 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03160 start = end 03161 end += 4 03162 (length,) = _struct_I.unpack(str[start:end]) 03163 start = end 03164 end += length 03165 _v227.frame_id = str[start:end] 03166 _v229 = _v226.pose 03167 _v230 = _v229.position 03168 _x = _v230 03169 start = end 03170 end += 24 03171 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03172 _v231 = _v229.orientation 03173 _x = _v231 03174 start = end 03175 end += 32 03176 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03177 start = end 03178 end += 8 03179 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 03180 self.action_goal.goal.path_constraints.visibility_constraints.append(val1) 03181 start = end 03182 end += 4 03183 (length,) = _struct_I.unpack(str[start:end]) 03184 self.action_goal.goal.additional_collision_operations.collision_operations = [] 03185 for i in range(0, length): 03186 val1 = arm_navigation_msgs.msg.CollisionOperation() 03187 start = end 03188 end += 4 03189 (length,) = _struct_I.unpack(str[start:end]) 03190 start = end 03191 end += length 03192 val1.object1 = str[start:end] 03193 start = end 03194 end += 4 03195 (length,) = _struct_I.unpack(str[start:end]) 03196 start = end 03197 end += length 03198 val1.object2 = str[start:end] 03199 _x = val1 03200 start = end 03201 end += 12 03202 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 03203 self.action_goal.goal.additional_collision_operations.collision_operations.append(val1) 03204 start = end 03205 end += 4 03206 (length,) = _struct_I.unpack(str[start:end]) 03207 self.action_goal.goal.additional_link_padding = [] 03208 for i in range(0, length): 03209 val1 = arm_navigation_msgs.msg.LinkPadding() 03210 start = end 03211 end += 4 03212 (length,) = _struct_I.unpack(str[start:end]) 03213 start = end 03214 end += length 03215 val1.link_name = str[start:end] 03216 start = end 03217 end += 8 03218 (val1.padding,) = _struct_d.unpack(str[start:end]) 03219 self.action_goal.goal.additional_link_padding.append(val1) 03220 start = end 03221 end += 4 03222 (length,) = _struct_I.unpack(str[start:end]) 03223 self.action_goal.goal.movable_obstacles = [] 03224 for i in range(0, length): 03225 val1 = object_manipulation_msgs.msg.GraspableObject() 03226 start = end 03227 end += 4 03228 (length,) = _struct_I.unpack(str[start:end]) 03229 start = end 03230 end += length 03231 val1.reference_frame_id = str[start:end] 03232 start = end 03233 end += 4 03234 (length,) = _struct_I.unpack(str[start:end]) 03235 val1.potential_models = [] 03236 for i in range(0, length): 03237 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 03238 start = end 03239 end += 4 03240 (val2.model_id,) = _struct_i.unpack(str[start:end]) 03241 _v232 = val2.pose 03242 _v233 = _v232.header 03243 start = end 03244 end += 4 03245 (_v233.seq,) = _struct_I.unpack(str[start:end]) 03246 _v234 = _v233.stamp 03247 _x = _v234 03248 start = end 03249 end += 8 03250 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03251 start = end 03252 end += 4 03253 (length,) = _struct_I.unpack(str[start:end]) 03254 start = end 03255 end += length 03256 _v233.frame_id = str[start:end] 03257 _v235 = _v232.pose 03258 _v236 = _v235.position 03259 _x = _v236 03260 start = end 03261 end += 24 03262 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03263 _v237 = _v235.orientation 03264 _x = _v237 03265 start = end 03266 end += 32 03267 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03268 start = end 03269 end += 4 03270 (val2.confidence,) = _struct_f.unpack(str[start:end]) 03271 start = end 03272 end += 4 03273 (length,) = _struct_I.unpack(str[start:end]) 03274 start = end 03275 end += length 03276 val2.detector_name = str[start:end] 03277 val1.potential_models.append(val2) 03278 _v238 = val1.cluster 03279 _v239 = _v238.header 03280 start = end 03281 end += 4 03282 (_v239.seq,) = _struct_I.unpack(str[start:end]) 03283 _v240 = _v239.stamp 03284 _x = _v240 03285 start = end 03286 end += 8 03287 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03288 start = end 03289 end += 4 03290 (length,) = _struct_I.unpack(str[start:end]) 03291 start = end 03292 end += length 03293 _v239.frame_id = str[start:end] 03294 start = end 03295 end += 4 03296 (length,) = _struct_I.unpack(str[start:end]) 03297 _v238.points = [] 03298 for i in range(0, length): 03299 val3 = geometry_msgs.msg.Point32() 03300 _x = val3 03301 start = end 03302 end += 12 03303 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03304 _v238.points.append(val3) 03305 start = end 03306 end += 4 03307 (length,) = _struct_I.unpack(str[start:end]) 03308 _v238.channels = [] 03309 for i in range(0, length): 03310 val3 = sensor_msgs.msg.ChannelFloat32() 03311 start = end 03312 end += 4 03313 (length,) = _struct_I.unpack(str[start:end]) 03314 start = end 03315 end += length 03316 val3.name = str[start:end] 03317 start = end 03318 end += 4 03319 (length,) = _struct_I.unpack(str[start:end]) 03320 pattern = '<%sf'%length 03321 start = end 03322 end += struct.calcsize(pattern) 03323 val3.values = struct.unpack(pattern, str[start:end]) 03324 _v238.channels.append(val3) 03325 _v241 = val1.region 03326 _v242 = _v241.cloud 03327 _v243 = _v242.header 03328 start = end 03329 end += 4 03330 (_v243.seq,) = _struct_I.unpack(str[start:end]) 03331 _v244 = _v243.stamp 03332 _x = _v244 03333 start = end 03334 end += 8 03335 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03336 start = end 03337 end += 4 03338 (length,) = _struct_I.unpack(str[start:end]) 03339 start = end 03340 end += length 03341 _v243.frame_id = str[start:end] 03342 _x = _v242 03343 start = end 03344 end += 8 03345 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03346 start = end 03347 end += 4 03348 (length,) = _struct_I.unpack(str[start:end]) 03349 _v242.fields = [] 03350 for i in range(0, length): 03351 val4 = sensor_msgs.msg.PointField() 03352 start = end 03353 end += 4 03354 (length,) = _struct_I.unpack(str[start:end]) 03355 start = end 03356 end += length 03357 val4.name = str[start:end] 03358 _x = val4 03359 start = end 03360 end += 9 03361 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03362 _v242.fields.append(val4) 03363 _x = _v242 03364 start = end 03365 end += 9 03366 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 03367 _v242.is_bigendian = bool(_v242.is_bigendian) 03368 start = end 03369 end += 4 03370 (length,) = _struct_I.unpack(str[start:end]) 03371 start = end 03372 end += length 03373 _v242.data = str[start:end] 03374 start = end 03375 end += 1 03376 (_v242.is_dense,) = _struct_B.unpack(str[start:end]) 03377 _v242.is_dense = bool(_v242.is_dense) 03378 start = end 03379 end += 4 03380 (length,) = _struct_I.unpack(str[start:end]) 03381 pattern = '<%si'%length 03382 start = end 03383 end += struct.calcsize(pattern) 03384 _v241.mask = struct.unpack(pattern, str[start:end]) 03385 _v245 = _v241.image 03386 _v246 = _v245.header 03387 start = end 03388 end += 4 03389 (_v246.seq,) = _struct_I.unpack(str[start:end]) 03390 _v247 = _v246.stamp 03391 _x = _v247 03392 start = end 03393 end += 8 03394 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03395 start = end 03396 end += 4 03397 (length,) = _struct_I.unpack(str[start:end]) 03398 start = end 03399 end += length 03400 _v246.frame_id = str[start:end] 03401 _x = _v245 03402 start = end 03403 end += 8 03404 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03405 start = end 03406 end += 4 03407 (length,) = _struct_I.unpack(str[start:end]) 03408 start = end 03409 end += length 03410 _v245.encoding = str[start:end] 03411 _x = _v245 03412 start = end 03413 end += 5 03414 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03415 start = end 03416 end += 4 03417 (length,) = _struct_I.unpack(str[start:end]) 03418 start = end 03419 end += length 03420 _v245.data = str[start:end] 03421 _v248 = _v241.disparity_image 03422 _v249 = _v248.header 03423 start = end 03424 end += 4 03425 (_v249.seq,) = _struct_I.unpack(str[start:end]) 03426 _v250 = _v249.stamp 03427 _x = _v250 03428 start = end 03429 end += 8 03430 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03431 start = end 03432 end += 4 03433 (length,) = _struct_I.unpack(str[start:end]) 03434 start = end 03435 end += length 03436 _v249.frame_id = str[start:end] 03437 _x = _v248 03438 start = end 03439 end += 8 03440 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03441 start = end 03442 end += 4 03443 (length,) = _struct_I.unpack(str[start:end]) 03444 start = end 03445 end += length 03446 _v248.encoding = str[start:end] 03447 _x = _v248 03448 start = end 03449 end += 5 03450 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03451 start = end 03452 end += 4 03453 (length,) = _struct_I.unpack(str[start:end]) 03454 start = end 03455 end += length 03456 _v248.data = str[start:end] 03457 _v251 = _v241.cam_info 03458 _v252 = _v251.header 03459 start = end 03460 end += 4 03461 (_v252.seq,) = _struct_I.unpack(str[start:end]) 03462 _v253 = _v252.stamp 03463 _x = _v253 03464 start = end 03465 end += 8 03466 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03467 start = end 03468 end += 4 03469 (length,) = _struct_I.unpack(str[start:end]) 03470 start = end 03471 end += length 03472 _v252.frame_id = str[start:end] 03473 _x = _v251 03474 start = end 03475 end += 8 03476 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03477 start = end 03478 end += 4 03479 (length,) = _struct_I.unpack(str[start:end]) 03480 start = end 03481 end += length 03482 _v251.distortion_model = str[start:end] 03483 start = end 03484 end += 4 03485 (length,) = _struct_I.unpack(str[start:end]) 03486 pattern = '<%sd'%length 03487 start = end 03488 end += struct.calcsize(pattern) 03489 _v251.D = struct.unpack(pattern, str[start:end]) 03490 start = end 03491 end += 72 03492 _v251.K = _struct_9d.unpack(str[start:end]) 03493 start = end 03494 end += 72 03495 _v251.R = _struct_9d.unpack(str[start:end]) 03496 start = end 03497 end += 96 03498 _v251.P = _struct_12d.unpack(str[start:end]) 03499 _x = _v251 03500 start = end 03501 end += 8 03502 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 03503 _v254 = _v251.roi 03504 _x = _v254 03505 start = end 03506 end += 17 03507 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 03508 _v254.do_rectify = bool(_v254.do_rectify) 03509 _v255 = _v241.roi_box_pose 03510 _v256 = _v255.header 03511 start = end 03512 end += 4 03513 (_v256.seq,) = _struct_I.unpack(str[start:end]) 03514 _v257 = _v256.stamp 03515 _x = _v257 03516 start = end 03517 end += 8 03518 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03519 start = end 03520 end += 4 03521 (length,) = _struct_I.unpack(str[start:end]) 03522 start = end 03523 end += length 03524 _v256.frame_id = str[start:end] 03525 _v258 = _v255.pose 03526 _v259 = _v258.position 03527 _x = _v259 03528 start = end 03529 end += 24 03530 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03531 _v260 = _v258.orientation 03532 _x = _v260 03533 start = end 03534 end += 32 03535 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03536 _v261 = _v241.roi_box_dims 03537 _x = _v261 03538 start = end 03539 end += 24 03540 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03541 start = end 03542 end += 4 03543 (length,) = _struct_I.unpack(str[start:end]) 03544 start = end 03545 end += length 03546 val1.collision_name = str[start:end] 03547 self.action_goal.goal.movable_obstacles.append(val1) 03548 _x = self 03549 start = end 03550 end += 16 03551 (_x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_f3I.unpack(str[start:end]) 03552 start = end 03553 end += 4 03554 (length,) = _struct_I.unpack(str[start:end]) 03555 start = end 03556 end += length 03557 self.action_result.header.frame_id = str[start:end] 03558 _x = self 03559 start = end 03560 end += 8 03561 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 03562 start = end 03563 end += 4 03564 (length,) = _struct_I.unpack(str[start:end]) 03565 start = end 03566 end += length 03567 self.action_result.status.goal_id.id = str[start:end] 03568 start = end 03569 end += 1 03570 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 03571 start = end 03572 end += 4 03573 (length,) = _struct_I.unpack(str[start:end]) 03574 start = end 03575 end += length 03576 self.action_result.status.text = str[start:end] 03577 _x = self 03578 start = end 03579 end += 16 03580 (_x.action_result.result.manipulation_result.value, _x.action_result.result.grasp.pre_grasp_posture.header.seq, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.secs, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end]) 03581 start = end 03582 end += 4 03583 (length,) = _struct_I.unpack(str[start:end]) 03584 start = end 03585 end += length 03586 self.action_result.result.grasp.pre_grasp_posture.header.frame_id = str[start:end] 03587 start = end 03588 end += 4 03589 (length,) = _struct_I.unpack(str[start:end]) 03590 self.action_result.result.grasp.pre_grasp_posture.name = [] 03591 for i in range(0, length): 03592 start = end 03593 end += 4 03594 (length,) = _struct_I.unpack(str[start:end]) 03595 start = end 03596 end += length 03597 val1 = str[start:end] 03598 self.action_result.result.grasp.pre_grasp_posture.name.append(val1) 03599 start = end 03600 end += 4 03601 (length,) = _struct_I.unpack(str[start:end]) 03602 pattern = '<%sd'%length 03603 start = end 03604 end += struct.calcsize(pattern) 03605 self.action_result.result.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end]) 03606 start = end 03607 end += 4 03608 (length,) = _struct_I.unpack(str[start:end]) 03609 pattern = '<%sd'%length 03610 start = end 03611 end += struct.calcsize(pattern) 03612 self.action_result.result.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 03613 start = end 03614 end += 4 03615 (length,) = _struct_I.unpack(str[start:end]) 03616 pattern = '<%sd'%length 03617 start = end 03618 end += struct.calcsize(pattern) 03619 self.action_result.result.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end]) 03620 _x = self 03621 start = end 03622 end += 12 03623 (_x.action_result.result.grasp.grasp_posture.header.seq, _x.action_result.result.grasp.grasp_posture.header.stamp.secs, _x.action_result.result.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03624 start = end 03625 end += 4 03626 (length,) = _struct_I.unpack(str[start:end]) 03627 start = end 03628 end += length 03629 self.action_result.result.grasp.grasp_posture.header.frame_id = str[start:end] 03630 start = end 03631 end += 4 03632 (length,) = _struct_I.unpack(str[start:end]) 03633 self.action_result.result.grasp.grasp_posture.name = [] 03634 for i in range(0, length): 03635 start = end 03636 end += 4 03637 (length,) = _struct_I.unpack(str[start:end]) 03638 start = end 03639 end += length 03640 val1 = str[start:end] 03641 self.action_result.result.grasp.grasp_posture.name.append(val1) 03642 start = end 03643 end += 4 03644 (length,) = _struct_I.unpack(str[start:end]) 03645 pattern = '<%sd'%length 03646 start = end 03647 end += struct.calcsize(pattern) 03648 self.action_result.result.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end]) 03649 start = end 03650 end += 4 03651 (length,) = _struct_I.unpack(str[start:end]) 03652 pattern = '<%sd'%length 03653 start = end 03654 end += struct.calcsize(pattern) 03655 self.action_result.result.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 03656 start = end 03657 end += 4 03658 (length,) = _struct_I.unpack(str[start:end]) 03659 pattern = '<%sd'%length 03660 start = end 03661 end += struct.calcsize(pattern) 03662 self.action_result.result.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end]) 03663 _x = self 03664 start = end 03665 end += 73 03666 (_x.action_result.result.grasp.grasp_pose.position.x, _x.action_result.result.grasp.grasp_pose.position.y, _x.action_result.result.grasp.grasp_pose.position.z, _x.action_result.result.grasp.grasp_pose.orientation.x, _x.action_result.result.grasp.grasp_pose.orientation.y, _x.action_result.result.grasp.grasp_pose.orientation.z, _x.action_result.result.grasp.grasp_pose.orientation.w, _x.action_result.result.grasp.success_probability, _x.action_result.result.grasp.cluster_rep, _x.action_result.result.grasp.desired_approach_distance, _x.action_result.result.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end]) 03667 self.action_result.result.grasp.cluster_rep = bool(self.action_result.result.grasp.cluster_rep) 03668 start = end 03669 end += 4 03670 (length,) = _struct_I.unpack(str[start:end]) 03671 self.action_result.result.grasp.moved_obstacles = [] 03672 for i in range(0, length): 03673 val1 = object_manipulation_msgs.msg.GraspableObject() 03674 start = end 03675 end += 4 03676 (length,) = _struct_I.unpack(str[start:end]) 03677 start = end 03678 end += length 03679 val1.reference_frame_id = str[start:end] 03680 start = end 03681 end += 4 03682 (length,) = _struct_I.unpack(str[start:end]) 03683 val1.potential_models = [] 03684 for i in range(0, length): 03685 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 03686 start = end 03687 end += 4 03688 (val2.model_id,) = _struct_i.unpack(str[start:end]) 03689 _v262 = val2.pose 03690 _v263 = _v262.header 03691 start = end 03692 end += 4 03693 (_v263.seq,) = _struct_I.unpack(str[start:end]) 03694 _v264 = _v263.stamp 03695 _x = _v264 03696 start = end 03697 end += 8 03698 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03699 start = end 03700 end += 4 03701 (length,) = _struct_I.unpack(str[start:end]) 03702 start = end 03703 end += length 03704 _v263.frame_id = str[start:end] 03705 _v265 = _v262.pose 03706 _v266 = _v265.position 03707 _x = _v266 03708 start = end 03709 end += 24 03710 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03711 _v267 = _v265.orientation 03712 _x = _v267 03713 start = end 03714 end += 32 03715 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03716 start = end 03717 end += 4 03718 (val2.confidence,) = _struct_f.unpack(str[start:end]) 03719 start = end 03720 end += 4 03721 (length,) = _struct_I.unpack(str[start:end]) 03722 start = end 03723 end += length 03724 val2.detector_name = str[start:end] 03725 val1.potential_models.append(val2) 03726 _v268 = val1.cluster 03727 _v269 = _v268.header 03728 start = end 03729 end += 4 03730 (_v269.seq,) = _struct_I.unpack(str[start:end]) 03731 _v270 = _v269.stamp 03732 _x = _v270 03733 start = end 03734 end += 8 03735 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03736 start = end 03737 end += 4 03738 (length,) = _struct_I.unpack(str[start:end]) 03739 start = end 03740 end += length 03741 _v269.frame_id = str[start:end] 03742 start = end 03743 end += 4 03744 (length,) = _struct_I.unpack(str[start:end]) 03745 _v268.points = [] 03746 for i in range(0, length): 03747 val3 = geometry_msgs.msg.Point32() 03748 _x = val3 03749 start = end 03750 end += 12 03751 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03752 _v268.points.append(val3) 03753 start = end 03754 end += 4 03755 (length,) = _struct_I.unpack(str[start:end]) 03756 _v268.channels = [] 03757 for i in range(0, length): 03758 val3 = sensor_msgs.msg.ChannelFloat32() 03759 start = end 03760 end += 4 03761 (length,) = _struct_I.unpack(str[start:end]) 03762 start = end 03763 end += length 03764 val3.name = str[start:end] 03765 start = end 03766 end += 4 03767 (length,) = _struct_I.unpack(str[start:end]) 03768 pattern = '<%sf'%length 03769 start = end 03770 end += struct.calcsize(pattern) 03771 val3.values = struct.unpack(pattern, str[start:end]) 03772 _v268.channels.append(val3) 03773 _v271 = val1.region 03774 _v272 = _v271.cloud 03775 _v273 = _v272.header 03776 start = end 03777 end += 4 03778 (_v273.seq,) = _struct_I.unpack(str[start:end]) 03779 _v274 = _v273.stamp 03780 _x = _v274 03781 start = end 03782 end += 8 03783 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03784 start = end 03785 end += 4 03786 (length,) = _struct_I.unpack(str[start:end]) 03787 start = end 03788 end += length 03789 _v273.frame_id = str[start:end] 03790 _x = _v272 03791 start = end 03792 end += 8 03793 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03794 start = end 03795 end += 4 03796 (length,) = _struct_I.unpack(str[start:end]) 03797 _v272.fields = [] 03798 for i in range(0, length): 03799 val4 = sensor_msgs.msg.PointField() 03800 start = end 03801 end += 4 03802 (length,) = _struct_I.unpack(str[start:end]) 03803 start = end 03804 end += length 03805 val4.name = str[start:end] 03806 _x = val4 03807 start = end 03808 end += 9 03809 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03810 _v272.fields.append(val4) 03811 _x = _v272 03812 start = end 03813 end += 9 03814 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 03815 _v272.is_bigendian = bool(_v272.is_bigendian) 03816 start = end 03817 end += 4 03818 (length,) = _struct_I.unpack(str[start:end]) 03819 start = end 03820 end += length 03821 _v272.data = str[start:end] 03822 start = end 03823 end += 1 03824 (_v272.is_dense,) = _struct_B.unpack(str[start:end]) 03825 _v272.is_dense = bool(_v272.is_dense) 03826 start = end 03827 end += 4 03828 (length,) = _struct_I.unpack(str[start:end]) 03829 pattern = '<%si'%length 03830 start = end 03831 end += struct.calcsize(pattern) 03832 _v271.mask = struct.unpack(pattern, str[start:end]) 03833 _v275 = _v271.image 03834 _v276 = _v275.header 03835 start = end 03836 end += 4 03837 (_v276.seq,) = _struct_I.unpack(str[start:end]) 03838 _v277 = _v276.stamp 03839 _x = _v277 03840 start = end 03841 end += 8 03842 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03843 start = end 03844 end += 4 03845 (length,) = _struct_I.unpack(str[start:end]) 03846 start = end 03847 end += length 03848 _v276.frame_id = str[start:end] 03849 _x = _v275 03850 start = end 03851 end += 8 03852 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03853 start = end 03854 end += 4 03855 (length,) = _struct_I.unpack(str[start:end]) 03856 start = end 03857 end += length 03858 _v275.encoding = str[start:end] 03859 _x = _v275 03860 start = end 03861 end += 5 03862 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03863 start = end 03864 end += 4 03865 (length,) = _struct_I.unpack(str[start:end]) 03866 start = end 03867 end += length 03868 _v275.data = str[start:end] 03869 _v278 = _v271.disparity_image 03870 _v279 = _v278.header 03871 start = end 03872 end += 4 03873 (_v279.seq,) = _struct_I.unpack(str[start:end]) 03874 _v280 = _v279.stamp 03875 _x = _v280 03876 start = end 03877 end += 8 03878 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03879 start = end 03880 end += 4 03881 (length,) = _struct_I.unpack(str[start:end]) 03882 start = end 03883 end += length 03884 _v279.frame_id = str[start:end] 03885 _x = _v278 03886 start = end 03887 end += 8 03888 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03889 start = end 03890 end += 4 03891 (length,) = _struct_I.unpack(str[start:end]) 03892 start = end 03893 end += length 03894 _v278.encoding = str[start:end] 03895 _x = _v278 03896 start = end 03897 end += 5 03898 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03899 start = end 03900 end += 4 03901 (length,) = _struct_I.unpack(str[start:end]) 03902 start = end 03903 end += length 03904 _v278.data = str[start:end] 03905 _v281 = _v271.cam_info 03906 _v282 = _v281.header 03907 start = end 03908 end += 4 03909 (_v282.seq,) = _struct_I.unpack(str[start:end]) 03910 _v283 = _v282.stamp 03911 _x = _v283 03912 start = end 03913 end += 8 03914 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03915 start = end 03916 end += 4 03917 (length,) = _struct_I.unpack(str[start:end]) 03918 start = end 03919 end += length 03920 _v282.frame_id = str[start:end] 03921 _x = _v281 03922 start = end 03923 end += 8 03924 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03925 start = end 03926 end += 4 03927 (length,) = _struct_I.unpack(str[start:end]) 03928 start = end 03929 end += length 03930 _v281.distortion_model = str[start:end] 03931 start = end 03932 end += 4 03933 (length,) = _struct_I.unpack(str[start:end]) 03934 pattern = '<%sd'%length 03935 start = end 03936 end += struct.calcsize(pattern) 03937 _v281.D = struct.unpack(pattern, str[start:end]) 03938 start = end 03939 end += 72 03940 _v281.K = _struct_9d.unpack(str[start:end]) 03941 start = end 03942 end += 72 03943 _v281.R = _struct_9d.unpack(str[start:end]) 03944 start = end 03945 end += 96 03946 _v281.P = _struct_12d.unpack(str[start:end]) 03947 _x = _v281 03948 start = end 03949 end += 8 03950 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 03951 _v284 = _v281.roi 03952 _x = _v284 03953 start = end 03954 end += 17 03955 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 03956 _v284.do_rectify = bool(_v284.do_rectify) 03957 _v285 = _v271.roi_box_pose 03958 _v286 = _v285.header 03959 start = end 03960 end += 4 03961 (_v286.seq,) = _struct_I.unpack(str[start:end]) 03962 _v287 = _v286.stamp 03963 _x = _v287 03964 start = end 03965 end += 8 03966 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03967 start = end 03968 end += 4 03969 (length,) = _struct_I.unpack(str[start:end]) 03970 start = end 03971 end += length 03972 _v286.frame_id = str[start:end] 03973 _v288 = _v285.pose 03974 _v289 = _v288.position 03975 _x = _v289 03976 start = end 03977 end += 24 03978 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03979 _v290 = _v288.orientation 03980 _x = _v290 03981 start = end 03982 end += 32 03983 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03984 _v291 = _v271.roi_box_dims 03985 _x = _v291 03986 start = end 03987 end += 24 03988 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03989 start = end 03990 end += 4 03991 (length,) = _struct_I.unpack(str[start:end]) 03992 start = end 03993 end += length 03994 val1.collision_name = str[start:end] 03995 self.action_result.result.grasp.moved_obstacles.append(val1) 03996 start = end 03997 end += 4 03998 (length,) = _struct_I.unpack(str[start:end]) 03999 self.action_result.result.attempted_grasps = [] 04000 for i in range(0, length): 04001 val1 = object_manipulation_msgs.msg.Grasp() 04002 _v292 = val1.pre_grasp_posture 04003 _v293 = _v292.header 04004 start = end 04005 end += 4 04006 (_v293.seq,) = _struct_I.unpack(str[start:end]) 04007 _v294 = _v293.stamp 04008 _x = _v294 04009 start = end 04010 end += 8 04011 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04012 start = end 04013 end += 4 04014 (length,) = _struct_I.unpack(str[start:end]) 04015 start = end 04016 end += length 04017 _v293.frame_id = str[start:end] 04018 start = end 04019 end += 4 04020 (length,) = _struct_I.unpack(str[start:end]) 04021 _v292.name = [] 04022 for i in range(0, length): 04023 start = end 04024 end += 4 04025 (length,) = _struct_I.unpack(str[start:end]) 04026 start = end 04027 end += length 04028 val3 = str[start:end] 04029 _v292.name.append(val3) 04030 start = end 04031 end += 4 04032 (length,) = _struct_I.unpack(str[start:end]) 04033 pattern = '<%sd'%length 04034 start = end 04035 end += struct.calcsize(pattern) 04036 _v292.position = struct.unpack(pattern, str[start:end]) 04037 start = end 04038 end += 4 04039 (length,) = _struct_I.unpack(str[start:end]) 04040 pattern = '<%sd'%length 04041 start = end 04042 end += struct.calcsize(pattern) 04043 _v292.velocity = struct.unpack(pattern, str[start:end]) 04044 start = end 04045 end += 4 04046 (length,) = _struct_I.unpack(str[start:end]) 04047 pattern = '<%sd'%length 04048 start = end 04049 end += struct.calcsize(pattern) 04050 _v292.effort = struct.unpack(pattern, str[start:end]) 04051 _v295 = val1.grasp_posture 04052 _v296 = _v295.header 04053 start = end 04054 end += 4 04055 (_v296.seq,) = _struct_I.unpack(str[start:end]) 04056 _v297 = _v296.stamp 04057 _x = _v297 04058 start = end 04059 end += 8 04060 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04061 start = end 04062 end += 4 04063 (length,) = _struct_I.unpack(str[start:end]) 04064 start = end 04065 end += length 04066 _v296.frame_id = str[start:end] 04067 start = end 04068 end += 4 04069 (length,) = _struct_I.unpack(str[start:end]) 04070 _v295.name = [] 04071 for i in range(0, length): 04072 start = end 04073 end += 4 04074 (length,) = _struct_I.unpack(str[start:end]) 04075 start = end 04076 end += length 04077 val3 = str[start:end] 04078 _v295.name.append(val3) 04079 start = end 04080 end += 4 04081 (length,) = _struct_I.unpack(str[start:end]) 04082 pattern = '<%sd'%length 04083 start = end 04084 end += struct.calcsize(pattern) 04085 _v295.position = struct.unpack(pattern, str[start:end]) 04086 start = end 04087 end += 4 04088 (length,) = _struct_I.unpack(str[start:end]) 04089 pattern = '<%sd'%length 04090 start = end 04091 end += struct.calcsize(pattern) 04092 _v295.velocity = struct.unpack(pattern, str[start:end]) 04093 start = end 04094 end += 4 04095 (length,) = _struct_I.unpack(str[start:end]) 04096 pattern = '<%sd'%length 04097 start = end 04098 end += struct.calcsize(pattern) 04099 _v295.effort = struct.unpack(pattern, str[start:end]) 04100 _v298 = val1.grasp_pose 04101 _v299 = _v298.position 04102 _x = _v299 04103 start = end 04104 end += 24 04105 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04106 _v300 = _v298.orientation 04107 _x = _v300 04108 start = end 04109 end += 32 04110 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04111 _x = val1 04112 start = end 04113 end += 17 04114 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 04115 val1.cluster_rep = bool(val1.cluster_rep) 04116 start = end 04117 end += 4 04118 (length,) = _struct_I.unpack(str[start:end]) 04119 val1.moved_obstacles = [] 04120 for i in range(0, length): 04121 val2 = object_manipulation_msgs.msg.GraspableObject() 04122 start = end 04123 end += 4 04124 (length,) = _struct_I.unpack(str[start:end]) 04125 start = end 04126 end += length 04127 val2.reference_frame_id = str[start:end] 04128 start = end 04129 end += 4 04130 (length,) = _struct_I.unpack(str[start:end]) 04131 val2.potential_models = [] 04132 for i in range(0, length): 04133 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 04134 start = end 04135 end += 4 04136 (val3.model_id,) = _struct_i.unpack(str[start:end]) 04137 _v301 = val3.pose 04138 _v302 = _v301.header 04139 start = end 04140 end += 4 04141 (_v302.seq,) = _struct_I.unpack(str[start:end]) 04142 _v303 = _v302.stamp 04143 _x = _v303 04144 start = end 04145 end += 8 04146 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04147 start = end 04148 end += 4 04149 (length,) = _struct_I.unpack(str[start:end]) 04150 start = end 04151 end += length 04152 _v302.frame_id = str[start:end] 04153 _v304 = _v301.pose 04154 _v305 = _v304.position 04155 _x = _v305 04156 start = end 04157 end += 24 04158 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04159 _v306 = _v304.orientation 04160 _x = _v306 04161 start = end 04162 end += 32 04163 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04164 start = end 04165 end += 4 04166 (val3.confidence,) = _struct_f.unpack(str[start:end]) 04167 start = end 04168 end += 4 04169 (length,) = _struct_I.unpack(str[start:end]) 04170 start = end 04171 end += length 04172 val3.detector_name = str[start:end] 04173 val2.potential_models.append(val3) 04174 _v307 = val2.cluster 04175 _v308 = _v307.header 04176 start = end 04177 end += 4 04178 (_v308.seq,) = _struct_I.unpack(str[start:end]) 04179 _v309 = _v308.stamp 04180 _x = _v309 04181 start = end 04182 end += 8 04183 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04184 start = end 04185 end += 4 04186 (length,) = _struct_I.unpack(str[start:end]) 04187 start = end 04188 end += length 04189 _v308.frame_id = str[start:end] 04190 start = end 04191 end += 4 04192 (length,) = _struct_I.unpack(str[start:end]) 04193 _v307.points = [] 04194 for i in range(0, length): 04195 val4 = geometry_msgs.msg.Point32() 04196 _x = val4 04197 start = end 04198 end += 12 04199 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 04200 _v307.points.append(val4) 04201 start = end 04202 end += 4 04203 (length,) = _struct_I.unpack(str[start:end]) 04204 _v307.channels = [] 04205 for i in range(0, length): 04206 val4 = sensor_msgs.msg.ChannelFloat32() 04207 start = end 04208 end += 4 04209 (length,) = _struct_I.unpack(str[start:end]) 04210 start = end 04211 end += length 04212 val4.name = str[start:end] 04213 start = end 04214 end += 4 04215 (length,) = _struct_I.unpack(str[start:end]) 04216 pattern = '<%sf'%length 04217 start = end 04218 end += struct.calcsize(pattern) 04219 val4.values = struct.unpack(pattern, str[start:end]) 04220 _v307.channels.append(val4) 04221 _v310 = val2.region 04222 _v311 = _v310.cloud 04223 _v312 = _v311.header 04224 start = end 04225 end += 4 04226 (_v312.seq,) = _struct_I.unpack(str[start:end]) 04227 _v313 = _v312.stamp 04228 _x = _v313 04229 start = end 04230 end += 8 04231 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04232 start = end 04233 end += 4 04234 (length,) = _struct_I.unpack(str[start:end]) 04235 start = end 04236 end += length 04237 _v312.frame_id = str[start:end] 04238 _x = _v311 04239 start = end 04240 end += 8 04241 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04242 start = end 04243 end += 4 04244 (length,) = _struct_I.unpack(str[start:end]) 04245 _v311.fields = [] 04246 for i in range(0, length): 04247 val5 = sensor_msgs.msg.PointField() 04248 start = end 04249 end += 4 04250 (length,) = _struct_I.unpack(str[start:end]) 04251 start = end 04252 end += length 04253 val5.name = str[start:end] 04254 _x = val5 04255 start = end 04256 end += 9 04257 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 04258 _v311.fields.append(val5) 04259 _x = _v311 04260 start = end 04261 end += 9 04262 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 04263 _v311.is_bigendian = bool(_v311.is_bigendian) 04264 start = end 04265 end += 4 04266 (length,) = _struct_I.unpack(str[start:end]) 04267 start = end 04268 end += length 04269 _v311.data = str[start:end] 04270 start = end 04271 end += 1 04272 (_v311.is_dense,) = _struct_B.unpack(str[start:end]) 04273 _v311.is_dense = bool(_v311.is_dense) 04274 start = end 04275 end += 4 04276 (length,) = _struct_I.unpack(str[start:end]) 04277 pattern = '<%si'%length 04278 start = end 04279 end += struct.calcsize(pattern) 04280 _v310.mask = struct.unpack(pattern, str[start:end]) 04281 _v314 = _v310.image 04282 _v315 = _v314.header 04283 start = end 04284 end += 4 04285 (_v315.seq,) = _struct_I.unpack(str[start:end]) 04286 _v316 = _v315.stamp 04287 _x = _v316 04288 start = end 04289 end += 8 04290 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04291 start = end 04292 end += 4 04293 (length,) = _struct_I.unpack(str[start:end]) 04294 start = end 04295 end += length 04296 _v315.frame_id = str[start:end] 04297 _x = _v314 04298 start = end 04299 end += 8 04300 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04301 start = end 04302 end += 4 04303 (length,) = _struct_I.unpack(str[start:end]) 04304 start = end 04305 end += length 04306 _v314.encoding = str[start:end] 04307 _x = _v314 04308 start = end 04309 end += 5 04310 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 04311 start = end 04312 end += 4 04313 (length,) = _struct_I.unpack(str[start:end]) 04314 start = end 04315 end += length 04316 _v314.data = str[start:end] 04317 _v317 = _v310.disparity_image 04318 _v318 = _v317.header 04319 start = end 04320 end += 4 04321 (_v318.seq,) = _struct_I.unpack(str[start:end]) 04322 _v319 = _v318.stamp 04323 _x = _v319 04324 start = end 04325 end += 8 04326 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04327 start = end 04328 end += 4 04329 (length,) = _struct_I.unpack(str[start:end]) 04330 start = end 04331 end += length 04332 _v318.frame_id = str[start:end] 04333 _x = _v317 04334 start = end 04335 end += 8 04336 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04337 start = end 04338 end += 4 04339 (length,) = _struct_I.unpack(str[start:end]) 04340 start = end 04341 end += length 04342 _v317.encoding = str[start:end] 04343 _x = _v317 04344 start = end 04345 end += 5 04346 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 04347 start = end 04348 end += 4 04349 (length,) = _struct_I.unpack(str[start:end]) 04350 start = end 04351 end += length 04352 _v317.data = str[start:end] 04353 _v320 = _v310.cam_info 04354 _v321 = _v320.header 04355 start = end 04356 end += 4 04357 (_v321.seq,) = _struct_I.unpack(str[start:end]) 04358 _v322 = _v321.stamp 04359 _x = _v322 04360 start = end 04361 end += 8 04362 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04363 start = end 04364 end += 4 04365 (length,) = _struct_I.unpack(str[start:end]) 04366 start = end 04367 end += length 04368 _v321.frame_id = str[start:end] 04369 _x = _v320 04370 start = end 04371 end += 8 04372 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04373 start = end 04374 end += 4 04375 (length,) = _struct_I.unpack(str[start:end]) 04376 start = end 04377 end += length 04378 _v320.distortion_model = str[start:end] 04379 start = end 04380 end += 4 04381 (length,) = _struct_I.unpack(str[start:end]) 04382 pattern = '<%sd'%length 04383 start = end 04384 end += struct.calcsize(pattern) 04385 _v320.D = struct.unpack(pattern, str[start:end]) 04386 start = end 04387 end += 72 04388 _v320.K = _struct_9d.unpack(str[start:end]) 04389 start = end 04390 end += 72 04391 _v320.R = _struct_9d.unpack(str[start:end]) 04392 start = end 04393 end += 96 04394 _v320.P = _struct_12d.unpack(str[start:end]) 04395 _x = _v320 04396 start = end 04397 end += 8 04398 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 04399 _v323 = _v320.roi 04400 _x = _v323 04401 start = end 04402 end += 17 04403 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 04404 _v323.do_rectify = bool(_v323.do_rectify) 04405 _v324 = _v310.roi_box_pose 04406 _v325 = _v324.header 04407 start = end 04408 end += 4 04409 (_v325.seq,) = _struct_I.unpack(str[start:end]) 04410 _v326 = _v325.stamp 04411 _x = _v326 04412 start = end 04413 end += 8 04414 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04415 start = end 04416 end += 4 04417 (length,) = _struct_I.unpack(str[start:end]) 04418 start = end 04419 end += length 04420 _v325.frame_id = str[start:end] 04421 _v327 = _v324.pose 04422 _v328 = _v327.position 04423 _x = _v328 04424 start = end 04425 end += 24 04426 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04427 _v329 = _v327.orientation 04428 _x = _v329 04429 start = end 04430 end += 32 04431 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04432 _v330 = _v310.roi_box_dims 04433 _x = _v330 04434 start = end 04435 end += 24 04436 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04437 start = end 04438 end += 4 04439 (length,) = _struct_I.unpack(str[start:end]) 04440 start = end 04441 end += length 04442 val2.collision_name = str[start:end] 04443 val1.moved_obstacles.append(val2) 04444 self.action_result.result.attempted_grasps.append(val1) 04445 start = end 04446 end += 4 04447 (length,) = _struct_I.unpack(str[start:end]) 04448 self.action_result.result.attempted_grasp_results = [] 04449 for i in range(0, length): 04450 val1 = object_manipulation_msgs.msg.GraspResult() 04451 _x = val1 04452 start = end 04453 end += 5 04454 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end]) 04455 val1.continuation_possible = bool(val1.continuation_possible) 04456 self.action_result.result.attempted_grasp_results.append(val1) 04457 _x = self 04458 start = end 04459 end += 12 04460 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 04461 start = end 04462 end += 4 04463 (length,) = _struct_I.unpack(str[start:end]) 04464 start = end 04465 end += length 04466 self.action_feedback.header.frame_id = str[start:end] 04467 _x = self 04468 start = end 04469 end += 8 04470 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 04471 start = end 04472 end += 4 04473 (length,) = _struct_I.unpack(str[start:end]) 04474 start = end 04475 end += length 04476 self.action_feedback.status.goal_id.id = str[start:end] 04477 start = end 04478 end += 1 04479 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 04480 start = end 04481 end += 4 04482 (length,) = _struct_I.unpack(str[start:end]) 04483 start = end 04484 end += length 04485 self.action_feedback.status.text = str[start:end] 04486 _x = self 04487 start = end 04488 end += 8 04489 (_x.action_feedback.feedback.current_grasp, _x.action_feedback.feedback.total_grasps,) = _struct_2i.unpack(str[start:end]) 04490 return self 04491 except struct.error as e: 04492 raise roslib.message.DeserializationError(e) #most likely buffer underfill 04493 04494 04495 def serialize_numpy(self, buff, numpy): 04496 """ 04497 serialize message with numpy array types into buffer 04498 @param buff: buffer 04499 @type buff: StringIO 04500 @param numpy: numpy python module 04501 @type numpy module 04502 """ 04503 try: 04504 _x = self 04505 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 04506 _x = self.action_goal.header.frame_id 04507 length = len(_x) 04508 buff.write(struct.pack('<I%ss'%length, length, _x)) 04509 _x = self 04510 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 04511 _x = self.action_goal.goal_id.id 04512 length = len(_x) 04513 buff.write(struct.pack('<I%ss'%length, length, _x)) 04514 _x = self.action_goal.goal.arm_name 04515 length = len(_x) 04516 buff.write(struct.pack('<I%ss'%length, length, _x)) 04517 _x = self.action_goal.goal.target.reference_frame_id 04518 length = len(_x) 04519 buff.write(struct.pack('<I%ss'%length, length, _x)) 04520 length = len(self.action_goal.goal.target.potential_models) 04521 buff.write(_struct_I.pack(length)) 04522 for val1 in self.action_goal.goal.target.potential_models: 04523 buff.write(_struct_i.pack(val1.model_id)) 04524 _v331 = val1.pose 04525 _v332 = _v331.header 04526 buff.write(_struct_I.pack(_v332.seq)) 04527 _v333 = _v332.stamp 04528 _x = _v333 04529 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04530 _x = _v332.frame_id 04531 length = len(_x) 04532 buff.write(struct.pack('<I%ss'%length, length, _x)) 04533 _v334 = _v331.pose 04534 _v335 = _v334.position 04535 _x = _v335 04536 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04537 _v336 = _v334.orientation 04538 _x = _v336 04539 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04540 buff.write(_struct_f.pack(val1.confidence)) 04541 _x = val1.detector_name 04542 length = len(_x) 04543 buff.write(struct.pack('<I%ss'%length, length, _x)) 04544 _x = self 04545 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs)) 04546 _x = self.action_goal.goal.target.cluster.header.frame_id 04547 length = len(_x) 04548 buff.write(struct.pack('<I%ss'%length, length, _x)) 04549 length = len(self.action_goal.goal.target.cluster.points) 04550 buff.write(_struct_I.pack(length)) 04551 for val1 in self.action_goal.goal.target.cluster.points: 04552 _x = val1 04553 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 04554 length = len(self.action_goal.goal.target.cluster.channels) 04555 buff.write(_struct_I.pack(length)) 04556 for val1 in self.action_goal.goal.target.cluster.channels: 04557 _x = val1.name 04558 length = len(_x) 04559 buff.write(struct.pack('<I%ss'%length, length, _x)) 04560 length = len(val1.values) 04561 buff.write(_struct_I.pack(length)) 04562 pattern = '<%sf'%length 04563 buff.write(val1.values.tostring()) 04564 _x = self 04565 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs)) 04566 _x = self.action_goal.goal.target.region.cloud.header.frame_id 04567 length = len(_x) 04568 buff.write(struct.pack('<I%ss'%length, length, _x)) 04569 _x = self 04570 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width)) 04571 length = len(self.action_goal.goal.target.region.cloud.fields) 04572 buff.write(_struct_I.pack(length)) 04573 for val1 in self.action_goal.goal.target.region.cloud.fields: 04574 _x = val1.name 04575 length = len(_x) 04576 buff.write(struct.pack('<I%ss'%length, length, _x)) 04577 _x = val1 04578 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 04579 _x = self 04580 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step)) 04581 _x = self.action_goal.goal.target.region.cloud.data 04582 length = len(_x) 04583 # - if encoded as a list instead, serialize as bytes instead of string 04584 if type(_x) in [list, tuple]: 04585 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04586 else: 04587 buff.write(struct.pack('<I%ss'%length, length, _x)) 04588 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense)) 04589 length = len(self.action_goal.goal.target.region.mask) 04590 buff.write(_struct_I.pack(length)) 04591 pattern = '<%si'%length 04592 buff.write(self.action_goal.goal.target.region.mask.tostring()) 04593 _x = self 04594 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs)) 04595 _x = self.action_goal.goal.target.region.image.header.frame_id 04596 length = len(_x) 04597 buff.write(struct.pack('<I%ss'%length, length, _x)) 04598 _x = self 04599 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width)) 04600 _x = self.action_goal.goal.target.region.image.encoding 04601 length = len(_x) 04602 buff.write(struct.pack('<I%ss'%length, length, _x)) 04603 _x = self 04604 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step)) 04605 _x = self.action_goal.goal.target.region.image.data 04606 length = len(_x) 04607 # - if encoded as a list instead, serialize as bytes instead of string 04608 if type(_x) in [list, tuple]: 04609 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04610 else: 04611 buff.write(struct.pack('<I%ss'%length, length, _x)) 04612 _x = self 04613 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs)) 04614 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id 04615 length = len(_x) 04616 buff.write(struct.pack('<I%ss'%length, length, _x)) 04617 _x = self 04618 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width)) 04619 _x = self.action_goal.goal.target.region.disparity_image.encoding 04620 length = len(_x) 04621 buff.write(struct.pack('<I%ss'%length, length, _x)) 04622 _x = self 04623 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step)) 04624 _x = self.action_goal.goal.target.region.disparity_image.data 04625 length = len(_x) 04626 # - if encoded as a list instead, serialize as bytes instead of string 04627 if type(_x) in [list, tuple]: 04628 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04629 else: 04630 buff.write(struct.pack('<I%ss'%length, length, _x)) 04631 _x = self 04632 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs)) 04633 _x = self.action_goal.goal.target.region.cam_info.header.frame_id 04634 length = len(_x) 04635 buff.write(struct.pack('<I%ss'%length, length, _x)) 04636 _x = self 04637 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width)) 04638 _x = self.action_goal.goal.target.region.cam_info.distortion_model 04639 length = len(_x) 04640 buff.write(struct.pack('<I%ss'%length, length, _x)) 04641 length = len(self.action_goal.goal.target.region.cam_info.D) 04642 buff.write(_struct_I.pack(length)) 04643 pattern = '<%sd'%length 04644 buff.write(self.action_goal.goal.target.region.cam_info.D.tostring()) 04645 buff.write(self.action_goal.goal.target.region.cam_info.K.tostring()) 04646 buff.write(self.action_goal.goal.target.region.cam_info.R.tostring()) 04647 buff.write(self.action_goal.goal.target.region.cam_info.P.tostring()) 04648 _x = self 04649 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs)) 04650 _x = self.action_goal.goal.target.region.roi_box_pose.header.frame_id 04651 length = len(_x) 04652 buff.write(struct.pack('<I%ss'%length, length, _x)) 04653 _x = self 04654 buff.write(_struct_10d.pack(_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z)) 04655 _x = self.action_goal.goal.target.collision_name 04656 length = len(_x) 04657 buff.write(struct.pack('<I%ss'%length, length, _x)) 04658 length = len(self.action_goal.goal.desired_grasps) 04659 buff.write(_struct_I.pack(length)) 04660 for val1 in self.action_goal.goal.desired_grasps: 04661 _v337 = val1.pre_grasp_posture 04662 _v338 = _v337.header 04663 buff.write(_struct_I.pack(_v338.seq)) 04664 _v339 = _v338.stamp 04665 _x = _v339 04666 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04667 _x = _v338.frame_id 04668 length = len(_x) 04669 buff.write(struct.pack('<I%ss'%length, length, _x)) 04670 length = len(_v337.name) 04671 buff.write(_struct_I.pack(length)) 04672 for val3 in _v337.name: 04673 length = len(val3) 04674 buff.write(struct.pack('<I%ss'%length, length, val3)) 04675 length = len(_v337.position) 04676 buff.write(_struct_I.pack(length)) 04677 pattern = '<%sd'%length 04678 buff.write(_v337.position.tostring()) 04679 length = len(_v337.velocity) 04680 buff.write(_struct_I.pack(length)) 04681 pattern = '<%sd'%length 04682 buff.write(_v337.velocity.tostring()) 04683 length = len(_v337.effort) 04684 buff.write(_struct_I.pack(length)) 04685 pattern = '<%sd'%length 04686 buff.write(_v337.effort.tostring()) 04687 _v340 = val1.grasp_posture 04688 _v341 = _v340.header 04689 buff.write(_struct_I.pack(_v341.seq)) 04690 _v342 = _v341.stamp 04691 _x = _v342 04692 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04693 _x = _v341.frame_id 04694 length = len(_x) 04695 buff.write(struct.pack('<I%ss'%length, length, _x)) 04696 length = len(_v340.name) 04697 buff.write(_struct_I.pack(length)) 04698 for val3 in _v340.name: 04699 length = len(val3) 04700 buff.write(struct.pack('<I%ss'%length, length, val3)) 04701 length = len(_v340.position) 04702 buff.write(_struct_I.pack(length)) 04703 pattern = '<%sd'%length 04704 buff.write(_v340.position.tostring()) 04705 length = len(_v340.velocity) 04706 buff.write(_struct_I.pack(length)) 04707 pattern = '<%sd'%length 04708 buff.write(_v340.velocity.tostring()) 04709 length = len(_v340.effort) 04710 buff.write(_struct_I.pack(length)) 04711 pattern = '<%sd'%length 04712 buff.write(_v340.effort.tostring()) 04713 _v343 = val1.grasp_pose 04714 _v344 = _v343.position 04715 _x = _v344 04716 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04717 _v345 = _v343.orientation 04718 _x = _v345 04719 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04720 _x = val1 04721 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 04722 length = len(val1.moved_obstacles) 04723 buff.write(_struct_I.pack(length)) 04724 for val2 in val1.moved_obstacles: 04725 _x = val2.reference_frame_id 04726 length = len(_x) 04727 buff.write(struct.pack('<I%ss'%length, length, _x)) 04728 length = len(val2.potential_models) 04729 buff.write(_struct_I.pack(length)) 04730 for val3 in val2.potential_models: 04731 buff.write(_struct_i.pack(val3.model_id)) 04732 _v346 = val3.pose 04733 _v347 = _v346.header 04734 buff.write(_struct_I.pack(_v347.seq)) 04735 _v348 = _v347.stamp 04736 _x = _v348 04737 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04738 _x = _v347.frame_id 04739 length = len(_x) 04740 buff.write(struct.pack('<I%ss'%length, length, _x)) 04741 _v349 = _v346.pose 04742 _v350 = _v349.position 04743 _x = _v350 04744 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04745 _v351 = _v349.orientation 04746 _x = _v351 04747 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04748 buff.write(_struct_f.pack(val3.confidence)) 04749 _x = val3.detector_name 04750 length = len(_x) 04751 buff.write(struct.pack('<I%ss'%length, length, _x)) 04752 _v352 = val2.cluster 04753 _v353 = _v352.header 04754 buff.write(_struct_I.pack(_v353.seq)) 04755 _v354 = _v353.stamp 04756 _x = _v354 04757 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04758 _x = _v353.frame_id 04759 length = len(_x) 04760 buff.write(struct.pack('<I%ss'%length, length, _x)) 04761 length = len(_v352.points) 04762 buff.write(_struct_I.pack(length)) 04763 for val4 in _v352.points: 04764 _x = val4 04765 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 04766 length = len(_v352.channels) 04767 buff.write(_struct_I.pack(length)) 04768 for val4 in _v352.channels: 04769 _x = val4.name 04770 length = len(_x) 04771 buff.write(struct.pack('<I%ss'%length, length, _x)) 04772 length = len(val4.values) 04773 buff.write(_struct_I.pack(length)) 04774 pattern = '<%sf'%length 04775 buff.write(val4.values.tostring()) 04776 _v355 = val2.region 04777 _v356 = _v355.cloud 04778 _v357 = _v356.header 04779 buff.write(_struct_I.pack(_v357.seq)) 04780 _v358 = _v357.stamp 04781 _x = _v358 04782 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04783 _x = _v357.frame_id 04784 length = len(_x) 04785 buff.write(struct.pack('<I%ss'%length, length, _x)) 04786 _x = _v356 04787 buff.write(_struct_2I.pack(_x.height, _x.width)) 04788 length = len(_v356.fields) 04789 buff.write(_struct_I.pack(length)) 04790 for val5 in _v356.fields: 04791 _x = val5.name 04792 length = len(_x) 04793 buff.write(struct.pack('<I%ss'%length, length, _x)) 04794 _x = val5 04795 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 04796 _x = _v356 04797 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 04798 _x = _v356.data 04799 length = len(_x) 04800 # - if encoded as a list instead, serialize as bytes instead of string 04801 if type(_x) in [list, tuple]: 04802 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04803 else: 04804 buff.write(struct.pack('<I%ss'%length, length, _x)) 04805 buff.write(_struct_B.pack(_v356.is_dense)) 04806 length = len(_v355.mask) 04807 buff.write(_struct_I.pack(length)) 04808 pattern = '<%si'%length 04809 buff.write(_v355.mask.tostring()) 04810 _v359 = _v355.image 04811 _v360 = _v359.header 04812 buff.write(_struct_I.pack(_v360.seq)) 04813 _v361 = _v360.stamp 04814 _x = _v361 04815 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04816 _x = _v360.frame_id 04817 length = len(_x) 04818 buff.write(struct.pack('<I%ss'%length, length, _x)) 04819 _x = _v359 04820 buff.write(_struct_2I.pack(_x.height, _x.width)) 04821 _x = _v359.encoding 04822 length = len(_x) 04823 buff.write(struct.pack('<I%ss'%length, length, _x)) 04824 _x = _v359 04825 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 04826 _x = _v359.data 04827 length = len(_x) 04828 # - if encoded as a list instead, serialize as bytes instead of string 04829 if type(_x) in [list, tuple]: 04830 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04831 else: 04832 buff.write(struct.pack('<I%ss'%length, length, _x)) 04833 _v362 = _v355.disparity_image 04834 _v363 = _v362.header 04835 buff.write(_struct_I.pack(_v363.seq)) 04836 _v364 = _v363.stamp 04837 _x = _v364 04838 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04839 _x = _v363.frame_id 04840 length = len(_x) 04841 buff.write(struct.pack('<I%ss'%length, length, _x)) 04842 _x = _v362 04843 buff.write(_struct_2I.pack(_x.height, _x.width)) 04844 _x = _v362.encoding 04845 length = len(_x) 04846 buff.write(struct.pack('<I%ss'%length, length, _x)) 04847 _x = _v362 04848 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 04849 _x = _v362.data 04850 length = len(_x) 04851 # - if encoded as a list instead, serialize as bytes instead of string 04852 if type(_x) in [list, tuple]: 04853 buff.write(struct.pack('<I%sB'%length, length, *_x)) 04854 else: 04855 buff.write(struct.pack('<I%ss'%length, length, _x)) 04856 _v365 = _v355.cam_info 04857 _v366 = _v365.header 04858 buff.write(_struct_I.pack(_v366.seq)) 04859 _v367 = _v366.stamp 04860 _x = _v367 04861 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04862 _x = _v366.frame_id 04863 length = len(_x) 04864 buff.write(struct.pack('<I%ss'%length, length, _x)) 04865 _x = _v365 04866 buff.write(_struct_2I.pack(_x.height, _x.width)) 04867 _x = _v365.distortion_model 04868 length = len(_x) 04869 buff.write(struct.pack('<I%ss'%length, length, _x)) 04870 length = len(_v365.D) 04871 buff.write(_struct_I.pack(length)) 04872 pattern = '<%sd'%length 04873 buff.write(_v365.D.tostring()) 04874 buff.write(_v365.K.tostring()) 04875 buff.write(_v365.R.tostring()) 04876 buff.write(_v365.P.tostring()) 04877 _x = _v365 04878 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 04879 _v368 = _v365.roi 04880 _x = _v368 04881 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 04882 _v369 = _v355.roi_box_pose 04883 _v370 = _v369.header 04884 buff.write(_struct_I.pack(_v370.seq)) 04885 _v371 = _v370.stamp 04886 _x = _v371 04887 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04888 _x = _v370.frame_id 04889 length = len(_x) 04890 buff.write(struct.pack('<I%ss'%length, length, _x)) 04891 _v372 = _v369.pose 04892 _v373 = _v372.position 04893 _x = _v373 04894 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04895 _v374 = _v372.orientation 04896 _x = _v374 04897 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04898 _v375 = _v355.roi_box_dims 04899 _x = _v375 04900 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04901 _x = val2.collision_name 04902 length = len(_x) 04903 buff.write(struct.pack('<I%ss'%length, length, _x)) 04904 _x = self 04905 buff.write(_struct_3I.pack(_x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs)) 04906 _x = self.action_goal.goal.lift.direction.header.frame_id 04907 length = len(_x) 04908 buff.write(struct.pack('<I%ss'%length, length, _x)) 04909 _x = self 04910 buff.write(_struct_3d2f.pack(_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance)) 04911 _x = self.action_goal.goal.collision_object_name 04912 length = len(_x) 04913 buff.write(struct.pack('<I%ss'%length, length, _x)) 04914 _x = self.action_goal.goal.collision_support_surface_name 04915 length = len(_x) 04916 buff.write(struct.pack('<I%ss'%length, length, _x)) 04917 _x = self 04918 buff.write(_struct_5B.pack(_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_execution, _x.action_goal.goal.use_reactive_lift, _x.action_goal.goal.only_perform_feasibility_test, _x.action_goal.goal.ignore_collisions)) 04919 length = len(self.action_goal.goal.path_constraints.joint_constraints) 04920 buff.write(_struct_I.pack(length)) 04921 for val1 in self.action_goal.goal.path_constraints.joint_constraints: 04922 _x = val1.joint_name 04923 length = len(_x) 04924 buff.write(struct.pack('<I%ss'%length, length, _x)) 04925 _x = val1 04926 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 04927 length = len(self.action_goal.goal.path_constraints.position_constraints) 04928 buff.write(_struct_I.pack(length)) 04929 for val1 in self.action_goal.goal.path_constraints.position_constraints: 04930 _v376 = val1.header 04931 buff.write(_struct_I.pack(_v376.seq)) 04932 _v377 = _v376.stamp 04933 _x = _v377 04934 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04935 _x = _v376.frame_id 04936 length = len(_x) 04937 buff.write(struct.pack('<I%ss'%length, length, _x)) 04938 _x = val1.link_name 04939 length = len(_x) 04940 buff.write(struct.pack('<I%ss'%length, length, _x)) 04941 _v378 = val1.target_point_offset 04942 _x = _v378 04943 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04944 _v379 = val1.position 04945 _x = _v379 04946 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04947 _v380 = val1.constraint_region_shape 04948 buff.write(_struct_b.pack(_v380.type)) 04949 length = len(_v380.dimensions) 04950 buff.write(_struct_I.pack(length)) 04951 pattern = '<%sd'%length 04952 buff.write(_v380.dimensions.tostring()) 04953 length = len(_v380.triangles) 04954 buff.write(_struct_I.pack(length)) 04955 pattern = '<%si'%length 04956 buff.write(_v380.triangles.tostring()) 04957 length = len(_v380.vertices) 04958 buff.write(_struct_I.pack(length)) 04959 for val3 in _v380.vertices: 04960 _x = val3 04961 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 04962 _v381 = val1.constraint_region_orientation 04963 _x = _v381 04964 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04965 buff.write(_struct_d.pack(val1.weight)) 04966 length = len(self.action_goal.goal.path_constraints.orientation_constraints) 04967 buff.write(_struct_I.pack(length)) 04968 for val1 in self.action_goal.goal.path_constraints.orientation_constraints: 04969 _v382 = val1.header 04970 buff.write(_struct_I.pack(_v382.seq)) 04971 _v383 = _v382.stamp 04972 _x = _v383 04973 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04974 _x = _v382.frame_id 04975 length = len(_x) 04976 buff.write(struct.pack('<I%ss'%length, length, _x)) 04977 _x = val1.link_name 04978 length = len(_x) 04979 buff.write(struct.pack('<I%ss'%length, length, _x)) 04980 buff.write(_struct_i.pack(val1.type)) 04981 _v384 = val1.orientation 04982 _x = _v384 04983 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 04984 _x = val1 04985 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 04986 length = len(self.action_goal.goal.path_constraints.visibility_constraints) 04987 buff.write(_struct_I.pack(length)) 04988 for val1 in self.action_goal.goal.path_constraints.visibility_constraints: 04989 _v385 = val1.header 04990 buff.write(_struct_I.pack(_v385.seq)) 04991 _v386 = _v385.stamp 04992 _x = _v386 04993 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 04994 _x = _v385.frame_id 04995 length = len(_x) 04996 buff.write(struct.pack('<I%ss'%length, length, _x)) 04997 _v387 = val1.target 04998 _v388 = _v387.header 04999 buff.write(_struct_I.pack(_v388.seq)) 05000 _v389 = _v388.stamp 05001 _x = _v389 05002 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05003 _x = _v388.frame_id 05004 length = len(_x) 05005 buff.write(struct.pack('<I%ss'%length, length, _x)) 05006 _v390 = _v387.point 05007 _x = _v390 05008 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05009 _v391 = val1.sensor_pose 05010 _v392 = _v391.header 05011 buff.write(_struct_I.pack(_v392.seq)) 05012 _v393 = _v392.stamp 05013 _x = _v393 05014 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05015 _x = _v392.frame_id 05016 length = len(_x) 05017 buff.write(struct.pack('<I%ss'%length, length, _x)) 05018 _v394 = _v391.pose 05019 _v395 = _v394.position 05020 _x = _v395 05021 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05022 _v396 = _v394.orientation 05023 _x = _v396 05024 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05025 buff.write(_struct_d.pack(val1.absolute_tolerance)) 05026 length = len(self.action_goal.goal.additional_collision_operations.collision_operations) 05027 buff.write(_struct_I.pack(length)) 05028 for val1 in self.action_goal.goal.additional_collision_operations.collision_operations: 05029 _x = val1.object1 05030 length = len(_x) 05031 buff.write(struct.pack('<I%ss'%length, length, _x)) 05032 _x = val1.object2 05033 length = len(_x) 05034 buff.write(struct.pack('<I%ss'%length, length, _x)) 05035 _x = val1 05036 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 05037 length = len(self.action_goal.goal.additional_link_padding) 05038 buff.write(_struct_I.pack(length)) 05039 for val1 in self.action_goal.goal.additional_link_padding: 05040 _x = val1.link_name 05041 length = len(_x) 05042 buff.write(struct.pack('<I%ss'%length, length, _x)) 05043 buff.write(_struct_d.pack(val1.padding)) 05044 length = len(self.action_goal.goal.movable_obstacles) 05045 buff.write(_struct_I.pack(length)) 05046 for val1 in self.action_goal.goal.movable_obstacles: 05047 _x = val1.reference_frame_id 05048 length = len(_x) 05049 buff.write(struct.pack('<I%ss'%length, length, _x)) 05050 length = len(val1.potential_models) 05051 buff.write(_struct_I.pack(length)) 05052 for val2 in val1.potential_models: 05053 buff.write(_struct_i.pack(val2.model_id)) 05054 _v397 = val2.pose 05055 _v398 = _v397.header 05056 buff.write(_struct_I.pack(_v398.seq)) 05057 _v399 = _v398.stamp 05058 _x = _v399 05059 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05060 _x = _v398.frame_id 05061 length = len(_x) 05062 buff.write(struct.pack('<I%ss'%length, length, _x)) 05063 _v400 = _v397.pose 05064 _v401 = _v400.position 05065 _x = _v401 05066 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05067 _v402 = _v400.orientation 05068 _x = _v402 05069 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05070 buff.write(_struct_f.pack(val2.confidence)) 05071 _x = val2.detector_name 05072 length = len(_x) 05073 buff.write(struct.pack('<I%ss'%length, length, _x)) 05074 _v403 = val1.cluster 05075 _v404 = _v403.header 05076 buff.write(_struct_I.pack(_v404.seq)) 05077 _v405 = _v404.stamp 05078 _x = _v405 05079 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05080 _x = _v404.frame_id 05081 length = len(_x) 05082 buff.write(struct.pack('<I%ss'%length, length, _x)) 05083 length = len(_v403.points) 05084 buff.write(_struct_I.pack(length)) 05085 for val3 in _v403.points: 05086 _x = val3 05087 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 05088 length = len(_v403.channels) 05089 buff.write(_struct_I.pack(length)) 05090 for val3 in _v403.channels: 05091 _x = val3.name 05092 length = len(_x) 05093 buff.write(struct.pack('<I%ss'%length, length, _x)) 05094 length = len(val3.values) 05095 buff.write(_struct_I.pack(length)) 05096 pattern = '<%sf'%length 05097 buff.write(val3.values.tostring()) 05098 _v406 = val1.region 05099 _v407 = _v406.cloud 05100 _v408 = _v407.header 05101 buff.write(_struct_I.pack(_v408.seq)) 05102 _v409 = _v408.stamp 05103 _x = _v409 05104 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05105 _x = _v408.frame_id 05106 length = len(_x) 05107 buff.write(struct.pack('<I%ss'%length, length, _x)) 05108 _x = _v407 05109 buff.write(_struct_2I.pack(_x.height, _x.width)) 05110 length = len(_v407.fields) 05111 buff.write(_struct_I.pack(length)) 05112 for val4 in _v407.fields: 05113 _x = val4.name 05114 length = len(_x) 05115 buff.write(struct.pack('<I%ss'%length, length, _x)) 05116 _x = val4 05117 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 05118 _x = _v407 05119 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 05120 _x = _v407.data 05121 length = len(_x) 05122 # - if encoded as a list instead, serialize as bytes instead of string 05123 if type(_x) in [list, tuple]: 05124 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05125 else: 05126 buff.write(struct.pack('<I%ss'%length, length, _x)) 05127 buff.write(_struct_B.pack(_v407.is_dense)) 05128 length = len(_v406.mask) 05129 buff.write(_struct_I.pack(length)) 05130 pattern = '<%si'%length 05131 buff.write(_v406.mask.tostring()) 05132 _v410 = _v406.image 05133 _v411 = _v410.header 05134 buff.write(_struct_I.pack(_v411.seq)) 05135 _v412 = _v411.stamp 05136 _x = _v412 05137 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05138 _x = _v411.frame_id 05139 length = len(_x) 05140 buff.write(struct.pack('<I%ss'%length, length, _x)) 05141 _x = _v410 05142 buff.write(_struct_2I.pack(_x.height, _x.width)) 05143 _x = _v410.encoding 05144 length = len(_x) 05145 buff.write(struct.pack('<I%ss'%length, length, _x)) 05146 _x = _v410 05147 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05148 _x = _v410.data 05149 length = len(_x) 05150 # - if encoded as a list instead, serialize as bytes instead of string 05151 if type(_x) in [list, tuple]: 05152 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05153 else: 05154 buff.write(struct.pack('<I%ss'%length, length, _x)) 05155 _v413 = _v406.disparity_image 05156 _v414 = _v413.header 05157 buff.write(_struct_I.pack(_v414.seq)) 05158 _v415 = _v414.stamp 05159 _x = _v415 05160 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05161 _x = _v414.frame_id 05162 length = len(_x) 05163 buff.write(struct.pack('<I%ss'%length, length, _x)) 05164 _x = _v413 05165 buff.write(_struct_2I.pack(_x.height, _x.width)) 05166 _x = _v413.encoding 05167 length = len(_x) 05168 buff.write(struct.pack('<I%ss'%length, length, _x)) 05169 _x = _v413 05170 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05171 _x = _v413.data 05172 length = len(_x) 05173 # - if encoded as a list instead, serialize as bytes instead of string 05174 if type(_x) in [list, tuple]: 05175 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05176 else: 05177 buff.write(struct.pack('<I%ss'%length, length, _x)) 05178 _v416 = _v406.cam_info 05179 _v417 = _v416.header 05180 buff.write(_struct_I.pack(_v417.seq)) 05181 _v418 = _v417.stamp 05182 _x = _v418 05183 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05184 _x = _v417.frame_id 05185 length = len(_x) 05186 buff.write(struct.pack('<I%ss'%length, length, _x)) 05187 _x = _v416 05188 buff.write(_struct_2I.pack(_x.height, _x.width)) 05189 _x = _v416.distortion_model 05190 length = len(_x) 05191 buff.write(struct.pack('<I%ss'%length, length, _x)) 05192 length = len(_v416.D) 05193 buff.write(_struct_I.pack(length)) 05194 pattern = '<%sd'%length 05195 buff.write(_v416.D.tostring()) 05196 buff.write(_v416.K.tostring()) 05197 buff.write(_v416.R.tostring()) 05198 buff.write(_v416.P.tostring()) 05199 _x = _v416 05200 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 05201 _v419 = _v416.roi 05202 _x = _v419 05203 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 05204 _v420 = _v406.roi_box_pose 05205 _v421 = _v420.header 05206 buff.write(_struct_I.pack(_v421.seq)) 05207 _v422 = _v421.stamp 05208 _x = _v422 05209 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05210 _x = _v421.frame_id 05211 length = len(_x) 05212 buff.write(struct.pack('<I%ss'%length, length, _x)) 05213 _v423 = _v420.pose 05214 _v424 = _v423.position 05215 _x = _v424 05216 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05217 _v425 = _v423.orientation 05218 _x = _v425 05219 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05220 _v426 = _v406.roi_box_dims 05221 _x = _v426 05222 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05223 _x = val1.collision_name 05224 length = len(_x) 05225 buff.write(struct.pack('<I%ss'%length, length, _x)) 05226 _x = self 05227 buff.write(_struct_f3I.pack(_x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 05228 _x = self.action_result.header.frame_id 05229 length = len(_x) 05230 buff.write(struct.pack('<I%ss'%length, length, _x)) 05231 _x = self 05232 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 05233 _x = self.action_result.status.goal_id.id 05234 length = len(_x) 05235 buff.write(struct.pack('<I%ss'%length, length, _x)) 05236 buff.write(_struct_B.pack(self.action_result.status.status)) 05237 _x = self.action_result.status.text 05238 length = len(_x) 05239 buff.write(struct.pack('<I%ss'%length, length, _x)) 05240 _x = self 05241 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_result.result.grasp.pre_grasp_posture.header.seq, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.secs, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.nsecs)) 05242 _x = self.action_result.result.grasp.pre_grasp_posture.header.frame_id 05243 length = len(_x) 05244 buff.write(struct.pack('<I%ss'%length, length, _x)) 05245 length = len(self.action_result.result.grasp.pre_grasp_posture.name) 05246 buff.write(_struct_I.pack(length)) 05247 for val1 in self.action_result.result.grasp.pre_grasp_posture.name: 05248 length = len(val1) 05249 buff.write(struct.pack('<I%ss'%length, length, val1)) 05250 length = len(self.action_result.result.grasp.pre_grasp_posture.position) 05251 buff.write(_struct_I.pack(length)) 05252 pattern = '<%sd'%length 05253 buff.write(self.action_result.result.grasp.pre_grasp_posture.position.tostring()) 05254 length = len(self.action_result.result.grasp.pre_grasp_posture.velocity) 05255 buff.write(_struct_I.pack(length)) 05256 pattern = '<%sd'%length 05257 buff.write(self.action_result.result.grasp.pre_grasp_posture.velocity.tostring()) 05258 length = len(self.action_result.result.grasp.pre_grasp_posture.effort) 05259 buff.write(_struct_I.pack(length)) 05260 pattern = '<%sd'%length 05261 buff.write(self.action_result.result.grasp.pre_grasp_posture.effort.tostring()) 05262 _x = self 05263 buff.write(_struct_3I.pack(_x.action_result.result.grasp.grasp_posture.header.seq, _x.action_result.result.grasp.grasp_posture.header.stamp.secs, _x.action_result.result.grasp.grasp_posture.header.stamp.nsecs)) 05264 _x = self.action_result.result.grasp.grasp_posture.header.frame_id 05265 length = len(_x) 05266 buff.write(struct.pack('<I%ss'%length, length, _x)) 05267 length = len(self.action_result.result.grasp.grasp_posture.name) 05268 buff.write(_struct_I.pack(length)) 05269 for val1 in self.action_result.result.grasp.grasp_posture.name: 05270 length = len(val1) 05271 buff.write(struct.pack('<I%ss'%length, length, val1)) 05272 length = len(self.action_result.result.grasp.grasp_posture.position) 05273 buff.write(_struct_I.pack(length)) 05274 pattern = '<%sd'%length 05275 buff.write(self.action_result.result.grasp.grasp_posture.position.tostring()) 05276 length = len(self.action_result.result.grasp.grasp_posture.velocity) 05277 buff.write(_struct_I.pack(length)) 05278 pattern = '<%sd'%length 05279 buff.write(self.action_result.result.grasp.grasp_posture.velocity.tostring()) 05280 length = len(self.action_result.result.grasp.grasp_posture.effort) 05281 buff.write(_struct_I.pack(length)) 05282 pattern = '<%sd'%length 05283 buff.write(self.action_result.result.grasp.grasp_posture.effort.tostring()) 05284 _x = self 05285 buff.write(_struct_8dB2f.pack(_x.action_result.result.grasp.grasp_pose.position.x, _x.action_result.result.grasp.grasp_pose.position.y, _x.action_result.result.grasp.grasp_pose.position.z, _x.action_result.result.grasp.grasp_pose.orientation.x, _x.action_result.result.grasp.grasp_pose.orientation.y, _x.action_result.result.grasp.grasp_pose.orientation.z, _x.action_result.result.grasp.grasp_pose.orientation.w, _x.action_result.result.grasp.success_probability, _x.action_result.result.grasp.cluster_rep, _x.action_result.result.grasp.desired_approach_distance, _x.action_result.result.grasp.min_approach_distance)) 05286 length = len(self.action_result.result.grasp.moved_obstacles) 05287 buff.write(_struct_I.pack(length)) 05288 for val1 in self.action_result.result.grasp.moved_obstacles: 05289 _x = val1.reference_frame_id 05290 length = len(_x) 05291 buff.write(struct.pack('<I%ss'%length, length, _x)) 05292 length = len(val1.potential_models) 05293 buff.write(_struct_I.pack(length)) 05294 for val2 in val1.potential_models: 05295 buff.write(_struct_i.pack(val2.model_id)) 05296 _v427 = val2.pose 05297 _v428 = _v427.header 05298 buff.write(_struct_I.pack(_v428.seq)) 05299 _v429 = _v428.stamp 05300 _x = _v429 05301 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05302 _x = _v428.frame_id 05303 length = len(_x) 05304 buff.write(struct.pack('<I%ss'%length, length, _x)) 05305 _v430 = _v427.pose 05306 _v431 = _v430.position 05307 _x = _v431 05308 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05309 _v432 = _v430.orientation 05310 _x = _v432 05311 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05312 buff.write(_struct_f.pack(val2.confidence)) 05313 _x = val2.detector_name 05314 length = len(_x) 05315 buff.write(struct.pack('<I%ss'%length, length, _x)) 05316 _v433 = val1.cluster 05317 _v434 = _v433.header 05318 buff.write(_struct_I.pack(_v434.seq)) 05319 _v435 = _v434.stamp 05320 _x = _v435 05321 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05322 _x = _v434.frame_id 05323 length = len(_x) 05324 buff.write(struct.pack('<I%ss'%length, length, _x)) 05325 length = len(_v433.points) 05326 buff.write(_struct_I.pack(length)) 05327 for val3 in _v433.points: 05328 _x = val3 05329 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 05330 length = len(_v433.channels) 05331 buff.write(_struct_I.pack(length)) 05332 for val3 in _v433.channels: 05333 _x = val3.name 05334 length = len(_x) 05335 buff.write(struct.pack('<I%ss'%length, length, _x)) 05336 length = len(val3.values) 05337 buff.write(_struct_I.pack(length)) 05338 pattern = '<%sf'%length 05339 buff.write(val3.values.tostring()) 05340 _v436 = val1.region 05341 _v437 = _v436.cloud 05342 _v438 = _v437.header 05343 buff.write(_struct_I.pack(_v438.seq)) 05344 _v439 = _v438.stamp 05345 _x = _v439 05346 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05347 _x = _v438.frame_id 05348 length = len(_x) 05349 buff.write(struct.pack('<I%ss'%length, length, _x)) 05350 _x = _v437 05351 buff.write(_struct_2I.pack(_x.height, _x.width)) 05352 length = len(_v437.fields) 05353 buff.write(_struct_I.pack(length)) 05354 for val4 in _v437.fields: 05355 _x = val4.name 05356 length = len(_x) 05357 buff.write(struct.pack('<I%ss'%length, length, _x)) 05358 _x = val4 05359 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 05360 _x = _v437 05361 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 05362 _x = _v437.data 05363 length = len(_x) 05364 # - if encoded as a list instead, serialize as bytes instead of string 05365 if type(_x) in [list, tuple]: 05366 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05367 else: 05368 buff.write(struct.pack('<I%ss'%length, length, _x)) 05369 buff.write(_struct_B.pack(_v437.is_dense)) 05370 length = len(_v436.mask) 05371 buff.write(_struct_I.pack(length)) 05372 pattern = '<%si'%length 05373 buff.write(_v436.mask.tostring()) 05374 _v440 = _v436.image 05375 _v441 = _v440.header 05376 buff.write(_struct_I.pack(_v441.seq)) 05377 _v442 = _v441.stamp 05378 _x = _v442 05379 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05380 _x = _v441.frame_id 05381 length = len(_x) 05382 buff.write(struct.pack('<I%ss'%length, length, _x)) 05383 _x = _v440 05384 buff.write(_struct_2I.pack(_x.height, _x.width)) 05385 _x = _v440.encoding 05386 length = len(_x) 05387 buff.write(struct.pack('<I%ss'%length, length, _x)) 05388 _x = _v440 05389 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05390 _x = _v440.data 05391 length = len(_x) 05392 # - if encoded as a list instead, serialize as bytes instead of string 05393 if type(_x) in [list, tuple]: 05394 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05395 else: 05396 buff.write(struct.pack('<I%ss'%length, length, _x)) 05397 _v443 = _v436.disparity_image 05398 _v444 = _v443.header 05399 buff.write(_struct_I.pack(_v444.seq)) 05400 _v445 = _v444.stamp 05401 _x = _v445 05402 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05403 _x = _v444.frame_id 05404 length = len(_x) 05405 buff.write(struct.pack('<I%ss'%length, length, _x)) 05406 _x = _v443 05407 buff.write(_struct_2I.pack(_x.height, _x.width)) 05408 _x = _v443.encoding 05409 length = len(_x) 05410 buff.write(struct.pack('<I%ss'%length, length, _x)) 05411 _x = _v443 05412 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05413 _x = _v443.data 05414 length = len(_x) 05415 # - if encoded as a list instead, serialize as bytes instead of string 05416 if type(_x) in [list, tuple]: 05417 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05418 else: 05419 buff.write(struct.pack('<I%ss'%length, length, _x)) 05420 _v446 = _v436.cam_info 05421 _v447 = _v446.header 05422 buff.write(_struct_I.pack(_v447.seq)) 05423 _v448 = _v447.stamp 05424 _x = _v448 05425 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05426 _x = _v447.frame_id 05427 length = len(_x) 05428 buff.write(struct.pack('<I%ss'%length, length, _x)) 05429 _x = _v446 05430 buff.write(_struct_2I.pack(_x.height, _x.width)) 05431 _x = _v446.distortion_model 05432 length = len(_x) 05433 buff.write(struct.pack('<I%ss'%length, length, _x)) 05434 length = len(_v446.D) 05435 buff.write(_struct_I.pack(length)) 05436 pattern = '<%sd'%length 05437 buff.write(_v446.D.tostring()) 05438 buff.write(_v446.K.tostring()) 05439 buff.write(_v446.R.tostring()) 05440 buff.write(_v446.P.tostring()) 05441 _x = _v446 05442 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 05443 _v449 = _v446.roi 05444 _x = _v449 05445 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 05446 _v450 = _v436.roi_box_pose 05447 _v451 = _v450.header 05448 buff.write(_struct_I.pack(_v451.seq)) 05449 _v452 = _v451.stamp 05450 _x = _v452 05451 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05452 _x = _v451.frame_id 05453 length = len(_x) 05454 buff.write(struct.pack('<I%ss'%length, length, _x)) 05455 _v453 = _v450.pose 05456 _v454 = _v453.position 05457 _x = _v454 05458 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05459 _v455 = _v453.orientation 05460 _x = _v455 05461 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05462 _v456 = _v436.roi_box_dims 05463 _x = _v456 05464 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05465 _x = val1.collision_name 05466 length = len(_x) 05467 buff.write(struct.pack('<I%ss'%length, length, _x)) 05468 length = len(self.action_result.result.attempted_grasps) 05469 buff.write(_struct_I.pack(length)) 05470 for val1 in self.action_result.result.attempted_grasps: 05471 _v457 = val1.pre_grasp_posture 05472 _v458 = _v457.header 05473 buff.write(_struct_I.pack(_v458.seq)) 05474 _v459 = _v458.stamp 05475 _x = _v459 05476 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05477 _x = _v458.frame_id 05478 length = len(_x) 05479 buff.write(struct.pack('<I%ss'%length, length, _x)) 05480 length = len(_v457.name) 05481 buff.write(_struct_I.pack(length)) 05482 for val3 in _v457.name: 05483 length = len(val3) 05484 buff.write(struct.pack('<I%ss'%length, length, val3)) 05485 length = len(_v457.position) 05486 buff.write(_struct_I.pack(length)) 05487 pattern = '<%sd'%length 05488 buff.write(_v457.position.tostring()) 05489 length = len(_v457.velocity) 05490 buff.write(_struct_I.pack(length)) 05491 pattern = '<%sd'%length 05492 buff.write(_v457.velocity.tostring()) 05493 length = len(_v457.effort) 05494 buff.write(_struct_I.pack(length)) 05495 pattern = '<%sd'%length 05496 buff.write(_v457.effort.tostring()) 05497 _v460 = val1.grasp_posture 05498 _v461 = _v460.header 05499 buff.write(_struct_I.pack(_v461.seq)) 05500 _v462 = _v461.stamp 05501 _x = _v462 05502 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05503 _x = _v461.frame_id 05504 length = len(_x) 05505 buff.write(struct.pack('<I%ss'%length, length, _x)) 05506 length = len(_v460.name) 05507 buff.write(_struct_I.pack(length)) 05508 for val3 in _v460.name: 05509 length = len(val3) 05510 buff.write(struct.pack('<I%ss'%length, length, val3)) 05511 length = len(_v460.position) 05512 buff.write(_struct_I.pack(length)) 05513 pattern = '<%sd'%length 05514 buff.write(_v460.position.tostring()) 05515 length = len(_v460.velocity) 05516 buff.write(_struct_I.pack(length)) 05517 pattern = '<%sd'%length 05518 buff.write(_v460.velocity.tostring()) 05519 length = len(_v460.effort) 05520 buff.write(_struct_I.pack(length)) 05521 pattern = '<%sd'%length 05522 buff.write(_v460.effort.tostring()) 05523 _v463 = val1.grasp_pose 05524 _v464 = _v463.position 05525 _x = _v464 05526 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05527 _v465 = _v463.orientation 05528 _x = _v465 05529 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05530 _x = val1 05531 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 05532 length = len(val1.moved_obstacles) 05533 buff.write(_struct_I.pack(length)) 05534 for val2 in val1.moved_obstacles: 05535 _x = val2.reference_frame_id 05536 length = len(_x) 05537 buff.write(struct.pack('<I%ss'%length, length, _x)) 05538 length = len(val2.potential_models) 05539 buff.write(_struct_I.pack(length)) 05540 for val3 in val2.potential_models: 05541 buff.write(_struct_i.pack(val3.model_id)) 05542 _v466 = val3.pose 05543 _v467 = _v466.header 05544 buff.write(_struct_I.pack(_v467.seq)) 05545 _v468 = _v467.stamp 05546 _x = _v468 05547 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05548 _x = _v467.frame_id 05549 length = len(_x) 05550 buff.write(struct.pack('<I%ss'%length, length, _x)) 05551 _v469 = _v466.pose 05552 _v470 = _v469.position 05553 _x = _v470 05554 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05555 _v471 = _v469.orientation 05556 _x = _v471 05557 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05558 buff.write(_struct_f.pack(val3.confidence)) 05559 _x = val3.detector_name 05560 length = len(_x) 05561 buff.write(struct.pack('<I%ss'%length, length, _x)) 05562 _v472 = val2.cluster 05563 _v473 = _v472.header 05564 buff.write(_struct_I.pack(_v473.seq)) 05565 _v474 = _v473.stamp 05566 _x = _v474 05567 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05568 _x = _v473.frame_id 05569 length = len(_x) 05570 buff.write(struct.pack('<I%ss'%length, length, _x)) 05571 length = len(_v472.points) 05572 buff.write(_struct_I.pack(length)) 05573 for val4 in _v472.points: 05574 _x = val4 05575 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 05576 length = len(_v472.channels) 05577 buff.write(_struct_I.pack(length)) 05578 for val4 in _v472.channels: 05579 _x = val4.name 05580 length = len(_x) 05581 buff.write(struct.pack('<I%ss'%length, length, _x)) 05582 length = len(val4.values) 05583 buff.write(_struct_I.pack(length)) 05584 pattern = '<%sf'%length 05585 buff.write(val4.values.tostring()) 05586 _v475 = val2.region 05587 _v476 = _v475.cloud 05588 _v477 = _v476.header 05589 buff.write(_struct_I.pack(_v477.seq)) 05590 _v478 = _v477.stamp 05591 _x = _v478 05592 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05593 _x = _v477.frame_id 05594 length = len(_x) 05595 buff.write(struct.pack('<I%ss'%length, length, _x)) 05596 _x = _v476 05597 buff.write(_struct_2I.pack(_x.height, _x.width)) 05598 length = len(_v476.fields) 05599 buff.write(_struct_I.pack(length)) 05600 for val5 in _v476.fields: 05601 _x = val5.name 05602 length = len(_x) 05603 buff.write(struct.pack('<I%ss'%length, length, _x)) 05604 _x = val5 05605 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 05606 _x = _v476 05607 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 05608 _x = _v476.data 05609 length = len(_x) 05610 # - if encoded as a list instead, serialize as bytes instead of string 05611 if type(_x) in [list, tuple]: 05612 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05613 else: 05614 buff.write(struct.pack('<I%ss'%length, length, _x)) 05615 buff.write(_struct_B.pack(_v476.is_dense)) 05616 length = len(_v475.mask) 05617 buff.write(_struct_I.pack(length)) 05618 pattern = '<%si'%length 05619 buff.write(_v475.mask.tostring()) 05620 _v479 = _v475.image 05621 _v480 = _v479.header 05622 buff.write(_struct_I.pack(_v480.seq)) 05623 _v481 = _v480.stamp 05624 _x = _v481 05625 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05626 _x = _v480.frame_id 05627 length = len(_x) 05628 buff.write(struct.pack('<I%ss'%length, length, _x)) 05629 _x = _v479 05630 buff.write(_struct_2I.pack(_x.height, _x.width)) 05631 _x = _v479.encoding 05632 length = len(_x) 05633 buff.write(struct.pack('<I%ss'%length, length, _x)) 05634 _x = _v479 05635 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05636 _x = _v479.data 05637 length = len(_x) 05638 # - if encoded as a list instead, serialize as bytes instead of string 05639 if type(_x) in [list, tuple]: 05640 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05641 else: 05642 buff.write(struct.pack('<I%ss'%length, length, _x)) 05643 _v482 = _v475.disparity_image 05644 _v483 = _v482.header 05645 buff.write(_struct_I.pack(_v483.seq)) 05646 _v484 = _v483.stamp 05647 _x = _v484 05648 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05649 _x = _v483.frame_id 05650 length = len(_x) 05651 buff.write(struct.pack('<I%ss'%length, length, _x)) 05652 _x = _v482 05653 buff.write(_struct_2I.pack(_x.height, _x.width)) 05654 _x = _v482.encoding 05655 length = len(_x) 05656 buff.write(struct.pack('<I%ss'%length, length, _x)) 05657 _x = _v482 05658 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 05659 _x = _v482.data 05660 length = len(_x) 05661 # - if encoded as a list instead, serialize as bytes instead of string 05662 if type(_x) in [list, tuple]: 05663 buff.write(struct.pack('<I%sB'%length, length, *_x)) 05664 else: 05665 buff.write(struct.pack('<I%ss'%length, length, _x)) 05666 _v485 = _v475.cam_info 05667 _v486 = _v485.header 05668 buff.write(_struct_I.pack(_v486.seq)) 05669 _v487 = _v486.stamp 05670 _x = _v487 05671 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05672 _x = _v486.frame_id 05673 length = len(_x) 05674 buff.write(struct.pack('<I%ss'%length, length, _x)) 05675 _x = _v485 05676 buff.write(_struct_2I.pack(_x.height, _x.width)) 05677 _x = _v485.distortion_model 05678 length = len(_x) 05679 buff.write(struct.pack('<I%ss'%length, length, _x)) 05680 length = len(_v485.D) 05681 buff.write(_struct_I.pack(length)) 05682 pattern = '<%sd'%length 05683 buff.write(_v485.D.tostring()) 05684 buff.write(_v485.K.tostring()) 05685 buff.write(_v485.R.tostring()) 05686 buff.write(_v485.P.tostring()) 05687 _x = _v485 05688 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 05689 _v488 = _v485.roi 05690 _x = _v488 05691 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 05692 _v489 = _v475.roi_box_pose 05693 _v490 = _v489.header 05694 buff.write(_struct_I.pack(_v490.seq)) 05695 _v491 = _v490.stamp 05696 _x = _v491 05697 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 05698 _x = _v490.frame_id 05699 length = len(_x) 05700 buff.write(struct.pack('<I%ss'%length, length, _x)) 05701 _v492 = _v489.pose 05702 _v493 = _v492.position 05703 _x = _v493 05704 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05705 _v494 = _v492.orientation 05706 _x = _v494 05707 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 05708 _v495 = _v475.roi_box_dims 05709 _x = _v495 05710 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 05711 _x = val2.collision_name 05712 length = len(_x) 05713 buff.write(struct.pack('<I%ss'%length, length, _x)) 05714 length = len(self.action_result.result.attempted_grasp_results) 05715 buff.write(_struct_I.pack(length)) 05716 for val1 in self.action_result.result.attempted_grasp_results: 05717 _x = val1 05718 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible)) 05719 _x = self 05720 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 05721 _x = self.action_feedback.header.frame_id 05722 length = len(_x) 05723 buff.write(struct.pack('<I%ss'%length, length, _x)) 05724 _x = self 05725 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 05726 _x = self.action_feedback.status.goal_id.id 05727 length = len(_x) 05728 buff.write(struct.pack('<I%ss'%length, length, _x)) 05729 buff.write(_struct_B.pack(self.action_feedback.status.status)) 05730 _x = self.action_feedback.status.text 05731 length = len(_x) 05732 buff.write(struct.pack('<I%ss'%length, length, _x)) 05733 _x = self 05734 buff.write(_struct_2i.pack(_x.action_feedback.feedback.current_grasp, _x.action_feedback.feedback.total_grasps)) 05735 except struct.error as se: self._check_types(se) 05736 except TypeError as te: self._check_types(te) 05737 05738 def deserialize_numpy(self, str, numpy): 05739 """ 05740 unpack serialized message in str into this message instance using numpy for array types 05741 @param str: byte array of serialized message 05742 @type str: str 05743 @param numpy: numpy python module 05744 @type numpy: module 05745 """ 05746 try: 05747 if self.action_goal is None: 05748 self.action_goal = object_manipulation_msgs.msg.PickupActionGoal() 05749 if self.action_result is None: 05750 self.action_result = object_manipulation_msgs.msg.PickupActionResult() 05751 if self.action_feedback is None: 05752 self.action_feedback = object_manipulation_msgs.msg.PickupActionFeedback() 05753 end = 0 05754 _x = self 05755 start = end 05756 end += 12 05757 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 05758 start = end 05759 end += 4 05760 (length,) = _struct_I.unpack(str[start:end]) 05761 start = end 05762 end += length 05763 self.action_goal.header.frame_id = str[start:end] 05764 _x = self 05765 start = end 05766 end += 8 05767 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 05768 start = end 05769 end += 4 05770 (length,) = _struct_I.unpack(str[start:end]) 05771 start = end 05772 end += length 05773 self.action_goal.goal_id.id = str[start:end] 05774 start = end 05775 end += 4 05776 (length,) = _struct_I.unpack(str[start:end]) 05777 start = end 05778 end += length 05779 self.action_goal.goal.arm_name = str[start:end] 05780 start = end 05781 end += 4 05782 (length,) = _struct_I.unpack(str[start:end]) 05783 start = end 05784 end += length 05785 self.action_goal.goal.target.reference_frame_id = str[start:end] 05786 start = end 05787 end += 4 05788 (length,) = _struct_I.unpack(str[start:end]) 05789 self.action_goal.goal.target.potential_models = [] 05790 for i in range(0, length): 05791 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 05792 start = end 05793 end += 4 05794 (val1.model_id,) = _struct_i.unpack(str[start:end]) 05795 _v496 = val1.pose 05796 _v497 = _v496.header 05797 start = end 05798 end += 4 05799 (_v497.seq,) = _struct_I.unpack(str[start:end]) 05800 _v498 = _v497.stamp 05801 _x = _v498 05802 start = end 05803 end += 8 05804 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 05805 start = end 05806 end += 4 05807 (length,) = _struct_I.unpack(str[start:end]) 05808 start = end 05809 end += length 05810 _v497.frame_id = str[start:end] 05811 _v499 = _v496.pose 05812 _v500 = _v499.position 05813 _x = _v500 05814 start = end 05815 end += 24 05816 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 05817 _v501 = _v499.orientation 05818 _x = _v501 05819 start = end 05820 end += 32 05821 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 05822 start = end 05823 end += 4 05824 (val1.confidence,) = _struct_f.unpack(str[start:end]) 05825 start = end 05826 end += 4 05827 (length,) = _struct_I.unpack(str[start:end]) 05828 start = end 05829 end += length 05830 val1.detector_name = str[start:end] 05831 self.action_goal.goal.target.potential_models.append(val1) 05832 _x = self 05833 start = end 05834 end += 12 05835 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 05836 start = end 05837 end += 4 05838 (length,) = _struct_I.unpack(str[start:end]) 05839 start = end 05840 end += length 05841 self.action_goal.goal.target.cluster.header.frame_id = str[start:end] 05842 start = end 05843 end += 4 05844 (length,) = _struct_I.unpack(str[start:end]) 05845 self.action_goal.goal.target.cluster.points = [] 05846 for i in range(0, length): 05847 val1 = geometry_msgs.msg.Point32() 05848 _x = val1 05849 start = end 05850 end += 12 05851 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 05852 self.action_goal.goal.target.cluster.points.append(val1) 05853 start = end 05854 end += 4 05855 (length,) = _struct_I.unpack(str[start:end]) 05856 self.action_goal.goal.target.cluster.channels = [] 05857 for i in range(0, length): 05858 val1 = sensor_msgs.msg.ChannelFloat32() 05859 start = end 05860 end += 4 05861 (length,) = _struct_I.unpack(str[start:end]) 05862 start = end 05863 end += length 05864 val1.name = str[start:end] 05865 start = end 05866 end += 4 05867 (length,) = _struct_I.unpack(str[start:end]) 05868 pattern = '<%sf'%length 05869 start = end 05870 end += struct.calcsize(pattern) 05871 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 05872 self.action_goal.goal.target.cluster.channels.append(val1) 05873 _x = self 05874 start = end 05875 end += 12 05876 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 05877 start = end 05878 end += 4 05879 (length,) = _struct_I.unpack(str[start:end]) 05880 start = end 05881 end += length 05882 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end] 05883 _x = self 05884 start = end 05885 end += 8 05886 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 05887 start = end 05888 end += 4 05889 (length,) = _struct_I.unpack(str[start:end]) 05890 self.action_goal.goal.target.region.cloud.fields = [] 05891 for i in range(0, length): 05892 val1 = sensor_msgs.msg.PointField() 05893 start = end 05894 end += 4 05895 (length,) = _struct_I.unpack(str[start:end]) 05896 start = end 05897 end += length 05898 val1.name = str[start:end] 05899 _x = val1 05900 start = end 05901 end += 9 05902 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 05903 self.action_goal.goal.target.region.cloud.fields.append(val1) 05904 _x = self 05905 start = end 05906 end += 9 05907 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 05908 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian) 05909 start = end 05910 end += 4 05911 (length,) = _struct_I.unpack(str[start:end]) 05912 start = end 05913 end += length 05914 self.action_goal.goal.target.region.cloud.data = str[start:end] 05915 start = end 05916 end += 1 05917 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 05918 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense) 05919 start = end 05920 end += 4 05921 (length,) = _struct_I.unpack(str[start:end]) 05922 pattern = '<%si'%length 05923 start = end 05924 end += struct.calcsize(pattern) 05925 self.action_goal.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 05926 _x = self 05927 start = end 05928 end += 12 05929 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 05930 start = end 05931 end += 4 05932 (length,) = _struct_I.unpack(str[start:end]) 05933 start = end 05934 end += length 05935 self.action_goal.goal.target.region.image.header.frame_id = str[start:end] 05936 _x = self 05937 start = end 05938 end += 8 05939 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 05940 start = end 05941 end += 4 05942 (length,) = _struct_I.unpack(str[start:end]) 05943 start = end 05944 end += length 05945 self.action_goal.goal.target.region.image.encoding = str[start:end] 05946 _x = self 05947 start = end 05948 end += 5 05949 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 05950 start = end 05951 end += 4 05952 (length,) = _struct_I.unpack(str[start:end]) 05953 start = end 05954 end += length 05955 self.action_goal.goal.target.region.image.data = str[start:end] 05956 _x = self 05957 start = end 05958 end += 12 05959 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 05960 start = end 05961 end += 4 05962 (length,) = _struct_I.unpack(str[start:end]) 05963 start = end 05964 end += length 05965 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end] 05966 _x = self 05967 start = end 05968 end += 8 05969 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 05970 start = end 05971 end += 4 05972 (length,) = _struct_I.unpack(str[start:end]) 05973 start = end 05974 end += length 05975 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end] 05976 _x = self 05977 start = end 05978 end += 5 05979 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 05980 start = end 05981 end += 4 05982 (length,) = _struct_I.unpack(str[start:end]) 05983 start = end 05984 end += length 05985 self.action_goal.goal.target.region.disparity_image.data = str[start:end] 05986 _x = self 05987 start = end 05988 end += 12 05989 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 05990 start = end 05991 end += 4 05992 (length,) = _struct_I.unpack(str[start:end]) 05993 start = end 05994 end += length 05995 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end] 05996 _x = self 05997 start = end 05998 end += 8 05999 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 06000 start = end 06001 end += 4 06002 (length,) = _struct_I.unpack(str[start:end]) 06003 start = end 06004 end += length 06005 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end] 06006 start = end 06007 end += 4 06008 (length,) = _struct_I.unpack(str[start:end]) 06009 pattern = '<%sd'%length 06010 start = end 06011 end += struct.calcsize(pattern) 06012 self.action_goal.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06013 start = end 06014 end += 72 06015 self.action_goal.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 06016 start = end 06017 end += 72 06018 self.action_goal.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 06019 start = end 06020 end += 96 06021 self.action_goal.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 06022 _x = self 06023 start = end 06024 end += 37 06025 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 06026 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify) 06027 start = end 06028 end += 4 06029 (length,) = _struct_I.unpack(str[start:end]) 06030 start = end 06031 end += length 06032 self.action_goal.goal.target.region.roi_box_pose.header.frame_id = str[start:end] 06033 _x = self 06034 start = end 06035 end += 80 06036 (_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 06037 start = end 06038 end += 4 06039 (length,) = _struct_I.unpack(str[start:end]) 06040 start = end 06041 end += length 06042 self.action_goal.goal.target.collision_name = str[start:end] 06043 start = end 06044 end += 4 06045 (length,) = _struct_I.unpack(str[start:end]) 06046 self.action_goal.goal.desired_grasps = [] 06047 for i in range(0, length): 06048 val1 = object_manipulation_msgs.msg.Grasp() 06049 _v502 = val1.pre_grasp_posture 06050 _v503 = _v502.header 06051 start = end 06052 end += 4 06053 (_v503.seq,) = _struct_I.unpack(str[start:end]) 06054 _v504 = _v503.stamp 06055 _x = _v504 06056 start = end 06057 end += 8 06058 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06059 start = end 06060 end += 4 06061 (length,) = _struct_I.unpack(str[start:end]) 06062 start = end 06063 end += length 06064 _v503.frame_id = str[start:end] 06065 start = end 06066 end += 4 06067 (length,) = _struct_I.unpack(str[start:end]) 06068 _v502.name = [] 06069 for i in range(0, length): 06070 start = end 06071 end += 4 06072 (length,) = _struct_I.unpack(str[start:end]) 06073 start = end 06074 end += length 06075 val3 = str[start:end] 06076 _v502.name.append(val3) 06077 start = end 06078 end += 4 06079 (length,) = _struct_I.unpack(str[start:end]) 06080 pattern = '<%sd'%length 06081 start = end 06082 end += struct.calcsize(pattern) 06083 _v502.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06084 start = end 06085 end += 4 06086 (length,) = _struct_I.unpack(str[start:end]) 06087 pattern = '<%sd'%length 06088 start = end 06089 end += struct.calcsize(pattern) 06090 _v502.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06091 start = end 06092 end += 4 06093 (length,) = _struct_I.unpack(str[start:end]) 06094 pattern = '<%sd'%length 06095 start = end 06096 end += struct.calcsize(pattern) 06097 _v502.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06098 _v505 = val1.grasp_posture 06099 _v506 = _v505.header 06100 start = end 06101 end += 4 06102 (_v506.seq,) = _struct_I.unpack(str[start:end]) 06103 _v507 = _v506.stamp 06104 _x = _v507 06105 start = end 06106 end += 8 06107 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06108 start = end 06109 end += 4 06110 (length,) = _struct_I.unpack(str[start:end]) 06111 start = end 06112 end += length 06113 _v506.frame_id = str[start:end] 06114 start = end 06115 end += 4 06116 (length,) = _struct_I.unpack(str[start:end]) 06117 _v505.name = [] 06118 for i in range(0, length): 06119 start = end 06120 end += 4 06121 (length,) = _struct_I.unpack(str[start:end]) 06122 start = end 06123 end += length 06124 val3 = str[start:end] 06125 _v505.name.append(val3) 06126 start = end 06127 end += 4 06128 (length,) = _struct_I.unpack(str[start:end]) 06129 pattern = '<%sd'%length 06130 start = end 06131 end += struct.calcsize(pattern) 06132 _v505.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06133 start = end 06134 end += 4 06135 (length,) = _struct_I.unpack(str[start:end]) 06136 pattern = '<%sd'%length 06137 start = end 06138 end += struct.calcsize(pattern) 06139 _v505.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06140 start = end 06141 end += 4 06142 (length,) = _struct_I.unpack(str[start:end]) 06143 pattern = '<%sd'%length 06144 start = end 06145 end += struct.calcsize(pattern) 06146 _v505.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06147 _v508 = val1.grasp_pose 06148 _v509 = _v508.position 06149 _x = _v509 06150 start = end 06151 end += 24 06152 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06153 _v510 = _v508.orientation 06154 _x = _v510 06155 start = end 06156 end += 32 06157 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06158 _x = val1 06159 start = end 06160 end += 17 06161 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 06162 val1.cluster_rep = bool(val1.cluster_rep) 06163 start = end 06164 end += 4 06165 (length,) = _struct_I.unpack(str[start:end]) 06166 val1.moved_obstacles = [] 06167 for i in range(0, length): 06168 val2 = object_manipulation_msgs.msg.GraspableObject() 06169 start = end 06170 end += 4 06171 (length,) = _struct_I.unpack(str[start:end]) 06172 start = end 06173 end += length 06174 val2.reference_frame_id = str[start:end] 06175 start = end 06176 end += 4 06177 (length,) = _struct_I.unpack(str[start:end]) 06178 val2.potential_models = [] 06179 for i in range(0, length): 06180 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 06181 start = end 06182 end += 4 06183 (val3.model_id,) = _struct_i.unpack(str[start:end]) 06184 _v511 = val3.pose 06185 _v512 = _v511.header 06186 start = end 06187 end += 4 06188 (_v512.seq,) = _struct_I.unpack(str[start:end]) 06189 _v513 = _v512.stamp 06190 _x = _v513 06191 start = end 06192 end += 8 06193 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06194 start = end 06195 end += 4 06196 (length,) = _struct_I.unpack(str[start:end]) 06197 start = end 06198 end += length 06199 _v512.frame_id = str[start:end] 06200 _v514 = _v511.pose 06201 _v515 = _v514.position 06202 _x = _v515 06203 start = end 06204 end += 24 06205 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06206 _v516 = _v514.orientation 06207 _x = _v516 06208 start = end 06209 end += 32 06210 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06211 start = end 06212 end += 4 06213 (val3.confidence,) = _struct_f.unpack(str[start:end]) 06214 start = end 06215 end += 4 06216 (length,) = _struct_I.unpack(str[start:end]) 06217 start = end 06218 end += length 06219 val3.detector_name = str[start:end] 06220 val2.potential_models.append(val3) 06221 _v517 = val2.cluster 06222 _v518 = _v517.header 06223 start = end 06224 end += 4 06225 (_v518.seq,) = _struct_I.unpack(str[start:end]) 06226 _v519 = _v518.stamp 06227 _x = _v519 06228 start = end 06229 end += 8 06230 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06231 start = end 06232 end += 4 06233 (length,) = _struct_I.unpack(str[start:end]) 06234 start = end 06235 end += length 06236 _v518.frame_id = str[start:end] 06237 start = end 06238 end += 4 06239 (length,) = _struct_I.unpack(str[start:end]) 06240 _v517.points = [] 06241 for i in range(0, length): 06242 val4 = geometry_msgs.msg.Point32() 06243 _x = val4 06244 start = end 06245 end += 12 06246 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 06247 _v517.points.append(val4) 06248 start = end 06249 end += 4 06250 (length,) = _struct_I.unpack(str[start:end]) 06251 _v517.channels = [] 06252 for i in range(0, length): 06253 val4 = sensor_msgs.msg.ChannelFloat32() 06254 start = end 06255 end += 4 06256 (length,) = _struct_I.unpack(str[start:end]) 06257 start = end 06258 end += length 06259 val4.name = str[start:end] 06260 start = end 06261 end += 4 06262 (length,) = _struct_I.unpack(str[start:end]) 06263 pattern = '<%sf'%length 06264 start = end 06265 end += struct.calcsize(pattern) 06266 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 06267 _v517.channels.append(val4) 06268 _v520 = val2.region 06269 _v521 = _v520.cloud 06270 _v522 = _v521.header 06271 start = end 06272 end += 4 06273 (_v522.seq,) = _struct_I.unpack(str[start:end]) 06274 _v523 = _v522.stamp 06275 _x = _v523 06276 start = end 06277 end += 8 06278 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06279 start = end 06280 end += 4 06281 (length,) = _struct_I.unpack(str[start:end]) 06282 start = end 06283 end += length 06284 _v522.frame_id = str[start:end] 06285 _x = _v521 06286 start = end 06287 end += 8 06288 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06289 start = end 06290 end += 4 06291 (length,) = _struct_I.unpack(str[start:end]) 06292 _v521.fields = [] 06293 for i in range(0, length): 06294 val5 = sensor_msgs.msg.PointField() 06295 start = end 06296 end += 4 06297 (length,) = _struct_I.unpack(str[start:end]) 06298 start = end 06299 end += length 06300 val5.name = str[start:end] 06301 _x = val5 06302 start = end 06303 end += 9 06304 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 06305 _v521.fields.append(val5) 06306 _x = _v521 06307 start = end 06308 end += 9 06309 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 06310 _v521.is_bigendian = bool(_v521.is_bigendian) 06311 start = end 06312 end += 4 06313 (length,) = _struct_I.unpack(str[start:end]) 06314 start = end 06315 end += length 06316 _v521.data = str[start:end] 06317 start = end 06318 end += 1 06319 (_v521.is_dense,) = _struct_B.unpack(str[start:end]) 06320 _v521.is_dense = bool(_v521.is_dense) 06321 start = end 06322 end += 4 06323 (length,) = _struct_I.unpack(str[start:end]) 06324 pattern = '<%si'%length 06325 start = end 06326 end += struct.calcsize(pattern) 06327 _v520.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 06328 _v524 = _v520.image 06329 _v525 = _v524.header 06330 start = end 06331 end += 4 06332 (_v525.seq,) = _struct_I.unpack(str[start:end]) 06333 _v526 = _v525.stamp 06334 _x = _v526 06335 start = end 06336 end += 8 06337 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06338 start = end 06339 end += 4 06340 (length,) = _struct_I.unpack(str[start:end]) 06341 start = end 06342 end += length 06343 _v525.frame_id = str[start:end] 06344 _x = _v524 06345 start = end 06346 end += 8 06347 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06348 start = end 06349 end += 4 06350 (length,) = _struct_I.unpack(str[start:end]) 06351 start = end 06352 end += length 06353 _v524.encoding = str[start:end] 06354 _x = _v524 06355 start = end 06356 end += 5 06357 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 06358 start = end 06359 end += 4 06360 (length,) = _struct_I.unpack(str[start:end]) 06361 start = end 06362 end += length 06363 _v524.data = str[start:end] 06364 _v527 = _v520.disparity_image 06365 _v528 = _v527.header 06366 start = end 06367 end += 4 06368 (_v528.seq,) = _struct_I.unpack(str[start:end]) 06369 _v529 = _v528.stamp 06370 _x = _v529 06371 start = end 06372 end += 8 06373 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06374 start = end 06375 end += 4 06376 (length,) = _struct_I.unpack(str[start:end]) 06377 start = end 06378 end += length 06379 _v528.frame_id = str[start:end] 06380 _x = _v527 06381 start = end 06382 end += 8 06383 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06384 start = end 06385 end += 4 06386 (length,) = _struct_I.unpack(str[start:end]) 06387 start = end 06388 end += length 06389 _v527.encoding = str[start:end] 06390 _x = _v527 06391 start = end 06392 end += 5 06393 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 06394 start = end 06395 end += 4 06396 (length,) = _struct_I.unpack(str[start:end]) 06397 start = end 06398 end += length 06399 _v527.data = str[start:end] 06400 _v530 = _v520.cam_info 06401 _v531 = _v530.header 06402 start = end 06403 end += 4 06404 (_v531.seq,) = _struct_I.unpack(str[start:end]) 06405 _v532 = _v531.stamp 06406 _x = _v532 06407 start = end 06408 end += 8 06409 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06410 start = end 06411 end += 4 06412 (length,) = _struct_I.unpack(str[start:end]) 06413 start = end 06414 end += length 06415 _v531.frame_id = str[start:end] 06416 _x = _v530 06417 start = end 06418 end += 8 06419 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06420 start = end 06421 end += 4 06422 (length,) = _struct_I.unpack(str[start:end]) 06423 start = end 06424 end += length 06425 _v530.distortion_model = str[start:end] 06426 start = end 06427 end += 4 06428 (length,) = _struct_I.unpack(str[start:end]) 06429 pattern = '<%sd'%length 06430 start = end 06431 end += struct.calcsize(pattern) 06432 _v530.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06433 start = end 06434 end += 72 06435 _v530.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 06436 start = end 06437 end += 72 06438 _v530.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 06439 start = end 06440 end += 96 06441 _v530.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 06442 _x = _v530 06443 start = end 06444 end += 8 06445 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 06446 _v533 = _v530.roi 06447 _x = _v533 06448 start = end 06449 end += 17 06450 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 06451 _v533.do_rectify = bool(_v533.do_rectify) 06452 _v534 = _v520.roi_box_pose 06453 _v535 = _v534.header 06454 start = end 06455 end += 4 06456 (_v535.seq,) = _struct_I.unpack(str[start:end]) 06457 _v536 = _v535.stamp 06458 _x = _v536 06459 start = end 06460 end += 8 06461 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06462 start = end 06463 end += 4 06464 (length,) = _struct_I.unpack(str[start:end]) 06465 start = end 06466 end += length 06467 _v535.frame_id = str[start:end] 06468 _v537 = _v534.pose 06469 _v538 = _v537.position 06470 _x = _v538 06471 start = end 06472 end += 24 06473 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06474 _v539 = _v537.orientation 06475 _x = _v539 06476 start = end 06477 end += 32 06478 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06479 _v540 = _v520.roi_box_dims 06480 _x = _v540 06481 start = end 06482 end += 24 06483 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06484 start = end 06485 end += 4 06486 (length,) = _struct_I.unpack(str[start:end]) 06487 start = end 06488 end += length 06489 val2.collision_name = str[start:end] 06490 val1.moved_obstacles.append(val2) 06491 self.action_goal.goal.desired_grasps.append(val1) 06492 _x = self 06493 start = end 06494 end += 12 06495 (_x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 06496 start = end 06497 end += 4 06498 (length,) = _struct_I.unpack(str[start:end]) 06499 start = end 06500 end += length 06501 self.action_goal.goal.lift.direction.header.frame_id = str[start:end] 06502 _x = self 06503 start = end 06504 end += 32 06505 (_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance,) = _struct_3d2f.unpack(str[start:end]) 06506 start = end 06507 end += 4 06508 (length,) = _struct_I.unpack(str[start:end]) 06509 start = end 06510 end += length 06511 self.action_goal.goal.collision_object_name = str[start:end] 06512 start = end 06513 end += 4 06514 (length,) = _struct_I.unpack(str[start:end]) 06515 start = end 06516 end += length 06517 self.action_goal.goal.collision_support_surface_name = str[start:end] 06518 _x = self 06519 start = end 06520 end += 5 06521 (_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_execution, _x.action_goal.goal.use_reactive_lift, _x.action_goal.goal.only_perform_feasibility_test, _x.action_goal.goal.ignore_collisions,) = _struct_5B.unpack(str[start:end]) 06522 self.action_goal.goal.allow_gripper_support_collision = bool(self.action_goal.goal.allow_gripper_support_collision) 06523 self.action_goal.goal.use_reactive_execution = bool(self.action_goal.goal.use_reactive_execution) 06524 self.action_goal.goal.use_reactive_lift = bool(self.action_goal.goal.use_reactive_lift) 06525 self.action_goal.goal.only_perform_feasibility_test = bool(self.action_goal.goal.only_perform_feasibility_test) 06526 self.action_goal.goal.ignore_collisions = bool(self.action_goal.goal.ignore_collisions) 06527 start = end 06528 end += 4 06529 (length,) = _struct_I.unpack(str[start:end]) 06530 self.action_goal.goal.path_constraints.joint_constraints = [] 06531 for i in range(0, length): 06532 val1 = arm_navigation_msgs.msg.JointConstraint() 06533 start = end 06534 end += 4 06535 (length,) = _struct_I.unpack(str[start:end]) 06536 start = end 06537 end += length 06538 val1.joint_name = str[start:end] 06539 _x = val1 06540 start = end 06541 end += 32 06542 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 06543 self.action_goal.goal.path_constraints.joint_constraints.append(val1) 06544 start = end 06545 end += 4 06546 (length,) = _struct_I.unpack(str[start:end]) 06547 self.action_goal.goal.path_constraints.position_constraints = [] 06548 for i in range(0, length): 06549 val1 = arm_navigation_msgs.msg.PositionConstraint() 06550 _v541 = val1.header 06551 start = end 06552 end += 4 06553 (_v541.seq,) = _struct_I.unpack(str[start:end]) 06554 _v542 = _v541.stamp 06555 _x = _v542 06556 start = end 06557 end += 8 06558 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06559 start = end 06560 end += 4 06561 (length,) = _struct_I.unpack(str[start:end]) 06562 start = end 06563 end += length 06564 _v541.frame_id = str[start:end] 06565 start = end 06566 end += 4 06567 (length,) = _struct_I.unpack(str[start:end]) 06568 start = end 06569 end += length 06570 val1.link_name = str[start:end] 06571 _v543 = val1.target_point_offset 06572 _x = _v543 06573 start = end 06574 end += 24 06575 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06576 _v544 = val1.position 06577 _x = _v544 06578 start = end 06579 end += 24 06580 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06581 _v545 = val1.constraint_region_shape 06582 start = end 06583 end += 1 06584 (_v545.type,) = _struct_b.unpack(str[start:end]) 06585 start = end 06586 end += 4 06587 (length,) = _struct_I.unpack(str[start:end]) 06588 pattern = '<%sd'%length 06589 start = end 06590 end += struct.calcsize(pattern) 06591 _v545.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 06592 start = end 06593 end += 4 06594 (length,) = _struct_I.unpack(str[start:end]) 06595 pattern = '<%si'%length 06596 start = end 06597 end += struct.calcsize(pattern) 06598 _v545.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 06599 start = end 06600 end += 4 06601 (length,) = _struct_I.unpack(str[start:end]) 06602 _v545.vertices = [] 06603 for i in range(0, length): 06604 val3 = geometry_msgs.msg.Point() 06605 _x = val3 06606 start = end 06607 end += 24 06608 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06609 _v545.vertices.append(val3) 06610 _v546 = val1.constraint_region_orientation 06611 _x = _v546 06612 start = end 06613 end += 32 06614 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06615 start = end 06616 end += 8 06617 (val1.weight,) = _struct_d.unpack(str[start:end]) 06618 self.action_goal.goal.path_constraints.position_constraints.append(val1) 06619 start = end 06620 end += 4 06621 (length,) = _struct_I.unpack(str[start:end]) 06622 self.action_goal.goal.path_constraints.orientation_constraints = [] 06623 for i in range(0, length): 06624 val1 = arm_navigation_msgs.msg.OrientationConstraint() 06625 _v547 = val1.header 06626 start = end 06627 end += 4 06628 (_v547.seq,) = _struct_I.unpack(str[start:end]) 06629 _v548 = _v547.stamp 06630 _x = _v548 06631 start = end 06632 end += 8 06633 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06634 start = end 06635 end += 4 06636 (length,) = _struct_I.unpack(str[start:end]) 06637 start = end 06638 end += length 06639 _v547.frame_id = str[start:end] 06640 start = end 06641 end += 4 06642 (length,) = _struct_I.unpack(str[start:end]) 06643 start = end 06644 end += length 06645 val1.link_name = str[start:end] 06646 start = end 06647 end += 4 06648 (val1.type,) = _struct_i.unpack(str[start:end]) 06649 _v549 = val1.orientation 06650 _x = _v549 06651 start = end 06652 end += 32 06653 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06654 _x = val1 06655 start = end 06656 end += 32 06657 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 06658 self.action_goal.goal.path_constraints.orientation_constraints.append(val1) 06659 start = end 06660 end += 4 06661 (length,) = _struct_I.unpack(str[start:end]) 06662 self.action_goal.goal.path_constraints.visibility_constraints = [] 06663 for i in range(0, length): 06664 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 06665 _v550 = val1.header 06666 start = end 06667 end += 4 06668 (_v550.seq,) = _struct_I.unpack(str[start:end]) 06669 _v551 = _v550.stamp 06670 _x = _v551 06671 start = end 06672 end += 8 06673 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06674 start = end 06675 end += 4 06676 (length,) = _struct_I.unpack(str[start:end]) 06677 start = end 06678 end += length 06679 _v550.frame_id = str[start:end] 06680 _v552 = val1.target 06681 _v553 = _v552.header 06682 start = end 06683 end += 4 06684 (_v553.seq,) = _struct_I.unpack(str[start:end]) 06685 _v554 = _v553.stamp 06686 _x = _v554 06687 start = end 06688 end += 8 06689 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06690 start = end 06691 end += 4 06692 (length,) = _struct_I.unpack(str[start:end]) 06693 start = end 06694 end += length 06695 _v553.frame_id = str[start:end] 06696 _v555 = _v552.point 06697 _x = _v555 06698 start = end 06699 end += 24 06700 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06701 _v556 = val1.sensor_pose 06702 _v557 = _v556.header 06703 start = end 06704 end += 4 06705 (_v557.seq,) = _struct_I.unpack(str[start:end]) 06706 _v558 = _v557.stamp 06707 _x = _v558 06708 start = end 06709 end += 8 06710 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06711 start = end 06712 end += 4 06713 (length,) = _struct_I.unpack(str[start:end]) 06714 start = end 06715 end += length 06716 _v557.frame_id = str[start:end] 06717 _v559 = _v556.pose 06718 _v560 = _v559.position 06719 _x = _v560 06720 start = end 06721 end += 24 06722 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06723 _v561 = _v559.orientation 06724 _x = _v561 06725 start = end 06726 end += 32 06727 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06728 start = end 06729 end += 8 06730 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 06731 self.action_goal.goal.path_constraints.visibility_constraints.append(val1) 06732 start = end 06733 end += 4 06734 (length,) = _struct_I.unpack(str[start:end]) 06735 self.action_goal.goal.additional_collision_operations.collision_operations = [] 06736 for i in range(0, length): 06737 val1 = arm_navigation_msgs.msg.CollisionOperation() 06738 start = end 06739 end += 4 06740 (length,) = _struct_I.unpack(str[start:end]) 06741 start = end 06742 end += length 06743 val1.object1 = str[start:end] 06744 start = end 06745 end += 4 06746 (length,) = _struct_I.unpack(str[start:end]) 06747 start = end 06748 end += length 06749 val1.object2 = str[start:end] 06750 _x = val1 06751 start = end 06752 end += 12 06753 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 06754 self.action_goal.goal.additional_collision_operations.collision_operations.append(val1) 06755 start = end 06756 end += 4 06757 (length,) = _struct_I.unpack(str[start:end]) 06758 self.action_goal.goal.additional_link_padding = [] 06759 for i in range(0, length): 06760 val1 = arm_navigation_msgs.msg.LinkPadding() 06761 start = end 06762 end += 4 06763 (length,) = _struct_I.unpack(str[start:end]) 06764 start = end 06765 end += length 06766 val1.link_name = str[start:end] 06767 start = end 06768 end += 8 06769 (val1.padding,) = _struct_d.unpack(str[start:end]) 06770 self.action_goal.goal.additional_link_padding.append(val1) 06771 start = end 06772 end += 4 06773 (length,) = _struct_I.unpack(str[start:end]) 06774 self.action_goal.goal.movable_obstacles = [] 06775 for i in range(0, length): 06776 val1 = object_manipulation_msgs.msg.GraspableObject() 06777 start = end 06778 end += 4 06779 (length,) = _struct_I.unpack(str[start:end]) 06780 start = end 06781 end += length 06782 val1.reference_frame_id = str[start:end] 06783 start = end 06784 end += 4 06785 (length,) = _struct_I.unpack(str[start:end]) 06786 val1.potential_models = [] 06787 for i in range(0, length): 06788 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 06789 start = end 06790 end += 4 06791 (val2.model_id,) = _struct_i.unpack(str[start:end]) 06792 _v562 = val2.pose 06793 _v563 = _v562.header 06794 start = end 06795 end += 4 06796 (_v563.seq,) = _struct_I.unpack(str[start:end]) 06797 _v564 = _v563.stamp 06798 _x = _v564 06799 start = end 06800 end += 8 06801 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06802 start = end 06803 end += 4 06804 (length,) = _struct_I.unpack(str[start:end]) 06805 start = end 06806 end += length 06807 _v563.frame_id = str[start:end] 06808 _v565 = _v562.pose 06809 _v566 = _v565.position 06810 _x = _v566 06811 start = end 06812 end += 24 06813 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 06814 _v567 = _v565.orientation 06815 _x = _v567 06816 start = end 06817 end += 32 06818 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 06819 start = end 06820 end += 4 06821 (val2.confidence,) = _struct_f.unpack(str[start:end]) 06822 start = end 06823 end += 4 06824 (length,) = _struct_I.unpack(str[start:end]) 06825 start = end 06826 end += length 06827 val2.detector_name = str[start:end] 06828 val1.potential_models.append(val2) 06829 _v568 = val1.cluster 06830 _v569 = _v568.header 06831 start = end 06832 end += 4 06833 (_v569.seq,) = _struct_I.unpack(str[start:end]) 06834 _v570 = _v569.stamp 06835 _x = _v570 06836 start = end 06837 end += 8 06838 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06839 start = end 06840 end += 4 06841 (length,) = _struct_I.unpack(str[start:end]) 06842 start = end 06843 end += length 06844 _v569.frame_id = str[start:end] 06845 start = end 06846 end += 4 06847 (length,) = _struct_I.unpack(str[start:end]) 06848 _v568.points = [] 06849 for i in range(0, length): 06850 val3 = geometry_msgs.msg.Point32() 06851 _x = val3 06852 start = end 06853 end += 12 06854 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 06855 _v568.points.append(val3) 06856 start = end 06857 end += 4 06858 (length,) = _struct_I.unpack(str[start:end]) 06859 _v568.channels = [] 06860 for i in range(0, length): 06861 val3 = sensor_msgs.msg.ChannelFloat32() 06862 start = end 06863 end += 4 06864 (length,) = _struct_I.unpack(str[start:end]) 06865 start = end 06866 end += length 06867 val3.name = str[start:end] 06868 start = end 06869 end += 4 06870 (length,) = _struct_I.unpack(str[start:end]) 06871 pattern = '<%sf'%length 06872 start = end 06873 end += struct.calcsize(pattern) 06874 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 06875 _v568.channels.append(val3) 06876 _v571 = val1.region 06877 _v572 = _v571.cloud 06878 _v573 = _v572.header 06879 start = end 06880 end += 4 06881 (_v573.seq,) = _struct_I.unpack(str[start:end]) 06882 _v574 = _v573.stamp 06883 _x = _v574 06884 start = end 06885 end += 8 06886 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06887 start = end 06888 end += 4 06889 (length,) = _struct_I.unpack(str[start:end]) 06890 start = end 06891 end += length 06892 _v573.frame_id = str[start:end] 06893 _x = _v572 06894 start = end 06895 end += 8 06896 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06897 start = end 06898 end += 4 06899 (length,) = _struct_I.unpack(str[start:end]) 06900 _v572.fields = [] 06901 for i in range(0, length): 06902 val4 = sensor_msgs.msg.PointField() 06903 start = end 06904 end += 4 06905 (length,) = _struct_I.unpack(str[start:end]) 06906 start = end 06907 end += length 06908 val4.name = str[start:end] 06909 _x = val4 06910 start = end 06911 end += 9 06912 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 06913 _v572.fields.append(val4) 06914 _x = _v572 06915 start = end 06916 end += 9 06917 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 06918 _v572.is_bigendian = bool(_v572.is_bigendian) 06919 start = end 06920 end += 4 06921 (length,) = _struct_I.unpack(str[start:end]) 06922 start = end 06923 end += length 06924 _v572.data = str[start:end] 06925 start = end 06926 end += 1 06927 (_v572.is_dense,) = _struct_B.unpack(str[start:end]) 06928 _v572.is_dense = bool(_v572.is_dense) 06929 start = end 06930 end += 4 06931 (length,) = _struct_I.unpack(str[start:end]) 06932 pattern = '<%si'%length 06933 start = end 06934 end += struct.calcsize(pattern) 06935 _v571.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 06936 _v575 = _v571.image 06937 _v576 = _v575.header 06938 start = end 06939 end += 4 06940 (_v576.seq,) = _struct_I.unpack(str[start:end]) 06941 _v577 = _v576.stamp 06942 _x = _v577 06943 start = end 06944 end += 8 06945 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06946 start = end 06947 end += 4 06948 (length,) = _struct_I.unpack(str[start:end]) 06949 start = end 06950 end += length 06951 _v576.frame_id = str[start:end] 06952 _x = _v575 06953 start = end 06954 end += 8 06955 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06956 start = end 06957 end += 4 06958 (length,) = _struct_I.unpack(str[start:end]) 06959 start = end 06960 end += length 06961 _v575.encoding = str[start:end] 06962 _x = _v575 06963 start = end 06964 end += 5 06965 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 06966 start = end 06967 end += 4 06968 (length,) = _struct_I.unpack(str[start:end]) 06969 start = end 06970 end += length 06971 _v575.data = str[start:end] 06972 _v578 = _v571.disparity_image 06973 _v579 = _v578.header 06974 start = end 06975 end += 4 06976 (_v579.seq,) = _struct_I.unpack(str[start:end]) 06977 _v580 = _v579.stamp 06978 _x = _v580 06979 start = end 06980 end += 8 06981 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 06982 start = end 06983 end += 4 06984 (length,) = _struct_I.unpack(str[start:end]) 06985 start = end 06986 end += length 06987 _v579.frame_id = str[start:end] 06988 _x = _v578 06989 start = end 06990 end += 8 06991 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 06992 start = end 06993 end += 4 06994 (length,) = _struct_I.unpack(str[start:end]) 06995 start = end 06996 end += length 06997 _v578.encoding = str[start:end] 06998 _x = _v578 06999 start = end 07000 end += 5 07001 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 07002 start = end 07003 end += 4 07004 (length,) = _struct_I.unpack(str[start:end]) 07005 start = end 07006 end += length 07007 _v578.data = str[start:end] 07008 _v581 = _v571.cam_info 07009 _v582 = _v581.header 07010 start = end 07011 end += 4 07012 (_v582.seq,) = _struct_I.unpack(str[start:end]) 07013 _v583 = _v582.stamp 07014 _x = _v583 07015 start = end 07016 end += 8 07017 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07018 start = end 07019 end += 4 07020 (length,) = _struct_I.unpack(str[start:end]) 07021 start = end 07022 end += length 07023 _v582.frame_id = str[start:end] 07024 _x = _v581 07025 start = end 07026 end += 8 07027 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07028 start = end 07029 end += 4 07030 (length,) = _struct_I.unpack(str[start:end]) 07031 start = end 07032 end += length 07033 _v581.distortion_model = str[start:end] 07034 start = end 07035 end += 4 07036 (length,) = _struct_I.unpack(str[start:end]) 07037 pattern = '<%sd'%length 07038 start = end 07039 end += struct.calcsize(pattern) 07040 _v581.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07041 start = end 07042 end += 72 07043 _v581.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 07044 start = end 07045 end += 72 07046 _v581.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 07047 start = end 07048 end += 96 07049 _v581.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 07050 _x = _v581 07051 start = end 07052 end += 8 07053 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 07054 _v584 = _v581.roi 07055 _x = _v584 07056 start = end 07057 end += 17 07058 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 07059 _v584.do_rectify = bool(_v584.do_rectify) 07060 _v585 = _v571.roi_box_pose 07061 _v586 = _v585.header 07062 start = end 07063 end += 4 07064 (_v586.seq,) = _struct_I.unpack(str[start:end]) 07065 _v587 = _v586.stamp 07066 _x = _v587 07067 start = end 07068 end += 8 07069 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07070 start = end 07071 end += 4 07072 (length,) = _struct_I.unpack(str[start:end]) 07073 start = end 07074 end += length 07075 _v586.frame_id = str[start:end] 07076 _v588 = _v585.pose 07077 _v589 = _v588.position 07078 _x = _v589 07079 start = end 07080 end += 24 07081 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07082 _v590 = _v588.orientation 07083 _x = _v590 07084 start = end 07085 end += 32 07086 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 07087 _v591 = _v571.roi_box_dims 07088 _x = _v591 07089 start = end 07090 end += 24 07091 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07092 start = end 07093 end += 4 07094 (length,) = _struct_I.unpack(str[start:end]) 07095 start = end 07096 end += length 07097 val1.collision_name = str[start:end] 07098 self.action_goal.goal.movable_obstacles.append(val1) 07099 _x = self 07100 start = end 07101 end += 16 07102 (_x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_f3I.unpack(str[start:end]) 07103 start = end 07104 end += 4 07105 (length,) = _struct_I.unpack(str[start:end]) 07106 start = end 07107 end += length 07108 self.action_result.header.frame_id = str[start:end] 07109 _x = self 07110 start = end 07111 end += 8 07112 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 07113 start = end 07114 end += 4 07115 (length,) = _struct_I.unpack(str[start:end]) 07116 start = end 07117 end += length 07118 self.action_result.status.goal_id.id = str[start:end] 07119 start = end 07120 end += 1 07121 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 07122 start = end 07123 end += 4 07124 (length,) = _struct_I.unpack(str[start:end]) 07125 start = end 07126 end += length 07127 self.action_result.status.text = str[start:end] 07128 _x = self 07129 start = end 07130 end += 16 07131 (_x.action_result.result.manipulation_result.value, _x.action_result.result.grasp.pre_grasp_posture.header.seq, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.secs, _x.action_result.result.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end]) 07132 start = end 07133 end += 4 07134 (length,) = _struct_I.unpack(str[start:end]) 07135 start = end 07136 end += length 07137 self.action_result.result.grasp.pre_grasp_posture.header.frame_id = str[start:end] 07138 start = end 07139 end += 4 07140 (length,) = _struct_I.unpack(str[start:end]) 07141 self.action_result.result.grasp.pre_grasp_posture.name = [] 07142 for i in range(0, length): 07143 start = end 07144 end += 4 07145 (length,) = _struct_I.unpack(str[start:end]) 07146 start = end 07147 end += length 07148 val1 = str[start:end] 07149 self.action_result.result.grasp.pre_grasp_posture.name.append(val1) 07150 start = end 07151 end += 4 07152 (length,) = _struct_I.unpack(str[start:end]) 07153 pattern = '<%sd'%length 07154 start = end 07155 end += struct.calcsize(pattern) 07156 self.action_result.result.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07157 start = end 07158 end += 4 07159 (length,) = _struct_I.unpack(str[start:end]) 07160 pattern = '<%sd'%length 07161 start = end 07162 end += struct.calcsize(pattern) 07163 self.action_result.result.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07164 start = end 07165 end += 4 07166 (length,) = _struct_I.unpack(str[start:end]) 07167 pattern = '<%sd'%length 07168 start = end 07169 end += struct.calcsize(pattern) 07170 self.action_result.result.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07171 _x = self 07172 start = end 07173 end += 12 07174 (_x.action_result.result.grasp.grasp_posture.header.seq, _x.action_result.result.grasp.grasp_posture.header.stamp.secs, _x.action_result.result.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 07175 start = end 07176 end += 4 07177 (length,) = _struct_I.unpack(str[start:end]) 07178 start = end 07179 end += length 07180 self.action_result.result.grasp.grasp_posture.header.frame_id = str[start:end] 07181 start = end 07182 end += 4 07183 (length,) = _struct_I.unpack(str[start:end]) 07184 self.action_result.result.grasp.grasp_posture.name = [] 07185 for i in range(0, length): 07186 start = end 07187 end += 4 07188 (length,) = _struct_I.unpack(str[start:end]) 07189 start = end 07190 end += length 07191 val1 = str[start:end] 07192 self.action_result.result.grasp.grasp_posture.name.append(val1) 07193 start = end 07194 end += 4 07195 (length,) = _struct_I.unpack(str[start:end]) 07196 pattern = '<%sd'%length 07197 start = end 07198 end += struct.calcsize(pattern) 07199 self.action_result.result.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07200 start = end 07201 end += 4 07202 (length,) = _struct_I.unpack(str[start:end]) 07203 pattern = '<%sd'%length 07204 start = end 07205 end += struct.calcsize(pattern) 07206 self.action_result.result.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07207 start = end 07208 end += 4 07209 (length,) = _struct_I.unpack(str[start:end]) 07210 pattern = '<%sd'%length 07211 start = end 07212 end += struct.calcsize(pattern) 07213 self.action_result.result.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07214 _x = self 07215 start = end 07216 end += 73 07217 (_x.action_result.result.grasp.grasp_pose.position.x, _x.action_result.result.grasp.grasp_pose.position.y, _x.action_result.result.grasp.grasp_pose.position.z, _x.action_result.result.grasp.grasp_pose.orientation.x, _x.action_result.result.grasp.grasp_pose.orientation.y, _x.action_result.result.grasp.grasp_pose.orientation.z, _x.action_result.result.grasp.grasp_pose.orientation.w, _x.action_result.result.grasp.success_probability, _x.action_result.result.grasp.cluster_rep, _x.action_result.result.grasp.desired_approach_distance, _x.action_result.result.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end]) 07218 self.action_result.result.grasp.cluster_rep = bool(self.action_result.result.grasp.cluster_rep) 07219 start = end 07220 end += 4 07221 (length,) = _struct_I.unpack(str[start:end]) 07222 self.action_result.result.grasp.moved_obstacles = [] 07223 for i in range(0, length): 07224 val1 = object_manipulation_msgs.msg.GraspableObject() 07225 start = end 07226 end += 4 07227 (length,) = _struct_I.unpack(str[start:end]) 07228 start = end 07229 end += length 07230 val1.reference_frame_id = str[start:end] 07231 start = end 07232 end += 4 07233 (length,) = _struct_I.unpack(str[start:end]) 07234 val1.potential_models = [] 07235 for i in range(0, length): 07236 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 07237 start = end 07238 end += 4 07239 (val2.model_id,) = _struct_i.unpack(str[start:end]) 07240 _v592 = val2.pose 07241 _v593 = _v592.header 07242 start = end 07243 end += 4 07244 (_v593.seq,) = _struct_I.unpack(str[start:end]) 07245 _v594 = _v593.stamp 07246 _x = _v594 07247 start = end 07248 end += 8 07249 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07250 start = end 07251 end += 4 07252 (length,) = _struct_I.unpack(str[start:end]) 07253 start = end 07254 end += length 07255 _v593.frame_id = str[start:end] 07256 _v595 = _v592.pose 07257 _v596 = _v595.position 07258 _x = _v596 07259 start = end 07260 end += 24 07261 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07262 _v597 = _v595.orientation 07263 _x = _v597 07264 start = end 07265 end += 32 07266 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 07267 start = end 07268 end += 4 07269 (val2.confidence,) = _struct_f.unpack(str[start:end]) 07270 start = end 07271 end += 4 07272 (length,) = _struct_I.unpack(str[start:end]) 07273 start = end 07274 end += length 07275 val2.detector_name = str[start:end] 07276 val1.potential_models.append(val2) 07277 _v598 = val1.cluster 07278 _v599 = _v598.header 07279 start = end 07280 end += 4 07281 (_v599.seq,) = _struct_I.unpack(str[start:end]) 07282 _v600 = _v599.stamp 07283 _x = _v600 07284 start = end 07285 end += 8 07286 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07287 start = end 07288 end += 4 07289 (length,) = _struct_I.unpack(str[start:end]) 07290 start = end 07291 end += length 07292 _v599.frame_id = str[start:end] 07293 start = end 07294 end += 4 07295 (length,) = _struct_I.unpack(str[start:end]) 07296 _v598.points = [] 07297 for i in range(0, length): 07298 val3 = geometry_msgs.msg.Point32() 07299 _x = val3 07300 start = end 07301 end += 12 07302 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 07303 _v598.points.append(val3) 07304 start = end 07305 end += 4 07306 (length,) = _struct_I.unpack(str[start:end]) 07307 _v598.channels = [] 07308 for i in range(0, length): 07309 val3 = sensor_msgs.msg.ChannelFloat32() 07310 start = end 07311 end += 4 07312 (length,) = _struct_I.unpack(str[start:end]) 07313 start = end 07314 end += length 07315 val3.name = str[start:end] 07316 start = end 07317 end += 4 07318 (length,) = _struct_I.unpack(str[start:end]) 07319 pattern = '<%sf'%length 07320 start = end 07321 end += struct.calcsize(pattern) 07322 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 07323 _v598.channels.append(val3) 07324 _v601 = val1.region 07325 _v602 = _v601.cloud 07326 _v603 = _v602.header 07327 start = end 07328 end += 4 07329 (_v603.seq,) = _struct_I.unpack(str[start:end]) 07330 _v604 = _v603.stamp 07331 _x = _v604 07332 start = end 07333 end += 8 07334 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07335 start = end 07336 end += 4 07337 (length,) = _struct_I.unpack(str[start:end]) 07338 start = end 07339 end += length 07340 _v603.frame_id = str[start:end] 07341 _x = _v602 07342 start = end 07343 end += 8 07344 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07345 start = end 07346 end += 4 07347 (length,) = _struct_I.unpack(str[start:end]) 07348 _v602.fields = [] 07349 for i in range(0, length): 07350 val4 = sensor_msgs.msg.PointField() 07351 start = end 07352 end += 4 07353 (length,) = _struct_I.unpack(str[start:end]) 07354 start = end 07355 end += length 07356 val4.name = str[start:end] 07357 _x = val4 07358 start = end 07359 end += 9 07360 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 07361 _v602.fields.append(val4) 07362 _x = _v602 07363 start = end 07364 end += 9 07365 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 07366 _v602.is_bigendian = bool(_v602.is_bigendian) 07367 start = end 07368 end += 4 07369 (length,) = _struct_I.unpack(str[start:end]) 07370 start = end 07371 end += length 07372 _v602.data = str[start:end] 07373 start = end 07374 end += 1 07375 (_v602.is_dense,) = _struct_B.unpack(str[start:end]) 07376 _v602.is_dense = bool(_v602.is_dense) 07377 start = end 07378 end += 4 07379 (length,) = _struct_I.unpack(str[start:end]) 07380 pattern = '<%si'%length 07381 start = end 07382 end += struct.calcsize(pattern) 07383 _v601.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 07384 _v605 = _v601.image 07385 _v606 = _v605.header 07386 start = end 07387 end += 4 07388 (_v606.seq,) = _struct_I.unpack(str[start:end]) 07389 _v607 = _v606.stamp 07390 _x = _v607 07391 start = end 07392 end += 8 07393 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07394 start = end 07395 end += 4 07396 (length,) = _struct_I.unpack(str[start:end]) 07397 start = end 07398 end += length 07399 _v606.frame_id = str[start:end] 07400 _x = _v605 07401 start = end 07402 end += 8 07403 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07404 start = end 07405 end += 4 07406 (length,) = _struct_I.unpack(str[start:end]) 07407 start = end 07408 end += length 07409 _v605.encoding = str[start:end] 07410 _x = _v605 07411 start = end 07412 end += 5 07413 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 07414 start = end 07415 end += 4 07416 (length,) = _struct_I.unpack(str[start:end]) 07417 start = end 07418 end += length 07419 _v605.data = str[start:end] 07420 _v608 = _v601.disparity_image 07421 _v609 = _v608.header 07422 start = end 07423 end += 4 07424 (_v609.seq,) = _struct_I.unpack(str[start:end]) 07425 _v610 = _v609.stamp 07426 _x = _v610 07427 start = end 07428 end += 8 07429 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07430 start = end 07431 end += 4 07432 (length,) = _struct_I.unpack(str[start:end]) 07433 start = end 07434 end += length 07435 _v609.frame_id = str[start:end] 07436 _x = _v608 07437 start = end 07438 end += 8 07439 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07440 start = end 07441 end += 4 07442 (length,) = _struct_I.unpack(str[start:end]) 07443 start = end 07444 end += length 07445 _v608.encoding = str[start:end] 07446 _x = _v608 07447 start = end 07448 end += 5 07449 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 07450 start = end 07451 end += 4 07452 (length,) = _struct_I.unpack(str[start:end]) 07453 start = end 07454 end += length 07455 _v608.data = str[start:end] 07456 _v611 = _v601.cam_info 07457 _v612 = _v611.header 07458 start = end 07459 end += 4 07460 (_v612.seq,) = _struct_I.unpack(str[start:end]) 07461 _v613 = _v612.stamp 07462 _x = _v613 07463 start = end 07464 end += 8 07465 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07466 start = end 07467 end += 4 07468 (length,) = _struct_I.unpack(str[start:end]) 07469 start = end 07470 end += length 07471 _v612.frame_id = str[start:end] 07472 _x = _v611 07473 start = end 07474 end += 8 07475 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07476 start = end 07477 end += 4 07478 (length,) = _struct_I.unpack(str[start:end]) 07479 start = end 07480 end += length 07481 _v611.distortion_model = str[start:end] 07482 start = end 07483 end += 4 07484 (length,) = _struct_I.unpack(str[start:end]) 07485 pattern = '<%sd'%length 07486 start = end 07487 end += struct.calcsize(pattern) 07488 _v611.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07489 start = end 07490 end += 72 07491 _v611.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 07492 start = end 07493 end += 72 07494 _v611.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 07495 start = end 07496 end += 96 07497 _v611.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 07498 _x = _v611 07499 start = end 07500 end += 8 07501 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 07502 _v614 = _v611.roi 07503 _x = _v614 07504 start = end 07505 end += 17 07506 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 07507 _v614.do_rectify = bool(_v614.do_rectify) 07508 _v615 = _v601.roi_box_pose 07509 _v616 = _v615.header 07510 start = end 07511 end += 4 07512 (_v616.seq,) = _struct_I.unpack(str[start:end]) 07513 _v617 = _v616.stamp 07514 _x = _v617 07515 start = end 07516 end += 8 07517 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07518 start = end 07519 end += 4 07520 (length,) = _struct_I.unpack(str[start:end]) 07521 start = end 07522 end += length 07523 _v616.frame_id = str[start:end] 07524 _v618 = _v615.pose 07525 _v619 = _v618.position 07526 _x = _v619 07527 start = end 07528 end += 24 07529 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07530 _v620 = _v618.orientation 07531 _x = _v620 07532 start = end 07533 end += 32 07534 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 07535 _v621 = _v601.roi_box_dims 07536 _x = _v621 07537 start = end 07538 end += 24 07539 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07540 start = end 07541 end += 4 07542 (length,) = _struct_I.unpack(str[start:end]) 07543 start = end 07544 end += length 07545 val1.collision_name = str[start:end] 07546 self.action_result.result.grasp.moved_obstacles.append(val1) 07547 start = end 07548 end += 4 07549 (length,) = _struct_I.unpack(str[start:end]) 07550 self.action_result.result.attempted_grasps = [] 07551 for i in range(0, length): 07552 val1 = object_manipulation_msgs.msg.Grasp() 07553 _v622 = val1.pre_grasp_posture 07554 _v623 = _v622.header 07555 start = end 07556 end += 4 07557 (_v623.seq,) = _struct_I.unpack(str[start:end]) 07558 _v624 = _v623.stamp 07559 _x = _v624 07560 start = end 07561 end += 8 07562 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07563 start = end 07564 end += 4 07565 (length,) = _struct_I.unpack(str[start:end]) 07566 start = end 07567 end += length 07568 _v623.frame_id = str[start:end] 07569 start = end 07570 end += 4 07571 (length,) = _struct_I.unpack(str[start:end]) 07572 _v622.name = [] 07573 for i in range(0, length): 07574 start = end 07575 end += 4 07576 (length,) = _struct_I.unpack(str[start:end]) 07577 start = end 07578 end += length 07579 val3 = str[start:end] 07580 _v622.name.append(val3) 07581 start = end 07582 end += 4 07583 (length,) = _struct_I.unpack(str[start:end]) 07584 pattern = '<%sd'%length 07585 start = end 07586 end += struct.calcsize(pattern) 07587 _v622.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07588 start = end 07589 end += 4 07590 (length,) = _struct_I.unpack(str[start:end]) 07591 pattern = '<%sd'%length 07592 start = end 07593 end += struct.calcsize(pattern) 07594 _v622.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07595 start = end 07596 end += 4 07597 (length,) = _struct_I.unpack(str[start:end]) 07598 pattern = '<%sd'%length 07599 start = end 07600 end += struct.calcsize(pattern) 07601 _v622.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07602 _v625 = val1.grasp_posture 07603 _v626 = _v625.header 07604 start = end 07605 end += 4 07606 (_v626.seq,) = _struct_I.unpack(str[start:end]) 07607 _v627 = _v626.stamp 07608 _x = _v627 07609 start = end 07610 end += 8 07611 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07612 start = end 07613 end += 4 07614 (length,) = _struct_I.unpack(str[start:end]) 07615 start = end 07616 end += length 07617 _v626.frame_id = str[start:end] 07618 start = end 07619 end += 4 07620 (length,) = _struct_I.unpack(str[start:end]) 07621 _v625.name = [] 07622 for i in range(0, length): 07623 start = end 07624 end += 4 07625 (length,) = _struct_I.unpack(str[start:end]) 07626 start = end 07627 end += length 07628 val3 = str[start:end] 07629 _v625.name.append(val3) 07630 start = end 07631 end += 4 07632 (length,) = _struct_I.unpack(str[start:end]) 07633 pattern = '<%sd'%length 07634 start = end 07635 end += struct.calcsize(pattern) 07636 _v625.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07637 start = end 07638 end += 4 07639 (length,) = _struct_I.unpack(str[start:end]) 07640 pattern = '<%sd'%length 07641 start = end 07642 end += struct.calcsize(pattern) 07643 _v625.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07644 start = end 07645 end += 4 07646 (length,) = _struct_I.unpack(str[start:end]) 07647 pattern = '<%sd'%length 07648 start = end 07649 end += struct.calcsize(pattern) 07650 _v625.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07651 _v628 = val1.grasp_pose 07652 _v629 = _v628.position 07653 _x = _v629 07654 start = end 07655 end += 24 07656 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07657 _v630 = _v628.orientation 07658 _x = _v630 07659 start = end 07660 end += 32 07661 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 07662 _x = val1 07663 start = end 07664 end += 17 07665 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 07666 val1.cluster_rep = bool(val1.cluster_rep) 07667 start = end 07668 end += 4 07669 (length,) = _struct_I.unpack(str[start:end]) 07670 val1.moved_obstacles = [] 07671 for i in range(0, length): 07672 val2 = object_manipulation_msgs.msg.GraspableObject() 07673 start = end 07674 end += 4 07675 (length,) = _struct_I.unpack(str[start:end]) 07676 start = end 07677 end += length 07678 val2.reference_frame_id = str[start:end] 07679 start = end 07680 end += 4 07681 (length,) = _struct_I.unpack(str[start:end]) 07682 val2.potential_models = [] 07683 for i in range(0, length): 07684 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 07685 start = end 07686 end += 4 07687 (val3.model_id,) = _struct_i.unpack(str[start:end]) 07688 _v631 = val3.pose 07689 _v632 = _v631.header 07690 start = end 07691 end += 4 07692 (_v632.seq,) = _struct_I.unpack(str[start:end]) 07693 _v633 = _v632.stamp 07694 _x = _v633 07695 start = end 07696 end += 8 07697 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07698 start = end 07699 end += 4 07700 (length,) = _struct_I.unpack(str[start:end]) 07701 start = end 07702 end += length 07703 _v632.frame_id = str[start:end] 07704 _v634 = _v631.pose 07705 _v635 = _v634.position 07706 _x = _v635 07707 start = end 07708 end += 24 07709 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07710 _v636 = _v634.orientation 07711 _x = _v636 07712 start = end 07713 end += 32 07714 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 07715 start = end 07716 end += 4 07717 (val3.confidence,) = _struct_f.unpack(str[start:end]) 07718 start = end 07719 end += 4 07720 (length,) = _struct_I.unpack(str[start:end]) 07721 start = end 07722 end += length 07723 val3.detector_name = str[start:end] 07724 val2.potential_models.append(val3) 07725 _v637 = val2.cluster 07726 _v638 = _v637.header 07727 start = end 07728 end += 4 07729 (_v638.seq,) = _struct_I.unpack(str[start:end]) 07730 _v639 = _v638.stamp 07731 _x = _v639 07732 start = end 07733 end += 8 07734 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07735 start = end 07736 end += 4 07737 (length,) = _struct_I.unpack(str[start:end]) 07738 start = end 07739 end += length 07740 _v638.frame_id = str[start:end] 07741 start = end 07742 end += 4 07743 (length,) = _struct_I.unpack(str[start:end]) 07744 _v637.points = [] 07745 for i in range(0, length): 07746 val4 = geometry_msgs.msg.Point32() 07747 _x = val4 07748 start = end 07749 end += 12 07750 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 07751 _v637.points.append(val4) 07752 start = end 07753 end += 4 07754 (length,) = _struct_I.unpack(str[start:end]) 07755 _v637.channels = [] 07756 for i in range(0, length): 07757 val4 = sensor_msgs.msg.ChannelFloat32() 07758 start = end 07759 end += 4 07760 (length,) = _struct_I.unpack(str[start:end]) 07761 start = end 07762 end += length 07763 val4.name = str[start:end] 07764 start = end 07765 end += 4 07766 (length,) = _struct_I.unpack(str[start:end]) 07767 pattern = '<%sf'%length 07768 start = end 07769 end += struct.calcsize(pattern) 07770 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 07771 _v637.channels.append(val4) 07772 _v640 = val2.region 07773 _v641 = _v640.cloud 07774 _v642 = _v641.header 07775 start = end 07776 end += 4 07777 (_v642.seq,) = _struct_I.unpack(str[start:end]) 07778 _v643 = _v642.stamp 07779 _x = _v643 07780 start = end 07781 end += 8 07782 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07783 start = end 07784 end += 4 07785 (length,) = _struct_I.unpack(str[start:end]) 07786 start = end 07787 end += length 07788 _v642.frame_id = str[start:end] 07789 _x = _v641 07790 start = end 07791 end += 8 07792 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07793 start = end 07794 end += 4 07795 (length,) = _struct_I.unpack(str[start:end]) 07796 _v641.fields = [] 07797 for i in range(0, length): 07798 val5 = sensor_msgs.msg.PointField() 07799 start = end 07800 end += 4 07801 (length,) = _struct_I.unpack(str[start:end]) 07802 start = end 07803 end += length 07804 val5.name = str[start:end] 07805 _x = val5 07806 start = end 07807 end += 9 07808 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 07809 _v641.fields.append(val5) 07810 _x = _v641 07811 start = end 07812 end += 9 07813 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 07814 _v641.is_bigendian = bool(_v641.is_bigendian) 07815 start = end 07816 end += 4 07817 (length,) = _struct_I.unpack(str[start:end]) 07818 start = end 07819 end += length 07820 _v641.data = str[start:end] 07821 start = end 07822 end += 1 07823 (_v641.is_dense,) = _struct_B.unpack(str[start:end]) 07824 _v641.is_dense = bool(_v641.is_dense) 07825 start = end 07826 end += 4 07827 (length,) = _struct_I.unpack(str[start:end]) 07828 pattern = '<%si'%length 07829 start = end 07830 end += struct.calcsize(pattern) 07831 _v640.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 07832 _v644 = _v640.image 07833 _v645 = _v644.header 07834 start = end 07835 end += 4 07836 (_v645.seq,) = _struct_I.unpack(str[start:end]) 07837 _v646 = _v645.stamp 07838 _x = _v646 07839 start = end 07840 end += 8 07841 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07842 start = end 07843 end += 4 07844 (length,) = _struct_I.unpack(str[start:end]) 07845 start = end 07846 end += length 07847 _v645.frame_id = str[start:end] 07848 _x = _v644 07849 start = end 07850 end += 8 07851 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07852 start = end 07853 end += 4 07854 (length,) = _struct_I.unpack(str[start:end]) 07855 start = end 07856 end += length 07857 _v644.encoding = str[start:end] 07858 _x = _v644 07859 start = end 07860 end += 5 07861 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 07862 start = end 07863 end += 4 07864 (length,) = _struct_I.unpack(str[start:end]) 07865 start = end 07866 end += length 07867 _v644.data = str[start:end] 07868 _v647 = _v640.disparity_image 07869 _v648 = _v647.header 07870 start = end 07871 end += 4 07872 (_v648.seq,) = _struct_I.unpack(str[start:end]) 07873 _v649 = _v648.stamp 07874 _x = _v649 07875 start = end 07876 end += 8 07877 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07878 start = end 07879 end += 4 07880 (length,) = _struct_I.unpack(str[start:end]) 07881 start = end 07882 end += length 07883 _v648.frame_id = str[start:end] 07884 _x = _v647 07885 start = end 07886 end += 8 07887 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07888 start = end 07889 end += 4 07890 (length,) = _struct_I.unpack(str[start:end]) 07891 start = end 07892 end += length 07893 _v647.encoding = str[start:end] 07894 _x = _v647 07895 start = end 07896 end += 5 07897 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 07898 start = end 07899 end += 4 07900 (length,) = _struct_I.unpack(str[start:end]) 07901 start = end 07902 end += length 07903 _v647.data = str[start:end] 07904 _v650 = _v640.cam_info 07905 _v651 = _v650.header 07906 start = end 07907 end += 4 07908 (_v651.seq,) = _struct_I.unpack(str[start:end]) 07909 _v652 = _v651.stamp 07910 _x = _v652 07911 start = end 07912 end += 8 07913 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07914 start = end 07915 end += 4 07916 (length,) = _struct_I.unpack(str[start:end]) 07917 start = end 07918 end += length 07919 _v651.frame_id = str[start:end] 07920 _x = _v650 07921 start = end 07922 end += 8 07923 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 07924 start = end 07925 end += 4 07926 (length,) = _struct_I.unpack(str[start:end]) 07927 start = end 07928 end += length 07929 _v650.distortion_model = str[start:end] 07930 start = end 07931 end += 4 07932 (length,) = _struct_I.unpack(str[start:end]) 07933 pattern = '<%sd'%length 07934 start = end 07935 end += struct.calcsize(pattern) 07936 _v650.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 07937 start = end 07938 end += 72 07939 _v650.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 07940 start = end 07941 end += 72 07942 _v650.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 07943 start = end 07944 end += 96 07945 _v650.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 07946 _x = _v650 07947 start = end 07948 end += 8 07949 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 07950 _v653 = _v650.roi 07951 _x = _v653 07952 start = end 07953 end += 17 07954 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 07955 _v653.do_rectify = bool(_v653.do_rectify) 07956 _v654 = _v640.roi_box_pose 07957 _v655 = _v654.header 07958 start = end 07959 end += 4 07960 (_v655.seq,) = _struct_I.unpack(str[start:end]) 07961 _v656 = _v655.stamp 07962 _x = _v656 07963 start = end 07964 end += 8 07965 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 07966 start = end 07967 end += 4 07968 (length,) = _struct_I.unpack(str[start:end]) 07969 start = end 07970 end += length 07971 _v655.frame_id = str[start:end] 07972 _v657 = _v654.pose 07973 _v658 = _v657.position 07974 _x = _v658 07975 start = end 07976 end += 24 07977 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07978 _v659 = _v657.orientation 07979 _x = _v659 07980 start = end 07981 end += 32 07982 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 07983 _v660 = _v640.roi_box_dims 07984 _x = _v660 07985 start = end 07986 end += 24 07987 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 07988 start = end 07989 end += 4 07990 (length,) = _struct_I.unpack(str[start:end]) 07991 start = end 07992 end += length 07993 val2.collision_name = str[start:end] 07994 val1.moved_obstacles.append(val2) 07995 self.action_result.result.attempted_grasps.append(val1) 07996 start = end 07997 end += 4 07998 (length,) = _struct_I.unpack(str[start:end]) 07999 self.action_result.result.attempted_grasp_results = [] 08000 for i in range(0, length): 08001 val1 = object_manipulation_msgs.msg.GraspResult() 08002 _x = val1 08003 start = end 08004 end += 5 08005 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end]) 08006 val1.continuation_possible = bool(val1.continuation_possible) 08007 self.action_result.result.attempted_grasp_results.append(val1) 08008 _x = self 08009 start = end 08010 end += 12 08011 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 08012 start = end 08013 end += 4 08014 (length,) = _struct_I.unpack(str[start:end]) 08015 start = end 08016 end += length 08017 self.action_feedback.header.frame_id = str[start:end] 08018 _x = self 08019 start = end 08020 end += 8 08021 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 08022 start = end 08023 end += 4 08024 (length,) = _struct_I.unpack(str[start:end]) 08025 start = end 08026 end += length 08027 self.action_feedback.status.goal_id.id = str[start:end] 08028 start = end 08029 end += 1 08030 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 08031 start = end 08032 end += 4 08033 (length,) = _struct_I.unpack(str[start:end]) 08034 start = end 08035 end += length 08036 self.action_feedback.status.text = str[start:end] 08037 _x = self 08038 start = end 08039 end += 8 08040 (_x.action_feedback.feedback.current_grasp, _x.action_feedback.feedback.total_grasps,) = _struct_2i.unpack(str[start:end]) 08041 return self 08042 except struct.error as e: 08043 raise roslib.message.DeserializationError(e) #most likely buffer underfill 08044 08045 _struct_I = roslib.message.struct_I 08046 _struct_IBI = struct.Struct("<IBI") 08047 _struct_12d = struct.Struct("<12d") 08048 _struct_f3I = struct.Struct("<f3I") 08049 _struct_dB2f = struct.Struct("<dB2f") 08050 _struct_BI = struct.Struct("<BI") 08051 _struct_10d = struct.Struct("<10d") 08052 _struct_3I = struct.Struct("<3I") 08053 _struct_B2I = struct.Struct("<B2I") 08054 _struct_5B = struct.Struct("<5B") 08055 _struct_8dB2f = struct.Struct("<8dB2f") 08056 _struct_i3I = struct.Struct("<i3I") 08057 _struct_iB = struct.Struct("<iB") 08058 _struct_3f = struct.Struct("<3f") 08059 _struct_3d = struct.Struct("<3d") 08060 _struct_B = struct.Struct("<B") 08061 _struct_di = struct.Struct("<di") 08062 _struct_9d = struct.Struct("<9d") 08063 _struct_2I = struct.Struct("<2I") 08064 _struct_6IB3I = struct.Struct("<6IB3I") 08065 _struct_b = struct.Struct("<b") 08066 _struct_d = struct.Struct("<d") 08067 _struct_f = struct.Struct("<f") 08068 _struct_i = struct.Struct("<i") 08069 _struct_3d2f = struct.Struct("<3d2f") 08070 _struct_4d = struct.Struct("<4d") 08071 _struct_2i = struct.Struct("<2i") 08072 _struct_4IB = struct.Struct("<4IB")