$search
00001 """autogenerated by genmsg_py from PickupGoal.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 geometry_msgs.msg 00008 import sensor_msgs.msg 00009 import household_objects_database_msgs.msg 00010 import std_msgs.msg 00011 00012 class PickupGoal(roslib.message.Message): 00013 _md5sum = "49435aa8e5853b02489b00dc68d7fa3b" 00014 _type = "object_manipulation_msgs/PickupGoal" 00015 _has_header = False #flag to mark the presence of a Header object 00016 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00017 # An action for picking up an object 00018 00019 # which arm to be used for grasping 00020 string arm_name 00021 00022 # the object to be grasped 00023 GraspableObject target 00024 00025 # a list of grasps to be used 00026 # if empty, the grasp executive will call one of its own planners 00027 Grasp[] desired_grasps 00028 00029 # how the object should be lifted after the grasp 00030 # the frame_id that this lift is specified in MUST be either the robot_frame 00031 # or the gripper_frame specified in your hand description file 00032 GripperTranslation lift 00033 00034 # the name that the target object has in the collision map 00035 # can be left empty if no name is available 00036 string collision_object_name 00037 00038 # the name that the support surface (e.g. table) has in the collision map 00039 # can be left empty if no name is available 00040 string collision_support_surface_name 00041 00042 # whether collisions between the gripper and the support surface should be acceptable 00043 # during move from pre-grasp to grasp and during lift. Collisions when moving to the 00044 # pre-grasp location are still not allowed even if this is set to true. 00045 bool allow_gripper_support_collision 00046 00047 # whether reactive grasp execution using tactile sensors should be used 00048 bool use_reactive_execution 00049 00050 # whether reactive object lifting based on tactile sensors should be used 00051 bool use_reactive_lift 00052 00053 # set this to true if you only want to query the manipulation pipeline as to what 00054 # grasps it thinks are feasible, without actually executing them. If this is set to 00055 # true, the atempted_grasp_results field of the result will be populated, but no arm 00056 # movement will be attempted 00057 bool only_perform_feasibility_test 00058 00059 # set this to true if you want to ignore all collisions throughout the pickup 00060 # and also move directly to the pre-grasp using Cartesian controllers 00061 bool ignore_collisions 00062 00063 # OPTIONAL (These will not have to be filled out most of the time) 00064 # constraints to be imposed on every point in the motion of the arm 00065 arm_navigation_msgs/Constraints path_constraints 00066 00067 # OPTIONAL (These will not have to be filled out most of the time) 00068 # additional collision operations to be used for every arm movement performed 00069 # during grasping. Note that these will be added on top of (and thus overide) other 00070 # collision operations that the grasping pipeline deems necessary. Should be used 00071 # with care and only if special behaviors are desired 00072 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations 00073 00074 # OPTIONAL (These will not have to be filled out most of the time) 00075 # additional link paddings to be used for every arm movement performed 00076 # during grasping. Note that these will be added on top of (and thus overide) other 00077 # link paddings that the grasping pipeline deems necessary. Should be used 00078 # with care and only if special behaviors are desired 00079 arm_navigation_msgs/LinkPadding[] additional_link_padding 00080 00081 # an optional list of obstacles that we have semantic information about 00082 # and that can be moved in the course of grasping 00083 GraspableObject[] movable_obstacles 00084 00085 # the maximum contact force to use while grasping (<=0 to disable) 00086 float32 max_contact_force 00087 00088 00089 ================================================================================ 00090 MSG: object_manipulation_msgs/GraspableObject 00091 # an object that the object_manipulator can work on 00092 00093 # a graspable object can be represented in multiple ways. This message 00094 # can contain all of them. Which one is actually used is up to the receiver 00095 # of this message. When adding new representations, one must be careful that 00096 # they have reasonable lightweight defaults indicating that that particular 00097 # representation is not available. 00098 00099 # the tf frame to be used as a reference frame when combining information from 00100 # the different representations below 00101 string reference_frame_id 00102 00103 # potential recognition results from a database of models 00104 # all poses are relative to the object reference pose 00105 household_objects_database_msgs/DatabaseModelPose[] potential_models 00106 00107 # the point cloud itself 00108 sensor_msgs/PointCloud cluster 00109 00110 # a region of a PointCloud2 of interest 00111 object_manipulation_msgs/SceneRegion region 00112 00113 # the name that this object has in the collision environment 00114 string collision_name 00115 ================================================================================ 00116 MSG: household_objects_database_msgs/DatabaseModelPose 00117 # Informs that a specific model from the Model Database has been 00118 # identified at a certain location 00119 00120 # the database id of the model 00121 int32 model_id 00122 00123 # the pose that it can be found in 00124 geometry_msgs/PoseStamped pose 00125 00126 # a measure of the confidence level in this detection result 00127 float32 confidence 00128 00129 # the name of the object detector that generated this detection result 00130 string detector_name 00131 00132 ================================================================================ 00133 MSG: geometry_msgs/PoseStamped 00134 # A Pose with reference coordinate frame and timestamp 00135 Header header 00136 Pose pose 00137 00138 ================================================================================ 00139 MSG: std_msgs/Header 00140 # Standard metadata for higher-level stamped data types. 00141 # This is generally used to communicate timestamped data 00142 # in a particular coordinate frame. 00143 # 00144 # sequence ID: consecutively increasing ID 00145 uint32 seq 00146 #Two-integer timestamp that is expressed as: 00147 # * stamp.secs: seconds (stamp_secs) since epoch 00148 # * stamp.nsecs: nanoseconds since stamp_secs 00149 # time-handling sugar is provided by the client library 00150 time stamp 00151 #Frame this data is associated with 00152 # 0: no frame 00153 # 1: global frame 00154 string frame_id 00155 00156 ================================================================================ 00157 MSG: geometry_msgs/Pose 00158 # A representation of pose in free space, composed of postion and orientation. 00159 Point position 00160 Quaternion orientation 00161 00162 ================================================================================ 00163 MSG: geometry_msgs/Point 00164 # This contains the position of a point in free space 00165 float64 x 00166 float64 y 00167 float64 z 00168 00169 ================================================================================ 00170 MSG: geometry_msgs/Quaternion 00171 # This represents an orientation in free space in quaternion form. 00172 00173 float64 x 00174 float64 y 00175 float64 z 00176 float64 w 00177 00178 ================================================================================ 00179 MSG: sensor_msgs/PointCloud 00180 # This message holds a collection of 3d points, plus optional additional 00181 # information about each point. 00182 00183 # Time of sensor data acquisition, coordinate frame ID. 00184 Header header 00185 00186 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00187 # in the frame given in the header. 00188 geometry_msgs/Point32[] points 00189 00190 # Each channel should have the same number of elements as points array, 00191 # and the data in each channel should correspond 1:1 with each point. 00192 # Channel names in common practice are listed in ChannelFloat32.msg. 00193 ChannelFloat32[] channels 00194 00195 ================================================================================ 00196 MSG: geometry_msgs/Point32 00197 # This contains the position of a point in free space(with 32 bits of precision). 00198 # It is recommeded to use Point wherever possible instead of Point32. 00199 # 00200 # This recommendation is to promote interoperability. 00201 # 00202 # This message is designed to take up less space when sending 00203 # lots of points at once, as in the case of a PointCloud. 00204 00205 float32 x 00206 float32 y 00207 float32 z 00208 ================================================================================ 00209 MSG: sensor_msgs/ChannelFloat32 00210 # This message is used by the PointCloud message to hold optional data 00211 # associated with each point in the cloud. The length of the values 00212 # array should be the same as the length of the points array in the 00213 # PointCloud, and each value should be associated with the corresponding 00214 # point. 00215 00216 # Channel names in existing practice include: 00217 # "u", "v" - row and column (respectively) in the left stereo image. 00218 # This is opposite to usual conventions but remains for 00219 # historical reasons. The newer PointCloud2 message has no 00220 # such problem. 00221 # "rgb" - For point clouds produced by color stereo cameras. uint8 00222 # (R,G,B) values packed into the least significant 24 bits, 00223 # in order. 00224 # "intensity" - laser or pixel intensity. 00225 # "distance" 00226 00227 # The channel name should give semantics of the channel (e.g. 00228 # "intensity" instead of "value"). 00229 string name 00230 00231 # The values array should be 1-1 with the elements of the associated 00232 # PointCloud. 00233 float32[] values 00234 00235 ================================================================================ 00236 MSG: object_manipulation_msgs/SceneRegion 00237 # Point cloud 00238 sensor_msgs/PointCloud2 cloud 00239 00240 # Indices for the region of interest 00241 int32[] mask 00242 00243 # One of the corresponding 2D images, if applicable 00244 sensor_msgs/Image image 00245 00246 # The disparity image, if applicable 00247 sensor_msgs/Image disparity_image 00248 00249 # Camera info for the camera that took the image 00250 sensor_msgs/CameraInfo cam_info 00251 00252 # a 3D region of interest for grasp planning 00253 geometry_msgs/PoseStamped roi_box_pose 00254 geometry_msgs/Vector3 roi_box_dims 00255 00256 ================================================================================ 00257 MSG: sensor_msgs/PointCloud2 00258 # This message holds a collection of N-dimensional points, which may 00259 # contain additional information such as normals, intensity, etc. The 00260 # point data is stored as a binary blob, its layout described by the 00261 # contents of the "fields" array. 00262 00263 # The point cloud data may be organized 2d (image-like) or 1d 00264 # (unordered). Point clouds organized as 2d images may be produced by 00265 # camera depth sensors such as stereo or time-of-flight. 00266 00267 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00268 # points). 00269 Header header 00270 00271 # 2D structure of the point cloud. If the cloud is unordered, height is 00272 # 1 and width is the length of the point cloud. 00273 uint32 height 00274 uint32 width 00275 00276 # Describes the channels and their layout in the binary data blob. 00277 PointField[] fields 00278 00279 bool is_bigendian # Is this data bigendian? 00280 uint32 point_step # Length of a point in bytes 00281 uint32 row_step # Length of a row in bytes 00282 uint8[] data # Actual point data, size is (row_step*height) 00283 00284 bool is_dense # True if there are no invalid points 00285 00286 ================================================================================ 00287 MSG: sensor_msgs/PointField 00288 # This message holds the description of one point entry in the 00289 # PointCloud2 message format. 00290 uint8 INT8 = 1 00291 uint8 UINT8 = 2 00292 uint8 INT16 = 3 00293 uint8 UINT16 = 4 00294 uint8 INT32 = 5 00295 uint8 UINT32 = 6 00296 uint8 FLOAT32 = 7 00297 uint8 FLOAT64 = 8 00298 00299 string name # Name of field 00300 uint32 offset # Offset from start of point struct 00301 uint8 datatype # Datatype enumeration, see above 00302 uint32 count # How many elements in the field 00303 00304 ================================================================================ 00305 MSG: sensor_msgs/Image 00306 # This message contains an uncompressed image 00307 # (0, 0) is at top-left corner of image 00308 # 00309 00310 Header header # Header timestamp should be acquisition time of image 00311 # Header frame_id should be optical frame of camera 00312 # origin of frame should be optical center of cameara 00313 # +x should point to the right in the image 00314 # +y should point down in the image 00315 # +z should point into to plane of the image 00316 # If the frame_id here and the frame_id of the CameraInfo 00317 # message associated with the image conflict 00318 # the behavior is undefined 00319 00320 uint32 height # image height, that is, number of rows 00321 uint32 width # image width, that is, number of columns 00322 00323 # The legal values for encoding are in file src/image_encodings.cpp 00324 # If you want to standardize a new string format, join 00325 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00326 00327 string encoding # Encoding of pixels -- channel meaning, ordering, size 00328 # taken from the list of strings in src/image_encodings.cpp 00329 00330 uint8 is_bigendian # is this data bigendian? 00331 uint32 step # Full row length in bytes 00332 uint8[] data # actual matrix data, size is (step * rows) 00333 00334 ================================================================================ 00335 MSG: sensor_msgs/CameraInfo 00336 # This message defines meta information for a camera. It should be in a 00337 # camera namespace on topic "camera_info" and accompanied by up to five 00338 # image topics named: 00339 # 00340 # image_raw - raw data from the camera driver, possibly Bayer encoded 00341 # image - monochrome, distorted 00342 # image_color - color, distorted 00343 # image_rect - monochrome, rectified 00344 # image_rect_color - color, rectified 00345 # 00346 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00347 # for producing the four processed image topics from image_raw and 00348 # camera_info. The meaning of the camera parameters are described in 00349 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00350 # 00351 # The image_geometry package provides a user-friendly interface to 00352 # common operations using this meta information. If you want to, e.g., 00353 # project a 3d point into image coordinates, we strongly recommend 00354 # using image_geometry. 00355 # 00356 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00357 # zeroed out. In particular, clients may assume that K[0] == 0.0 00358 # indicates an uncalibrated camera. 00359 00360 ####################################################################### 00361 # Image acquisition info # 00362 ####################################################################### 00363 00364 # Time of image acquisition, camera coordinate frame ID 00365 Header header # Header timestamp should be acquisition time of image 00366 # Header frame_id should be optical frame of camera 00367 # origin of frame should be optical center of camera 00368 # +x should point to the right in the image 00369 # +y should point down in the image 00370 # +z should point into the plane of the image 00371 00372 00373 ####################################################################### 00374 # Calibration Parameters # 00375 ####################################################################### 00376 # These are fixed during camera calibration. Their values will be the # 00377 # same in all messages until the camera is recalibrated. Note that # 00378 # self-calibrating systems may "recalibrate" frequently. # 00379 # # 00380 # The internal parameters can be used to warp a raw (distorted) image # 00381 # to: # 00382 # 1. An undistorted image (requires D and K) # 00383 # 2. A rectified image (requires D, K, R) # 00384 # The projection matrix P projects 3D points into the rectified image.# 00385 ####################################################################### 00386 00387 # The image dimensions with which the camera was calibrated. Normally 00388 # this will be the full camera resolution in pixels. 00389 uint32 height 00390 uint32 width 00391 00392 # The distortion model used. Supported models are listed in 00393 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00394 # simple model of radial and tangential distortion - is sufficent. 00395 string distortion_model 00396 00397 # The distortion parameters, size depending on the distortion model. 00398 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00399 float64[] D 00400 00401 # Intrinsic camera matrix for the raw (distorted) images. 00402 # [fx 0 cx] 00403 # K = [ 0 fy cy] 00404 # [ 0 0 1] 00405 # Projects 3D points in the camera coordinate frame to 2D pixel 00406 # coordinates using the focal lengths (fx, fy) and principal point 00407 # (cx, cy). 00408 float64[9] K # 3x3 row-major matrix 00409 00410 # Rectification matrix (stereo cameras only) 00411 # A rotation matrix aligning the camera coordinate system to the ideal 00412 # stereo image plane so that epipolar lines in both stereo images are 00413 # parallel. 00414 float64[9] R # 3x3 row-major matrix 00415 00416 # Projection/camera matrix 00417 # [fx' 0 cx' Tx] 00418 # P = [ 0 fy' cy' Ty] 00419 # [ 0 0 1 0] 00420 # By convention, this matrix specifies the intrinsic (camera) matrix 00421 # of the processed (rectified) image. That is, the left 3x3 portion 00422 # is the normal camera intrinsic matrix for the rectified image. 00423 # It projects 3D points in the camera coordinate frame to 2D pixel 00424 # coordinates using the focal lengths (fx', fy') and principal point 00425 # (cx', cy') - these may differ from the values in K. 00426 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00427 # also have R = the identity and P[1:3,1:3] = K. 00428 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00429 # position of the optical center of the second camera in the first 00430 # camera's frame. We assume Tz = 0 so both cameras are in the same 00431 # stereo image plane. The first camera always has Tx = Ty = 0. For 00432 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00433 # Tx = -fx' * B, where B is the baseline between the cameras. 00434 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00435 # the rectified image is given by: 00436 # [u v w]' = P * [X Y Z 1]' 00437 # x = u / w 00438 # y = v / w 00439 # This holds for both images of a stereo pair. 00440 float64[12] P # 3x4 row-major matrix 00441 00442 00443 ####################################################################### 00444 # Operational Parameters # 00445 ####################################################################### 00446 # These define the image region actually captured by the camera # 00447 # driver. Although they affect the geometry of the output image, they # 00448 # may be changed freely without recalibrating the camera. # 00449 ####################################################################### 00450 00451 # Binning refers here to any camera setting which combines rectangular 00452 # neighborhoods of pixels into larger "super-pixels." It reduces the 00453 # resolution of the output image to 00454 # (width / binning_x) x (height / binning_y). 00455 # The default values binning_x = binning_y = 0 is considered the same 00456 # as binning_x = binning_y = 1 (no subsampling). 00457 uint32 binning_x 00458 uint32 binning_y 00459 00460 # Region of interest (subwindow of full camera resolution), given in 00461 # full resolution (unbinned) image coordinates. A particular ROI 00462 # always denotes the same window of pixels on the camera sensor, 00463 # regardless of binning settings. 00464 # The default setting of roi (all values 0) is considered the same as 00465 # full resolution (roi.width = width, roi.height = height). 00466 RegionOfInterest roi 00467 00468 ================================================================================ 00469 MSG: sensor_msgs/RegionOfInterest 00470 # This message is used to specify a region of interest within an image. 00471 # 00472 # When used to specify the ROI setting of the camera when the image was 00473 # taken, the height and width fields should either match the height and 00474 # width fields for the associated image; or height = width = 0 00475 # indicates that the full resolution image was captured. 00476 00477 uint32 x_offset # Leftmost pixel of the ROI 00478 # (0 if the ROI includes the left edge of the image) 00479 uint32 y_offset # Topmost pixel of the ROI 00480 # (0 if the ROI includes the top edge of the image) 00481 uint32 height # Height of ROI 00482 uint32 width # Width of ROI 00483 00484 # True if a distinct rectified ROI should be calculated from the "raw" 00485 # ROI in this message. Typically this should be False if the full image 00486 # is captured (ROI not used), and True if a subwindow is captured (ROI 00487 # used). 00488 bool do_rectify 00489 00490 ================================================================================ 00491 MSG: geometry_msgs/Vector3 00492 # This represents a vector in free space. 00493 00494 float64 x 00495 float64 y 00496 float64 z 00497 ================================================================================ 00498 MSG: object_manipulation_msgs/Grasp 00499 00500 # The internal posture of the hand for the pre-grasp 00501 # only positions are used 00502 sensor_msgs/JointState pre_grasp_posture 00503 00504 # The internal posture of the hand for the grasp 00505 # positions and efforts are used 00506 sensor_msgs/JointState grasp_posture 00507 00508 # The position of the end-effector for the grasp relative to a reference frame 00509 # (that is always specified elsewhere, not in this message) 00510 geometry_msgs/Pose grasp_pose 00511 00512 # The estimated probability of success for this grasp 00513 float64 success_probability 00514 00515 # Debug flag to indicate that this grasp would be the best in its cluster 00516 bool cluster_rep 00517 00518 # how far the pre-grasp should ideally be away from the grasp 00519 float32 desired_approach_distance 00520 00521 # how much distance between pre-grasp and grasp must actually be feasible 00522 # for the grasp not to be rejected 00523 float32 min_approach_distance 00524 00525 # an optional list of obstacles that we have semantic information about 00526 # and that we expect might move in the course of executing this grasp 00527 # the grasp planner is expected to make sure they move in an OK way; during 00528 # execution, grasp executors will not check for collisions against these objects 00529 GraspableObject[] moved_obstacles 00530 00531 ================================================================================ 00532 MSG: sensor_msgs/JointState 00533 # This is a message that holds data to describe the state of a set of torque controlled joints. 00534 # 00535 # The state of each joint (revolute or prismatic) is defined by: 00536 # * the position of the joint (rad or m), 00537 # * the velocity of the joint (rad/s or m/s) and 00538 # * the effort that is applied in the joint (Nm or N). 00539 # 00540 # Each joint is uniquely identified by its name 00541 # The header specifies the time at which the joint states were recorded. All the joint states 00542 # in one message have to be recorded at the same time. 00543 # 00544 # This message consists of a multiple arrays, one for each part of the joint state. 00545 # The goal is to make each of the fields optional. When e.g. your joints have no 00546 # effort associated with them, you can leave the effort array empty. 00547 # 00548 # All arrays in this message should have the same size, or be empty. 00549 # This is the only way to uniquely associate the joint name with the correct 00550 # states. 00551 00552 00553 Header header 00554 00555 string[] name 00556 float64[] position 00557 float64[] velocity 00558 float64[] effort 00559 00560 ================================================================================ 00561 MSG: object_manipulation_msgs/GripperTranslation 00562 # defines a translation for the gripper, used in pickup or place tasks 00563 # for example for lifting an object off a table or approaching the table for placing 00564 00565 # the direction of the translation 00566 geometry_msgs/Vector3Stamped direction 00567 00568 # the desired translation distance 00569 float32 desired_distance 00570 00571 # the min distance that must be considered feasible before the 00572 # grasp is even attempted 00573 float32 min_distance 00574 ================================================================================ 00575 MSG: geometry_msgs/Vector3Stamped 00576 # This represents a Vector3 with reference coordinate frame and timestamp 00577 Header header 00578 Vector3 vector 00579 00580 ================================================================================ 00581 MSG: arm_navigation_msgs/Constraints 00582 # This message contains a list of motion planning constraints. 00583 00584 arm_navigation_msgs/JointConstraint[] joint_constraints 00585 arm_navigation_msgs/PositionConstraint[] position_constraints 00586 arm_navigation_msgs/OrientationConstraint[] orientation_constraints 00587 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints 00588 00589 ================================================================================ 00590 MSG: arm_navigation_msgs/JointConstraint 00591 # Constrain the position of a joint to be within a certain bound 00592 string joint_name 00593 00594 # the bound to be achieved is [position - tolerance_below, position + tolerance_above] 00595 float64 position 00596 float64 tolerance_above 00597 float64 tolerance_below 00598 00599 # A weighting factor for this constraint 00600 float64 weight 00601 ================================================================================ 00602 MSG: arm_navigation_msgs/PositionConstraint 00603 # This message contains the definition of a position constraint. 00604 Header header 00605 00606 # The robot link this constraint refers to 00607 string link_name 00608 00609 # The offset (in the link frame) for the target point on the link we are planning for 00610 geometry_msgs/Point target_point_offset 00611 00612 # The nominal/target position for the point we are planning for 00613 geometry_msgs/Point position 00614 00615 # The shape of the bounded region that constrains the position of the end-effector 00616 # This region is always centered at the position defined above 00617 arm_navigation_msgs/Shape constraint_region_shape 00618 00619 # The orientation of the bounded region that constrains the position of the end-effector. 00620 # This allows the specification of non-axis aligned constraints 00621 geometry_msgs/Quaternion constraint_region_orientation 00622 00623 # Constraint weighting factor - a weight for this constraint 00624 float64 weight 00625 00626 ================================================================================ 00627 MSG: arm_navigation_msgs/Shape 00628 byte SPHERE=0 00629 byte BOX=1 00630 byte CYLINDER=2 00631 byte MESH=3 00632 00633 byte type 00634 00635 00636 #### define sphere, box, cylinder #### 00637 # the origin of each shape is considered at the shape's center 00638 00639 # for sphere 00640 # radius := dimensions[0] 00641 00642 # for cylinder 00643 # radius := dimensions[0] 00644 # length := dimensions[1] 00645 # the length is along the Z axis 00646 00647 # for box 00648 # size_x := dimensions[0] 00649 # size_y := dimensions[1] 00650 # size_z := dimensions[2] 00651 float64[] dimensions 00652 00653 00654 #### define mesh #### 00655 00656 # list of triangles; triangle k is defined by tre vertices located 00657 # at indices triangles[3k], triangles[3k+1], triangles[3k+2] 00658 int32[] triangles 00659 geometry_msgs/Point[] vertices 00660 00661 ================================================================================ 00662 MSG: arm_navigation_msgs/OrientationConstraint 00663 # This message contains the definition of an orientation constraint. 00664 Header header 00665 00666 # The robot link this constraint refers to 00667 string link_name 00668 00669 # The type of the constraint 00670 int32 type 00671 int32 LINK_FRAME=0 00672 int32 HEADER_FRAME=1 00673 00674 # The desired orientation of the robot link specified as a quaternion 00675 geometry_msgs/Quaternion orientation 00676 00677 # optional RPY error tolerances specified if 00678 float64 absolute_roll_tolerance 00679 float64 absolute_pitch_tolerance 00680 float64 absolute_yaw_tolerance 00681 00682 # Constraint weighting factor - a weight for this constraint 00683 float64 weight 00684 00685 ================================================================================ 00686 MSG: arm_navigation_msgs/VisibilityConstraint 00687 # This message contains the definition of a visibility constraint. 00688 Header header 00689 00690 # The point stamped target that needs to be kept within view of the sensor 00691 geometry_msgs/PointStamped target 00692 00693 # The local pose of the frame in which visibility is to be maintained 00694 # The frame id should represent the robot link to which the sensor is attached 00695 # The visual axis of the sensor is assumed to be along the X axis of this frame 00696 geometry_msgs/PoseStamped sensor_pose 00697 00698 # The deviation (in radians) that will be tolerated 00699 # Constraint error will be measured as the solid angle between the 00700 # X axis of the frame defined above and the vector between the origin 00701 # of the frame defined above and the target location 00702 float64 absolute_tolerance 00703 00704 00705 ================================================================================ 00706 MSG: geometry_msgs/PointStamped 00707 # This represents a Point with reference coordinate frame and timestamp 00708 Header header 00709 Point point 00710 00711 ================================================================================ 00712 MSG: arm_navigation_msgs/OrderedCollisionOperations 00713 # A set of collision operations that will be performed in the order they are specified 00714 CollisionOperation[] collision_operations 00715 ================================================================================ 00716 MSG: arm_navigation_msgs/CollisionOperation 00717 # A definition of a collision operation 00718 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions 00719 # between the gripper and all objects in the collision space 00720 00721 string object1 00722 string object2 00723 string COLLISION_SET_ALL="all" 00724 string COLLISION_SET_OBJECTS="objects" 00725 string COLLISION_SET_ATTACHED_OBJECTS="attached" 00726 00727 # The penetration distance to which collisions are allowed. This is 0.0 by default. 00728 float64 penetration_distance 00729 00730 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above 00731 int32 operation 00732 int32 DISABLE=0 00733 int32 ENABLE=1 00734 00735 ================================================================================ 00736 MSG: arm_navigation_msgs/LinkPadding 00737 #name for the link 00738 string link_name 00739 00740 # padding to apply to the link 00741 float64 padding 00742 00743 """ 00744 __slots__ = ['arm_name','target','desired_grasps','lift','collision_object_name','collision_support_surface_name','allow_gripper_support_collision','use_reactive_execution','use_reactive_lift','only_perform_feasibility_test','ignore_collisions','path_constraints','additional_collision_operations','additional_link_padding','movable_obstacles','max_contact_force'] 00745 _slot_types = ['string','object_manipulation_msgs/GraspableObject','object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GripperTranslation','string','string','bool','bool','bool','bool','bool','arm_navigation_msgs/Constraints','arm_navigation_msgs/OrderedCollisionOperations','arm_navigation_msgs/LinkPadding[]','object_manipulation_msgs/GraspableObject[]','float32'] 00746 00747 def __init__(self, *args, **kwds): 00748 """ 00749 Constructor. Any message fields that are implicitly/explicitly 00750 set to None will be assigned a default value. The recommend 00751 use is keyword arguments as this is more robust to future message 00752 changes. You cannot mix in-order arguments and keyword arguments. 00753 00754 The available fields are: 00755 arm_name,target,desired_grasps,lift,collision_object_name,collision_support_surface_name,allow_gripper_support_collision,use_reactive_execution,use_reactive_lift,only_perform_feasibility_test,ignore_collisions,path_constraints,additional_collision_operations,additional_link_padding,movable_obstacles,max_contact_force 00756 00757 @param args: complete set of field values, in .msg order 00758 @param kwds: use keyword arguments corresponding to message field names 00759 to set specific fields. 00760 """ 00761 if args or kwds: 00762 super(PickupGoal, self).__init__(*args, **kwds) 00763 #message fields cannot be None, assign default values for those that are 00764 if self.arm_name is None: 00765 self.arm_name = '' 00766 if self.target is None: 00767 self.target = object_manipulation_msgs.msg.GraspableObject() 00768 if self.desired_grasps is None: 00769 self.desired_grasps = [] 00770 if self.lift is None: 00771 self.lift = object_manipulation_msgs.msg.GripperTranslation() 00772 if self.collision_object_name is None: 00773 self.collision_object_name = '' 00774 if self.collision_support_surface_name is None: 00775 self.collision_support_surface_name = '' 00776 if self.allow_gripper_support_collision is None: 00777 self.allow_gripper_support_collision = False 00778 if self.use_reactive_execution is None: 00779 self.use_reactive_execution = False 00780 if self.use_reactive_lift is None: 00781 self.use_reactive_lift = False 00782 if self.only_perform_feasibility_test is None: 00783 self.only_perform_feasibility_test = False 00784 if self.ignore_collisions is None: 00785 self.ignore_collisions = False 00786 if self.path_constraints is None: 00787 self.path_constraints = arm_navigation_msgs.msg.Constraints() 00788 if self.additional_collision_operations is None: 00789 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations() 00790 if self.additional_link_padding is None: 00791 self.additional_link_padding = [] 00792 if self.movable_obstacles is None: 00793 self.movable_obstacles = [] 00794 if self.max_contact_force is None: 00795 self.max_contact_force = 0. 00796 else: 00797 self.arm_name = '' 00798 self.target = object_manipulation_msgs.msg.GraspableObject() 00799 self.desired_grasps = [] 00800 self.lift = object_manipulation_msgs.msg.GripperTranslation() 00801 self.collision_object_name = '' 00802 self.collision_support_surface_name = '' 00803 self.allow_gripper_support_collision = False 00804 self.use_reactive_execution = False 00805 self.use_reactive_lift = False 00806 self.only_perform_feasibility_test = False 00807 self.ignore_collisions = False 00808 self.path_constraints = arm_navigation_msgs.msg.Constraints() 00809 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations() 00810 self.additional_link_padding = [] 00811 self.movable_obstacles = [] 00812 self.max_contact_force = 0. 00813 00814 def _get_types(self): 00815 """ 00816 internal API method 00817 """ 00818 return self._slot_types 00819 00820 def serialize(self, buff): 00821 """ 00822 serialize message into buffer 00823 @param buff: buffer 00824 @type buff: StringIO 00825 """ 00826 try: 00827 _x = self.arm_name 00828 length = len(_x) 00829 buff.write(struct.pack('<I%ss'%length, length, _x)) 00830 _x = self.target.reference_frame_id 00831 length = len(_x) 00832 buff.write(struct.pack('<I%ss'%length, length, _x)) 00833 length = len(self.target.potential_models) 00834 buff.write(_struct_I.pack(length)) 00835 for val1 in self.target.potential_models: 00836 buff.write(_struct_i.pack(val1.model_id)) 00837 _v1 = val1.pose 00838 _v2 = _v1.header 00839 buff.write(_struct_I.pack(_v2.seq)) 00840 _v3 = _v2.stamp 00841 _x = _v3 00842 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00843 _x = _v2.frame_id 00844 length = len(_x) 00845 buff.write(struct.pack('<I%ss'%length, length, _x)) 00846 _v4 = _v1.pose 00847 _v5 = _v4.position 00848 _x = _v5 00849 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00850 _v6 = _v4.orientation 00851 _x = _v6 00852 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00853 buff.write(_struct_f.pack(val1.confidence)) 00854 _x = val1.detector_name 00855 length = len(_x) 00856 buff.write(struct.pack('<I%ss'%length, length, _x)) 00857 _x = self 00858 buff.write(_struct_3I.pack(_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs)) 00859 _x = self.target.cluster.header.frame_id 00860 length = len(_x) 00861 buff.write(struct.pack('<I%ss'%length, length, _x)) 00862 length = len(self.target.cluster.points) 00863 buff.write(_struct_I.pack(length)) 00864 for val1 in self.target.cluster.points: 00865 _x = val1 00866 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00867 length = len(self.target.cluster.channels) 00868 buff.write(_struct_I.pack(length)) 00869 for val1 in self.target.cluster.channels: 00870 _x = val1.name 00871 length = len(_x) 00872 buff.write(struct.pack('<I%ss'%length, length, _x)) 00873 length = len(val1.values) 00874 buff.write(_struct_I.pack(length)) 00875 pattern = '<%sf'%length 00876 buff.write(struct.pack(pattern, *val1.values)) 00877 _x = self 00878 buff.write(_struct_3I.pack(_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs)) 00879 _x = self.target.region.cloud.header.frame_id 00880 length = len(_x) 00881 buff.write(struct.pack('<I%ss'%length, length, _x)) 00882 _x = self 00883 buff.write(_struct_2I.pack(_x.target.region.cloud.height, _x.target.region.cloud.width)) 00884 length = len(self.target.region.cloud.fields) 00885 buff.write(_struct_I.pack(length)) 00886 for val1 in self.target.region.cloud.fields: 00887 _x = val1.name 00888 length = len(_x) 00889 buff.write(struct.pack('<I%ss'%length, length, _x)) 00890 _x = val1 00891 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00892 _x = self 00893 buff.write(_struct_B2I.pack(_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step)) 00894 _x = self.target.region.cloud.data 00895 length = len(_x) 00896 # - if encoded as a list instead, serialize as bytes instead of string 00897 if type(_x) in [list, tuple]: 00898 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00899 else: 00900 buff.write(struct.pack('<I%ss'%length, length, _x)) 00901 buff.write(_struct_B.pack(self.target.region.cloud.is_dense)) 00902 length = len(self.target.region.mask) 00903 buff.write(_struct_I.pack(length)) 00904 pattern = '<%si'%length 00905 buff.write(struct.pack(pattern, *self.target.region.mask)) 00906 _x = self 00907 buff.write(_struct_3I.pack(_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs)) 00908 _x = self.target.region.image.header.frame_id 00909 length = len(_x) 00910 buff.write(struct.pack('<I%ss'%length, length, _x)) 00911 _x = self 00912 buff.write(_struct_2I.pack(_x.target.region.image.height, _x.target.region.image.width)) 00913 _x = self.target.region.image.encoding 00914 length = len(_x) 00915 buff.write(struct.pack('<I%ss'%length, length, _x)) 00916 _x = self 00917 buff.write(_struct_BI.pack(_x.target.region.image.is_bigendian, _x.target.region.image.step)) 00918 _x = self.target.region.image.data 00919 length = len(_x) 00920 # - if encoded as a list instead, serialize as bytes instead of string 00921 if type(_x) in [list, tuple]: 00922 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00923 else: 00924 buff.write(struct.pack('<I%ss'%length, length, _x)) 00925 _x = self 00926 buff.write(_struct_3I.pack(_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs)) 00927 _x = self.target.region.disparity_image.header.frame_id 00928 length = len(_x) 00929 buff.write(struct.pack('<I%ss'%length, length, _x)) 00930 _x = self 00931 buff.write(_struct_2I.pack(_x.target.region.disparity_image.height, _x.target.region.disparity_image.width)) 00932 _x = self.target.region.disparity_image.encoding 00933 length = len(_x) 00934 buff.write(struct.pack('<I%ss'%length, length, _x)) 00935 _x = self 00936 buff.write(_struct_BI.pack(_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step)) 00937 _x = self.target.region.disparity_image.data 00938 length = len(_x) 00939 # - if encoded as a list instead, serialize as bytes instead of string 00940 if type(_x) in [list, tuple]: 00941 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00942 else: 00943 buff.write(struct.pack('<I%ss'%length, length, _x)) 00944 _x = self 00945 buff.write(_struct_3I.pack(_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs)) 00946 _x = self.target.region.cam_info.header.frame_id 00947 length = len(_x) 00948 buff.write(struct.pack('<I%ss'%length, length, _x)) 00949 _x = self 00950 buff.write(_struct_2I.pack(_x.target.region.cam_info.height, _x.target.region.cam_info.width)) 00951 _x = self.target.region.cam_info.distortion_model 00952 length = len(_x) 00953 buff.write(struct.pack('<I%ss'%length, length, _x)) 00954 length = len(self.target.region.cam_info.D) 00955 buff.write(_struct_I.pack(length)) 00956 pattern = '<%sd'%length 00957 buff.write(struct.pack(pattern, *self.target.region.cam_info.D)) 00958 buff.write(_struct_9d.pack(*self.target.region.cam_info.K)) 00959 buff.write(_struct_9d.pack(*self.target.region.cam_info.R)) 00960 buff.write(_struct_12d.pack(*self.target.region.cam_info.P)) 00961 _x = self 00962 buff.write(_struct_6IB3I.pack(_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs)) 00963 _x = self.target.region.roi_box_pose.header.frame_id 00964 length = len(_x) 00965 buff.write(struct.pack('<I%ss'%length, length, _x)) 00966 _x = self 00967 buff.write(_struct_10d.pack(_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z)) 00968 _x = self.target.collision_name 00969 length = len(_x) 00970 buff.write(struct.pack('<I%ss'%length, length, _x)) 00971 length = len(self.desired_grasps) 00972 buff.write(_struct_I.pack(length)) 00973 for val1 in self.desired_grasps: 00974 _v7 = val1.pre_grasp_posture 00975 _v8 = _v7.header 00976 buff.write(_struct_I.pack(_v8.seq)) 00977 _v9 = _v8.stamp 00978 _x = _v9 00979 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00980 _x = _v8.frame_id 00981 length = len(_x) 00982 buff.write(struct.pack('<I%ss'%length, length, _x)) 00983 length = len(_v7.name) 00984 buff.write(_struct_I.pack(length)) 00985 for val3 in _v7.name: 00986 length = len(val3) 00987 buff.write(struct.pack('<I%ss'%length, length, val3)) 00988 length = len(_v7.position) 00989 buff.write(_struct_I.pack(length)) 00990 pattern = '<%sd'%length 00991 buff.write(struct.pack(pattern, *_v7.position)) 00992 length = len(_v7.velocity) 00993 buff.write(_struct_I.pack(length)) 00994 pattern = '<%sd'%length 00995 buff.write(struct.pack(pattern, *_v7.velocity)) 00996 length = len(_v7.effort) 00997 buff.write(_struct_I.pack(length)) 00998 pattern = '<%sd'%length 00999 buff.write(struct.pack(pattern, *_v7.effort)) 01000 _v10 = val1.grasp_posture 01001 _v11 = _v10.header 01002 buff.write(_struct_I.pack(_v11.seq)) 01003 _v12 = _v11.stamp 01004 _x = _v12 01005 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01006 _x = _v11.frame_id 01007 length = len(_x) 01008 buff.write(struct.pack('<I%ss'%length, length, _x)) 01009 length = len(_v10.name) 01010 buff.write(_struct_I.pack(length)) 01011 for val3 in _v10.name: 01012 length = len(val3) 01013 buff.write(struct.pack('<I%ss'%length, length, val3)) 01014 length = len(_v10.position) 01015 buff.write(_struct_I.pack(length)) 01016 pattern = '<%sd'%length 01017 buff.write(struct.pack(pattern, *_v10.position)) 01018 length = len(_v10.velocity) 01019 buff.write(_struct_I.pack(length)) 01020 pattern = '<%sd'%length 01021 buff.write(struct.pack(pattern, *_v10.velocity)) 01022 length = len(_v10.effort) 01023 buff.write(_struct_I.pack(length)) 01024 pattern = '<%sd'%length 01025 buff.write(struct.pack(pattern, *_v10.effort)) 01026 _v13 = val1.grasp_pose 01027 _v14 = _v13.position 01028 _x = _v14 01029 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01030 _v15 = _v13.orientation 01031 _x = _v15 01032 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01033 _x = val1 01034 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 01035 length = len(val1.moved_obstacles) 01036 buff.write(_struct_I.pack(length)) 01037 for val2 in val1.moved_obstacles: 01038 _x = val2.reference_frame_id 01039 length = len(_x) 01040 buff.write(struct.pack('<I%ss'%length, length, _x)) 01041 length = len(val2.potential_models) 01042 buff.write(_struct_I.pack(length)) 01043 for val3 in val2.potential_models: 01044 buff.write(_struct_i.pack(val3.model_id)) 01045 _v16 = val3.pose 01046 _v17 = _v16.header 01047 buff.write(_struct_I.pack(_v17.seq)) 01048 _v18 = _v17.stamp 01049 _x = _v18 01050 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01051 _x = _v17.frame_id 01052 length = len(_x) 01053 buff.write(struct.pack('<I%ss'%length, length, _x)) 01054 _v19 = _v16.pose 01055 _v20 = _v19.position 01056 _x = _v20 01057 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01058 _v21 = _v19.orientation 01059 _x = _v21 01060 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01061 buff.write(_struct_f.pack(val3.confidence)) 01062 _x = val3.detector_name 01063 length = len(_x) 01064 buff.write(struct.pack('<I%ss'%length, length, _x)) 01065 _v22 = val2.cluster 01066 _v23 = _v22.header 01067 buff.write(_struct_I.pack(_v23.seq)) 01068 _v24 = _v23.stamp 01069 _x = _v24 01070 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01071 _x = _v23.frame_id 01072 length = len(_x) 01073 buff.write(struct.pack('<I%ss'%length, length, _x)) 01074 length = len(_v22.points) 01075 buff.write(_struct_I.pack(length)) 01076 for val4 in _v22.points: 01077 _x = val4 01078 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01079 length = len(_v22.channels) 01080 buff.write(_struct_I.pack(length)) 01081 for val4 in _v22.channels: 01082 _x = val4.name 01083 length = len(_x) 01084 buff.write(struct.pack('<I%ss'%length, length, _x)) 01085 length = len(val4.values) 01086 buff.write(_struct_I.pack(length)) 01087 pattern = '<%sf'%length 01088 buff.write(struct.pack(pattern, *val4.values)) 01089 _v25 = val2.region 01090 _v26 = _v25.cloud 01091 _v27 = _v26.header 01092 buff.write(_struct_I.pack(_v27.seq)) 01093 _v28 = _v27.stamp 01094 _x = _v28 01095 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01096 _x = _v27.frame_id 01097 length = len(_x) 01098 buff.write(struct.pack('<I%ss'%length, length, _x)) 01099 _x = _v26 01100 buff.write(_struct_2I.pack(_x.height, _x.width)) 01101 length = len(_v26.fields) 01102 buff.write(_struct_I.pack(length)) 01103 for val5 in _v26.fields: 01104 _x = val5.name 01105 length = len(_x) 01106 buff.write(struct.pack('<I%ss'%length, length, _x)) 01107 _x = val5 01108 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01109 _x = _v26 01110 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01111 _x = _v26.data 01112 length = len(_x) 01113 # - if encoded as a list instead, serialize as bytes instead of string 01114 if type(_x) in [list, tuple]: 01115 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01116 else: 01117 buff.write(struct.pack('<I%ss'%length, length, _x)) 01118 buff.write(_struct_B.pack(_v26.is_dense)) 01119 length = len(_v25.mask) 01120 buff.write(_struct_I.pack(length)) 01121 pattern = '<%si'%length 01122 buff.write(struct.pack(pattern, *_v25.mask)) 01123 _v29 = _v25.image 01124 _v30 = _v29.header 01125 buff.write(_struct_I.pack(_v30.seq)) 01126 _v31 = _v30.stamp 01127 _x = _v31 01128 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01129 _x = _v30.frame_id 01130 length = len(_x) 01131 buff.write(struct.pack('<I%ss'%length, length, _x)) 01132 _x = _v29 01133 buff.write(_struct_2I.pack(_x.height, _x.width)) 01134 _x = _v29.encoding 01135 length = len(_x) 01136 buff.write(struct.pack('<I%ss'%length, length, _x)) 01137 _x = _v29 01138 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01139 _x = _v29.data 01140 length = len(_x) 01141 # - if encoded as a list instead, serialize as bytes instead of string 01142 if type(_x) in [list, tuple]: 01143 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01144 else: 01145 buff.write(struct.pack('<I%ss'%length, length, _x)) 01146 _v32 = _v25.disparity_image 01147 _v33 = _v32.header 01148 buff.write(_struct_I.pack(_v33.seq)) 01149 _v34 = _v33.stamp 01150 _x = _v34 01151 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01152 _x = _v33.frame_id 01153 length = len(_x) 01154 buff.write(struct.pack('<I%ss'%length, length, _x)) 01155 _x = _v32 01156 buff.write(_struct_2I.pack(_x.height, _x.width)) 01157 _x = _v32.encoding 01158 length = len(_x) 01159 buff.write(struct.pack('<I%ss'%length, length, _x)) 01160 _x = _v32 01161 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01162 _x = _v32.data 01163 length = len(_x) 01164 # - if encoded as a list instead, serialize as bytes instead of string 01165 if type(_x) in [list, tuple]: 01166 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01167 else: 01168 buff.write(struct.pack('<I%ss'%length, length, _x)) 01169 _v35 = _v25.cam_info 01170 _v36 = _v35.header 01171 buff.write(_struct_I.pack(_v36.seq)) 01172 _v37 = _v36.stamp 01173 _x = _v37 01174 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01175 _x = _v36.frame_id 01176 length = len(_x) 01177 buff.write(struct.pack('<I%ss'%length, length, _x)) 01178 _x = _v35 01179 buff.write(_struct_2I.pack(_x.height, _x.width)) 01180 _x = _v35.distortion_model 01181 length = len(_x) 01182 buff.write(struct.pack('<I%ss'%length, length, _x)) 01183 length = len(_v35.D) 01184 buff.write(_struct_I.pack(length)) 01185 pattern = '<%sd'%length 01186 buff.write(struct.pack(pattern, *_v35.D)) 01187 buff.write(_struct_9d.pack(*_v35.K)) 01188 buff.write(_struct_9d.pack(*_v35.R)) 01189 buff.write(_struct_12d.pack(*_v35.P)) 01190 _x = _v35 01191 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01192 _v38 = _v35.roi 01193 _x = _v38 01194 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01195 _v39 = _v25.roi_box_pose 01196 _v40 = _v39.header 01197 buff.write(_struct_I.pack(_v40.seq)) 01198 _v41 = _v40.stamp 01199 _x = _v41 01200 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01201 _x = _v40.frame_id 01202 length = len(_x) 01203 buff.write(struct.pack('<I%ss'%length, length, _x)) 01204 _v42 = _v39.pose 01205 _v43 = _v42.position 01206 _x = _v43 01207 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01208 _v44 = _v42.orientation 01209 _x = _v44 01210 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01211 _v45 = _v25.roi_box_dims 01212 _x = _v45 01213 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01214 _x = val2.collision_name 01215 length = len(_x) 01216 buff.write(struct.pack('<I%ss'%length, length, _x)) 01217 _x = self 01218 buff.write(_struct_3I.pack(_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs)) 01219 _x = self.lift.direction.header.frame_id 01220 length = len(_x) 01221 buff.write(struct.pack('<I%ss'%length, length, _x)) 01222 _x = self 01223 buff.write(_struct_3d2f.pack(_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance)) 01224 _x = self.collision_object_name 01225 length = len(_x) 01226 buff.write(struct.pack('<I%ss'%length, length, _x)) 01227 _x = self.collision_support_surface_name 01228 length = len(_x) 01229 buff.write(struct.pack('<I%ss'%length, length, _x)) 01230 _x = self 01231 buff.write(_struct_5B.pack(_x.allow_gripper_support_collision, _x.use_reactive_execution, _x.use_reactive_lift, _x.only_perform_feasibility_test, _x.ignore_collisions)) 01232 length = len(self.path_constraints.joint_constraints) 01233 buff.write(_struct_I.pack(length)) 01234 for val1 in self.path_constraints.joint_constraints: 01235 _x = val1.joint_name 01236 length = len(_x) 01237 buff.write(struct.pack('<I%ss'%length, length, _x)) 01238 _x = val1 01239 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 01240 length = len(self.path_constraints.position_constraints) 01241 buff.write(_struct_I.pack(length)) 01242 for val1 in self.path_constraints.position_constraints: 01243 _v46 = val1.header 01244 buff.write(_struct_I.pack(_v46.seq)) 01245 _v47 = _v46.stamp 01246 _x = _v47 01247 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01248 _x = _v46.frame_id 01249 length = len(_x) 01250 buff.write(struct.pack('<I%ss'%length, length, _x)) 01251 _x = val1.link_name 01252 length = len(_x) 01253 buff.write(struct.pack('<I%ss'%length, length, _x)) 01254 _v48 = val1.target_point_offset 01255 _x = _v48 01256 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01257 _v49 = val1.position 01258 _x = _v49 01259 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01260 _v50 = val1.constraint_region_shape 01261 buff.write(_struct_b.pack(_v50.type)) 01262 length = len(_v50.dimensions) 01263 buff.write(_struct_I.pack(length)) 01264 pattern = '<%sd'%length 01265 buff.write(struct.pack(pattern, *_v50.dimensions)) 01266 length = len(_v50.triangles) 01267 buff.write(_struct_I.pack(length)) 01268 pattern = '<%si'%length 01269 buff.write(struct.pack(pattern, *_v50.triangles)) 01270 length = len(_v50.vertices) 01271 buff.write(_struct_I.pack(length)) 01272 for val3 in _v50.vertices: 01273 _x = val3 01274 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01275 _v51 = val1.constraint_region_orientation 01276 _x = _v51 01277 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01278 buff.write(_struct_d.pack(val1.weight)) 01279 length = len(self.path_constraints.orientation_constraints) 01280 buff.write(_struct_I.pack(length)) 01281 for val1 in self.path_constraints.orientation_constraints: 01282 _v52 = val1.header 01283 buff.write(_struct_I.pack(_v52.seq)) 01284 _v53 = _v52.stamp 01285 _x = _v53 01286 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01287 _x = _v52.frame_id 01288 length = len(_x) 01289 buff.write(struct.pack('<I%ss'%length, length, _x)) 01290 _x = val1.link_name 01291 length = len(_x) 01292 buff.write(struct.pack('<I%ss'%length, length, _x)) 01293 buff.write(_struct_i.pack(val1.type)) 01294 _v54 = val1.orientation 01295 _x = _v54 01296 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01297 _x = val1 01298 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 01299 length = len(self.path_constraints.visibility_constraints) 01300 buff.write(_struct_I.pack(length)) 01301 for val1 in self.path_constraints.visibility_constraints: 01302 _v55 = val1.header 01303 buff.write(_struct_I.pack(_v55.seq)) 01304 _v56 = _v55.stamp 01305 _x = _v56 01306 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01307 _x = _v55.frame_id 01308 length = len(_x) 01309 buff.write(struct.pack('<I%ss'%length, length, _x)) 01310 _v57 = val1.target 01311 _v58 = _v57.header 01312 buff.write(_struct_I.pack(_v58.seq)) 01313 _v59 = _v58.stamp 01314 _x = _v59 01315 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01316 _x = _v58.frame_id 01317 length = len(_x) 01318 buff.write(struct.pack('<I%ss'%length, length, _x)) 01319 _v60 = _v57.point 01320 _x = _v60 01321 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01322 _v61 = val1.sensor_pose 01323 _v62 = _v61.header 01324 buff.write(_struct_I.pack(_v62.seq)) 01325 _v63 = _v62.stamp 01326 _x = _v63 01327 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01328 _x = _v62.frame_id 01329 length = len(_x) 01330 buff.write(struct.pack('<I%ss'%length, length, _x)) 01331 _v64 = _v61.pose 01332 _v65 = _v64.position 01333 _x = _v65 01334 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01335 _v66 = _v64.orientation 01336 _x = _v66 01337 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01338 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01339 length = len(self.additional_collision_operations.collision_operations) 01340 buff.write(_struct_I.pack(length)) 01341 for val1 in self.additional_collision_operations.collision_operations: 01342 _x = val1.object1 01343 length = len(_x) 01344 buff.write(struct.pack('<I%ss'%length, length, _x)) 01345 _x = val1.object2 01346 length = len(_x) 01347 buff.write(struct.pack('<I%ss'%length, length, _x)) 01348 _x = val1 01349 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 01350 length = len(self.additional_link_padding) 01351 buff.write(_struct_I.pack(length)) 01352 for val1 in self.additional_link_padding: 01353 _x = val1.link_name 01354 length = len(_x) 01355 buff.write(struct.pack('<I%ss'%length, length, _x)) 01356 buff.write(_struct_d.pack(val1.padding)) 01357 length = len(self.movable_obstacles) 01358 buff.write(_struct_I.pack(length)) 01359 for val1 in self.movable_obstacles: 01360 _x = val1.reference_frame_id 01361 length = len(_x) 01362 buff.write(struct.pack('<I%ss'%length, length, _x)) 01363 length = len(val1.potential_models) 01364 buff.write(_struct_I.pack(length)) 01365 for val2 in val1.potential_models: 01366 buff.write(_struct_i.pack(val2.model_id)) 01367 _v67 = val2.pose 01368 _v68 = _v67.header 01369 buff.write(_struct_I.pack(_v68.seq)) 01370 _v69 = _v68.stamp 01371 _x = _v69 01372 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01373 _x = _v68.frame_id 01374 length = len(_x) 01375 buff.write(struct.pack('<I%ss'%length, length, _x)) 01376 _v70 = _v67.pose 01377 _v71 = _v70.position 01378 _x = _v71 01379 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01380 _v72 = _v70.orientation 01381 _x = _v72 01382 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01383 buff.write(_struct_f.pack(val2.confidence)) 01384 _x = val2.detector_name 01385 length = len(_x) 01386 buff.write(struct.pack('<I%ss'%length, length, _x)) 01387 _v73 = val1.cluster 01388 _v74 = _v73.header 01389 buff.write(_struct_I.pack(_v74.seq)) 01390 _v75 = _v74.stamp 01391 _x = _v75 01392 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01393 _x = _v74.frame_id 01394 length = len(_x) 01395 buff.write(struct.pack('<I%ss'%length, length, _x)) 01396 length = len(_v73.points) 01397 buff.write(_struct_I.pack(length)) 01398 for val3 in _v73.points: 01399 _x = val3 01400 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01401 length = len(_v73.channels) 01402 buff.write(_struct_I.pack(length)) 01403 for val3 in _v73.channels: 01404 _x = val3.name 01405 length = len(_x) 01406 buff.write(struct.pack('<I%ss'%length, length, _x)) 01407 length = len(val3.values) 01408 buff.write(_struct_I.pack(length)) 01409 pattern = '<%sf'%length 01410 buff.write(struct.pack(pattern, *val3.values)) 01411 _v76 = val1.region 01412 _v77 = _v76.cloud 01413 _v78 = _v77.header 01414 buff.write(_struct_I.pack(_v78.seq)) 01415 _v79 = _v78.stamp 01416 _x = _v79 01417 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01418 _x = _v78.frame_id 01419 length = len(_x) 01420 buff.write(struct.pack('<I%ss'%length, length, _x)) 01421 _x = _v77 01422 buff.write(_struct_2I.pack(_x.height, _x.width)) 01423 length = len(_v77.fields) 01424 buff.write(_struct_I.pack(length)) 01425 for val4 in _v77.fields: 01426 _x = val4.name 01427 length = len(_x) 01428 buff.write(struct.pack('<I%ss'%length, length, _x)) 01429 _x = val4 01430 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01431 _x = _v77 01432 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01433 _x = _v77.data 01434 length = len(_x) 01435 # - if encoded as a list instead, serialize as bytes instead of string 01436 if type(_x) in [list, tuple]: 01437 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01438 else: 01439 buff.write(struct.pack('<I%ss'%length, length, _x)) 01440 buff.write(_struct_B.pack(_v77.is_dense)) 01441 length = len(_v76.mask) 01442 buff.write(_struct_I.pack(length)) 01443 pattern = '<%si'%length 01444 buff.write(struct.pack(pattern, *_v76.mask)) 01445 _v80 = _v76.image 01446 _v81 = _v80.header 01447 buff.write(_struct_I.pack(_v81.seq)) 01448 _v82 = _v81.stamp 01449 _x = _v82 01450 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01451 _x = _v81.frame_id 01452 length = len(_x) 01453 buff.write(struct.pack('<I%ss'%length, length, _x)) 01454 _x = _v80 01455 buff.write(_struct_2I.pack(_x.height, _x.width)) 01456 _x = _v80.encoding 01457 length = len(_x) 01458 buff.write(struct.pack('<I%ss'%length, length, _x)) 01459 _x = _v80 01460 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01461 _x = _v80.data 01462 length = len(_x) 01463 # - if encoded as a list instead, serialize as bytes instead of string 01464 if type(_x) in [list, tuple]: 01465 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01466 else: 01467 buff.write(struct.pack('<I%ss'%length, length, _x)) 01468 _v83 = _v76.disparity_image 01469 _v84 = _v83.header 01470 buff.write(_struct_I.pack(_v84.seq)) 01471 _v85 = _v84.stamp 01472 _x = _v85 01473 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01474 _x = _v84.frame_id 01475 length = len(_x) 01476 buff.write(struct.pack('<I%ss'%length, length, _x)) 01477 _x = _v83 01478 buff.write(_struct_2I.pack(_x.height, _x.width)) 01479 _x = _v83.encoding 01480 length = len(_x) 01481 buff.write(struct.pack('<I%ss'%length, length, _x)) 01482 _x = _v83 01483 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01484 _x = _v83.data 01485 length = len(_x) 01486 # - if encoded as a list instead, serialize as bytes instead of string 01487 if type(_x) in [list, tuple]: 01488 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01489 else: 01490 buff.write(struct.pack('<I%ss'%length, length, _x)) 01491 _v86 = _v76.cam_info 01492 _v87 = _v86.header 01493 buff.write(_struct_I.pack(_v87.seq)) 01494 _v88 = _v87.stamp 01495 _x = _v88 01496 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01497 _x = _v87.frame_id 01498 length = len(_x) 01499 buff.write(struct.pack('<I%ss'%length, length, _x)) 01500 _x = _v86 01501 buff.write(_struct_2I.pack(_x.height, _x.width)) 01502 _x = _v86.distortion_model 01503 length = len(_x) 01504 buff.write(struct.pack('<I%ss'%length, length, _x)) 01505 length = len(_v86.D) 01506 buff.write(_struct_I.pack(length)) 01507 pattern = '<%sd'%length 01508 buff.write(struct.pack(pattern, *_v86.D)) 01509 buff.write(_struct_9d.pack(*_v86.K)) 01510 buff.write(_struct_9d.pack(*_v86.R)) 01511 buff.write(_struct_12d.pack(*_v86.P)) 01512 _x = _v86 01513 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01514 _v89 = _v86.roi 01515 _x = _v89 01516 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01517 _v90 = _v76.roi_box_pose 01518 _v91 = _v90.header 01519 buff.write(_struct_I.pack(_v91.seq)) 01520 _v92 = _v91.stamp 01521 _x = _v92 01522 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01523 _x = _v91.frame_id 01524 length = len(_x) 01525 buff.write(struct.pack('<I%ss'%length, length, _x)) 01526 _v93 = _v90.pose 01527 _v94 = _v93.position 01528 _x = _v94 01529 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01530 _v95 = _v93.orientation 01531 _x = _v95 01532 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01533 _v96 = _v76.roi_box_dims 01534 _x = _v96 01535 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01536 _x = val1.collision_name 01537 length = len(_x) 01538 buff.write(struct.pack('<I%ss'%length, length, _x)) 01539 buff.write(_struct_f.pack(self.max_contact_force)) 01540 except struct.error as se: self._check_types(se) 01541 except TypeError as te: self._check_types(te) 01542 01543 def deserialize(self, str): 01544 """ 01545 unpack serialized message in str into this message instance 01546 @param str: byte array of serialized message 01547 @type str: str 01548 """ 01549 try: 01550 if self.target is None: 01551 self.target = object_manipulation_msgs.msg.GraspableObject() 01552 if self.lift is None: 01553 self.lift = object_manipulation_msgs.msg.GripperTranslation() 01554 if self.path_constraints is None: 01555 self.path_constraints = arm_navigation_msgs.msg.Constraints() 01556 if self.additional_collision_operations is None: 01557 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations() 01558 end = 0 01559 start = end 01560 end += 4 01561 (length,) = _struct_I.unpack(str[start:end]) 01562 start = end 01563 end += length 01564 self.arm_name = str[start:end] 01565 start = end 01566 end += 4 01567 (length,) = _struct_I.unpack(str[start:end]) 01568 start = end 01569 end += length 01570 self.target.reference_frame_id = str[start:end] 01571 start = end 01572 end += 4 01573 (length,) = _struct_I.unpack(str[start:end]) 01574 self.target.potential_models = [] 01575 for i in range(0, length): 01576 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 01577 start = end 01578 end += 4 01579 (val1.model_id,) = _struct_i.unpack(str[start:end]) 01580 _v97 = val1.pose 01581 _v98 = _v97.header 01582 start = end 01583 end += 4 01584 (_v98.seq,) = _struct_I.unpack(str[start:end]) 01585 _v99 = _v98.stamp 01586 _x = _v99 01587 start = end 01588 end += 8 01589 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01590 start = end 01591 end += 4 01592 (length,) = _struct_I.unpack(str[start:end]) 01593 start = end 01594 end += length 01595 _v98.frame_id = str[start:end] 01596 _v100 = _v97.pose 01597 _v101 = _v100.position 01598 _x = _v101 01599 start = end 01600 end += 24 01601 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01602 _v102 = _v100.orientation 01603 _x = _v102 01604 start = end 01605 end += 32 01606 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01607 start = end 01608 end += 4 01609 (val1.confidence,) = _struct_f.unpack(str[start:end]) 01610 start = end 01611 end += 4 01612 (length,) = _struct_I.unpack(str[start:end]) 01613 start = end 01614 end += length 01615 val1.detector_name = str[start:end] 01616 self.target.potential_models.append(val1) 01617 _x = self 01618 start = end 01619 end += 12 01620 (_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01621 start = end 01622 end += 4 01623 (length,) = _struct_I.unpack(str[start:end]) 01624 start = end 01625 end += length 01626 self.target.cluster.header.frame_id = str[start:end] 01627 start = end 01628 end += 4 01629 (length,) = _struct_I.unpack(str[start:end]) 01630 self.target.cluster.points = [] 01631 for i in range(0, length): 01632 val1 = geometry_msgs.msg.Point32() 01633 _x = val1 01634 start = end 01635 end += 12 01636 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01637 self.target.cluster.points.append(val1) 01638 start = end 01639 end += 4 01640 (length,) = _struct_I.unpack(str[start:end]) 01641 self.target.cluster.channels = [] 01642 for i in range(0, length): 01643 val1 = sensor_msgs.msg.ChannelFloat32() 01644 start = end 01645 end += 4 01646 (length,) = _struct_I.unpack(str[start:end]) 01647 start = end 01648 end += length 01649 val1.name = str[start:end] 01650 start = end 01651 end += 4 01652 (length,) = _struct_I.unpack(str[start:end]) 01653 pattern = '<%sf'%length 01654 start = end 01655 end += struct.calcsize(pattern) 01656 val1.values = struct.unpack(pattern, str[start:end]) 01657 self.target.cluster.channels.append(val1) 01658 _x = self 01659 start = end 01660 end += 12 01661 (_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01662 start = end 01663 end += 4 01664 (length,) = _struct_I.unpack(str[start:end]) 01665 start = end 01666 end += length 01667 self.target.region.cloud.header.frame_id = str[start:end] 01668 _x = self 01669 start = end 01670 end += 8 01671 (_x.target.region.cloud.height, _x.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 01672 start = end 01673 end += 4 01674 (length,) = _struct_I.unpack(str[start:end]) 01675 self.target.region.cloud.fields = [] 01676 for i in range(0, length): 01677 val1 = sensor_msgs.msg.PointField() 01678 start = end 01679 end += 4 01680 (length,) = _struct_I.unpack(str[start:end]) 01681 start = end 01682 end += length 01683 val1.name = str[start:end] 01684 _x = val1 01685 start = end 01686 end += 9 01687 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01688 self.target.region.cloud.fields.append(val1) 01689 _x = self 01690 start = end 01691 end += 9 01692 (_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 01693 self.target.region.cloud.is_bigendian = bool(self.target.region.cloud.is_bigendian) 01694 start = end 01695 end += 4 01696 (length,) = _struct_I.unpack(str[start:end]) 01697 start = end 01698 end += length 01699 self.target.region.cloud.data = str[start:end] 01700 start = end 01701 end += 1 01702 (self.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 01703 self.target.region.cloud.is_dense = bool(self.target.region.cloud.is_dense) 01704 start = end 01705 end += 4 01706 (length,) = _struct_I.unpack(str[start:end]) 01707 pattern = '<%si'%length 01708 start = end 01709 end += struct.calcsize(pattern) 01710 self.target.region.mask = struct.unpack(pattern, str[start:end]) 01711 _x = self 01712 start = end 01713 end += 12 01714 (_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01715 start = end 01716 end += 4 01717 (length,) = _struct_I.unpack(str[start:end]) 01718 start = end 01719 end += length 01720 self.target.region.image.header.frame_id = str[start:end] 01721 _x = self 01722 start = end 01723 end += 8 01724 (_x.target.region.image.height, _x.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 01725 start = end 01726 end += 4 01727 (length,) = _struct_I.unpack(str[start:end]) 01728 start = end 01729 end += length 01730 self.target.region.image.encoding = str[start:end] 01731 _x = self 01732 start = end 01733 end += 5 01734 (_x.target.region.image.is_bigendian, _x.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 01735 start = end 01736 end += 4 01737 (length,) = _struct_I.unpack(str[start:end]) 01738 start = end 01739 end += length 01740 self.target.region.image.data = str[start:end] 01741 _x = self 01742 start = end 01743 end += 12 01744 (_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01745 start = end 01746 end += 4 01747 (length,) = _struct_I.unpack(str[start:end]) 01748 start = end 01749 end += length 01750 self.target.region.disparity_image.header.frame_id = str[start:end] 01751 _x = self 01752 start = end 01753 end += 8 01754 (_x.target.region.disparity_image.height, _x.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 01755 start = end 01756 end += 4 01757 (length,) = _struct_I.unpack(str[start:end]) 01758 start = end 01759 end += length 01760 self.target.region.disparity_image.encoding = str[start:end] 01761 _x = self 01762 start = end 01763 end += 5 01764 (_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 01765 start = end 01766 end += 4 01767 (length,) = _struct_I.unpack(str[start:end]) 01768 start = end 01769 end += length 01770 self.target.region.disparity_image.data = str[start:end] 01771 _x = self 01772 start = end 01773 end += 12 01774 (_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01775 start = end 01776 end += 4 01777 (length,) = _struct_I.unpack(str[start:end]) 01778 start = end 01779 end += length 01780 self.target.region.cam_info.header.frame_id = str[start:end] 01781 _x = self 01782 start = end 01783 end += 8 01784 (_x.target.region.cam_info.height, _x.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 01785 start = end 01786 end += 4 01787 (length,) = _struct_I.unpack(str[start:end]) 01788 start = end 01789 end += length 01790 self.target.region.cam_info.distortion_model = str[start:end] 01791 start = end 01792 end += 4 01793 (length,) = _struct_I.unpack(str[start:end]) 01794 pattern = '<%sd'%length 01795 start = end 01796 end += struct.calcsize(pattern) 01797 self.target.region.cam_info.D = struct.unpack(pattern, str[start:end]) 01798 start = end 01799 end += 72 01800 self.target.region.cam_info.K = _struct_9d.unpack(str[start:end]) 01801 start = end 01802 end += 72 01803 self.target.region.cam_info.R = _struct_9d.unpack(str[start:end]) 01804 start = end 01805 end += 96 01806 self.target.region.cam_info.P = _struct_12d.unpack(str[start:end]) 01807 _x = self 01808 start = end 01809 end += 37 01810 (_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 01811 self.target.region.cam_info.roi.do_rectify = bool(self.target.region.cam_info.roi.do_rectify) 01812 start = end 01813 end += 4 01814 (length,) = _struct_I.unpack(str[start:end]) 01815 start = end 01816 end += length 01817 self.target.region.roi_box_pose.header.frame_id = str[start:end] 01818 _x = self 01819 start = end 01820 end += 80 01821 (_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 01822 start = end 01823 end += 4 01824 (length,) = _struct_I.unpack(str[start:end]) 01825 start = end 01826 end += length 01827 self.target.collision_name = str[start:end] 01828 start = end 01829 end += 4 01830 (length,) = _struct_I.unpack(str[start:end]) 01831 self.desired_grasps = [] 01832 for i in range(0, length): 01833 val1 = object_manipulation_msgs.msg.Grasp() 01834 _v103 = val1.pre_grasp_posture 01835 _v104 = _v103.header 01836 start = end 01837 end += 4 01838 (_v104.seq,) = _struct_I.unpack(str[start:end]) 01839 _v105 = _v104.stamp 01840 _x = _v105 01841 start = end 01842 end += 8 01843 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01844 start = end 01845 end += 4 01846 (length,) = _struct_I.unpack(str[start:end]) 01847 start = end 01848 end += length 01849 _v104.frame_id = str[start:end] 01850 start = end 01851 end += 4 01852 (length,) = _struct_I.unpack(str[start:end]) 01853 _v103.name = [] 01854 for i in range(0, length): 01855 start = end 01856 end += 4 01857 (length,) = _struct_I.unpack(str[start:end]) 01858 start = end 01859 end += length 01860 val3 = str[start:end] 01861 _v103.name.append(val3) 01862 start = end 01863 end += 4 01864 (length,) = _struct_I.unpack(str[start:end]) 01865 pattern = '<%sd'%length 01866 start = end 01867 end += struct.calcsize(pattern) 01868 _v103.position = struct.unpack(pattern, str[start:end]) 01869 start = end 01870 end += 4 01871 (length,) = _struct_I.unpack(str[start:end]) 01872 pattern = '<%sd'%length 01873 start = end 01874 end += struct.calcsize(pattern) 01875 _v103.velocity = struct.unpack(pattern, str[start:end]) 01876 start = end 01877 end += 4 01878 (length,) = _struct_I.unpack(str[start:end]) 01879 pattern = '<%sd'%length 01880 start = end 01881 end += struct.calcsize(pattern) 01882 _v103.effort = struct.unpack(pattern, str[start:end]) 01883 _v106 = val1.grasp_posture 01884 _v107 = _v106.header 01885 start = end 01886 end += 4 01887 (_v107.seq,) = _struct_I.unpack(str[start:end]) 01888 _v108 = _v107.stamp 01889 _x = _v108 01890 start = end 01891 end += 8 01892 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01893 start = end 01894 end += 4 01895 (length,) = _struct_I.unpack(str[start:end]) 01896 start = end 01897 end += length 01898 _v107.frame_id = str[start:end] 01899 start = end 01900 end += 4 01901 (length,) = _struct_I.unpack(str[start:end]) 01902 _v106.name = [] 01903 for i in range(0, length): 01904 start = end 01905 end += 4 01906 (length,) = _struct_I.unpack(str[start:end]) 01907 start = end 01908 end += length 01909 val3 = str[start:end] 01910 _v106.name.append(val3) 01911 start = end 01912 end += 4 01913 (length,) = _struct_I.unpack(str[start:end]) 01914 pattern = '<%sd'%length 01915 start = end 01916 end += struct.calcsize(pattern) 01917 _v106.position = struct.unpack(pattern, str[start:end]) 01918 start = end 01919 end += 4 01920 (length,) = _struct_I.unpack(str[start:end]) 01921 pattern = '<%sd'%length 01922 start = end 01923 end += struct.calcsize(pattern) 01924 _v106.velocity = struct.unpack(pattern, str[start:end]) 01925 start = end 01926 end += 4 01927 (length,) = _struct_I.unpack(str[start:end]) 01928 pattern = '<%sd'%length 01929 start = end 01930 end += struct.calcsize(pattern) 01931 _v106.effort = struct.unpack(pattern, str[start:end]) 01932 _v109 = val1.grasp_pose 01933 _v110 = _v109.position 01934 _x = _v110 01935 start = end 01936 end += 24 01937 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01938 _v111 = _v109.orientation 01939 _x = _v111 01940 start = end 01941 end += 32 01942 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01943 _x = val1 01944 start = end 01945 end += 17 01946 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 01947 val1.cluster_rep = bool(val1.cluster_rep) 01948 start = end 01949 end += 4 01950 (length,) = _struct_I.unpack(str[start:end]) 01951 val1.moved_obstacles = [] 01952 for i in range(0, length): 01953 val2 = object_manipulation_msgs.msg.GraspableObject() 01954 start = end 01955 end += 4 01956 (length,) = _struct_I.unpack(str[start:end]) 01957 start = end 01958 end += length 01959 val2.reference_frame_id = str[start:end] 01960 start = end 01961 end += 4 01962 (length,) = _struct_I.unpack(str[start:end]) 01963 val2.potential_models = [] 01964 for i in range(0, length): 01965 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 01966 start = end 01967 end += 4 01968 (val3.model_id,) = _struct_i.unpack(str[start:end]) 01969 _v112 = val3.pose 01970 _v113 = _v112.header 01971 start = end 01972 end += 4 01973 (_v113.seq,) = _struct_I.unpack(str[start:end]) 01974 _v114 = _v113.stamp 01975 _x = _v114 01976 start = end 01977 end += 8 01978 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01979 start = end 01980 end += 4 01981 (length,) = _struct_I.unpack(str[start:end]) 01982 start = end 01983 end += length 01984 _v113.frame_id = str[start:end] 01985 _v115 = _v112.pose 01986 _v116 = _v115.position 01987 _x = _v116 01988 start = end 01989 end += 24 01990 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01991 _v117 = _v115.orientation 01992 _x = _v117 01993 start = end 01994 end += 32 01995 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01996 start = end 01997 end += 4 01998 (val3.confidence,) = _struct_f.unpack(str[start:end]) 01999 start = end 02000 end += 4 02001 (length,) = _struct_I.unpack(str[start:end]) 02002 start = end 02003 end += length 02004 val3.detector_name = str[start:end] 02005 val2.potential_models.append(val3) 02006 _v118 = val2.cluster 02007 _v119 = _v118.header 02008 start = end 02009 end += 4 02010 (_v119.seq,) = _struct_I.unpack(str[start:end]) 02011 _v120 = _v119.stamp 02012 _x = _v120 02013 start = end 02014 end += 8 02015 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02016 start = end 02017 end += 4 02018 (length,) = _struct_I.unpack(str[start:end]) 02019 start = end 02020 end += length 02021 _v119.frame_id = str[start:end] 02022 start = end 02023 end += 4 02024 (length,) = _struct_I.unpack(str[start:end]) 02025 _v118.points = [] 02026 for i in range(0, length): 02027 val4 = geometry_msgs.msg.Point32() 02028 _x = val4 02029 start = end 02030 end += 12 02031 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02032 _v118.points.append(val4) 02033 start = end 02034 end += 4 02035 (length,) = _struct_I.unpack(str[start:end]) 02036 _v118.channels = [] 02037 for i in range(0, length): 02038 val4 = sensor_msgs.msg.ChannelFloat32() 02039 start = end 02040 end += 4 02041 (length,) = _struct_I.unpack(str[start:end]) 02042 start = end 02043 end += length 02044 val4.name = str[start:end] 02045 start = end 02046 end += 4 02047 (length,) = _struct_I.unpack(str[start:end]) 02048 pattern = '<%sf'%length 02049 start = end 02050 end += struct.calcsize(pattern) 02051 val4.values = struct.unpack(pattern, str[start:end]) 02052 _v118.channels.append(val4) 02053 _v121 = val2.region 02054 _v122 = _v121.cloud 02055 _v123 = _v122.header 02056 start = end 02057 end += 4 02058 (_v123.seq,) = _struct_I.unpack(str[start:end]) 02059 _v124 = _v123.stamp 02060 _x = _v124 02061 start = end 02062 end += 8 02063 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02064 start = end 02065 end += 4 02066 (length,) = _struct_I.unpack(str[start:end]) 02067 start = end 02068 end += length 02069 _v123.frame_id = str[start:end] 02070 _x = _v122 02071 start = end 02072 end += 8 02073 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02074 start = end 02075 end += 4 02076 (length,) = _struct_I.unpack(str[start:end]) 02077 _v122.fields = [] 02078 for i in range(0, length): 02079 val5 = sensor_msgs.msg.PointField() 02080 start = end 02081 end += 4 02082 (length,) = _struct_I.unpack(str[start:end]) 02083 start = end 02084 end += length 02085 val5.name = str[start:end] 02086 _x = val5 02087 start = end 02088 end += 9 02089 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02090 _v122.fields.append(val5) 02091 _x = _v122 02092 start = end 02093 end += 9 02094 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 02095 _v122.is_bigendian = bool(_v122.is_bigendian) 02096 start = end 02097 end += 4 02098 (length,) = _struct_I.unpack(str[start:end]) 02099 start = end 02100 end += length 02101 _v122.data = str[start:end] 02102 start = end 02103 end += 1 02104 (_v122.is_dense,) = _struct_B.unpack(str[start:end]) 02105 _v122.is_dense = bool(_v122.is_dense) 02106 start = end 02107 end += 4 02108 (length,) = _struct_I.unpack(str[start:end]) 02109 pattern = '<%si'%length 02110 start = end 02111 end += struct.calcsize(pattern) 02112 _v121.mask = struct.unpack(pattern, str[start:end]) 02113 _v125 = _v121.image 02114 _v126 = _v125.header 02115 start = end 02116 end += 4 02117 (_v126.seq,) = _struct_I.unpack(str[start:end]) 02118 _v127 = _v126.stamp 02119 _x = _v127 02120 start = end 02121 end += 8 02122 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02123 start = end 02124 end += 4 02125 (length,) = _struct_I.unpack(str[start:end]) 02126 start = end 02127 end += length 02128 _v126.frame_id = str[start:end] 02129 _x = _v125 02130 start = end 02131 end += 8 02132 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02133 start = end 02134 end += 4 02135 (length,) = _struct_I.unpack(str[start:end]) 02136 start = end 02137 end += length 02138 _v125.encoding = str[start:end] 02139 _x = _v125 02140 start = end 02141 end += 5 02142 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02143 start = end 02144 end += 4 02145 (length,) = _struct_I.unpack(str[start:end]) 02146 start = end 02147 end += length 02148 _v125.data = str[start:end] 02149 _v128 = _v121.disparity_image 02150 _v129 = _v128.header 02151 start = end 02152 end += 4 02153 (_v129.seq,) = _struct_I.unpack(str[start:end]) 02154 _v130 = _v129.stamp 02155 _x = _v130 02156 start = end 02157 end += 8 02158 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02159 start = end 02160 end += 4 02161 (length,) = _struct_I.unpack(str[start:end]) 02162 start = end 02163 end += length 02164 _v129.frame_id = str[start:end] 02165 _x = _v128 02166 start = end 02167 end += 8 02168 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02169 start = end 02170 end += 4 02171 (length,) = _struct_I.unpack(str[start:end]) 02172 start = end 02173 end += length 02174 _v128.encoding = str[start:end] 02175 _x = _v128 02176 start = end 02177 end += 5 02178 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02179 start = end 02180 end += 4 02181 (length,) = _struct_I.unpack(str[start:end]) 02182 start = end 02183 end += length 02184 _v128.data = str[start:end] 02185 _v131 = _v121.cam_info 02186 _v132 = _v131.header 02187 start = end 02188 end += 4 02189 (_v132.seq,) = _struct_I.unpack(str[start:end]) 02190 _v133 = _v132.stamp 02191 _x = _v133 02192 start = end 02193 end += 8 02194 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02195 start = end 02196 end += 4 02197 (length,) = _struct_I.unpack(str[start:end]) 02198 start = end 02199 end += length 02200 _v132.frame_id = str[start:end] 02201 _x = _v131 02202 start = end 02203 end += 8 02204 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02205 start = end 02206 end += 4 02207 (length,) = _struct_I.unpack(str[start:end]) 02208 start = end 02209 end += length 02210 _v131.distortion_model = str[start:end] 02211 start = end 02212 end += 4 02213 (length,) = _struct_I.unpack(str[start:end]) 02214 pattern = '<%sd'%length 02215 start = end 02216 end += struct.calcsize(pattern) 02217 _v131.D = struct.unpack(pattern, str[start:end]) 02218 start = end 02219 end += 72 02220 _v131.K = _struct_9d.unpack(str[start:end]) 02221 start = end 02222 end += 72 02223 _v131.R = _struct_9d.unpack(str[start:end]) 02224 start = end 02225 end += 96 02226 _v131.P = _struct_12d.unpack(str[start:end]) 02227 _x = _v131 02228 start = end 02229 end += 8 02230 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02231 _v134 = _v131.roi 02232 _x = _v134 02233 start = end 02234 end += 17 02235 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02236 _v134.do_rectify = bool(_v134.do_rectify) 02237 _v135 = _v121.roi_box_pose 02238 _v136 = _v135.header 02239 start = end 02240 end += 4 02241 (_v136.seq,) = _struct_I.unpack(str[start:end]) 02242 _v137 = _v136.stamp 02243 _x = _v137 02244 start = end 02245 end += 8 02246 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02247 start = end 02248 end += 4 02249 (length,) = _struct_I.unpack(str[start:end]) 02250 start = end 02251 end += length 02252 _v136.frame_id = str[start:end] 02253 _v138 = _v135.pose 02254 _v139 = _v138.position 02255 _x = _v139 02256 start = end 02257 end += 24 02258 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02259 _v140 = _v138.orientation 02260 _x = _v140 02261 start = end 02262 end += 32 02263 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02264 _v141 = _v121.roi_box_dims 02265 _x = _v141 02266 start = end 02267 end += 24 02268 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02269 start = end 02270 end += 4 02271 (length,) = _struct_I.unpack(str[start:end]) 02272 start = end 02273 end += length 02274 val2.collision_name = str[start:end] 02275 val1.moved_obstacles.append(val2) 02276 self.desired_grasps.append(val1) 02277 _x = self 02278 start = end 02279 end += 12 02280 (_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02281 start = end 02282 end += 4 02283 (length,) = _struct_I.unpack(str[start:end]) 02284 start = end 02285 end += length 02286 self.lift.direction.header.frame_id = str[start:end] 02287 _x = self 02288 start = end 02289 end += 32 02290 (_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance,) = _struct_3d2f.unpack(str[start:end]) 02291 start = end 02292 end += 4 02293 (length,) = _struct_I.unpack(str[start:end]) 02294 start = end 02295 end += length 02296 self.collision_object_name = str[start:end] 02297 start = end 02298 end += 4 02299 (length,) = _struct_I.unpack(str[start:end]) 02300 start = end 02301 end += length 02302 self.collision_support_surface_name = str[start:end] 02303 _x = self 02304 start = end 02305 end += 5 02306 (_x.allow_gripper_support_collision, _x.use_reactive_execution, _x.use_reactive_lift, _x.only_perform_feasibility_test, _x.ignore_collisions,) = _struct_5B.unpack(str[start:end]) 02307 self.allow_gripper_support_collision = bool(self.allow_gripper_support_collision) 02308 self.use_reactive_execution = bool(self.use_reactive_execution) 02309 self.use_reactive_lift = bool(self.use_reactive_lift) 02310 self.only_perform_feasibility_test = bool(self.only_perform_feasibility_test) 02311 self.ignore_collisions = bool(self.ignore_collisions) 02312 start = end 02313 end += 4 02314 (length,) = _struct_I.unpack(str[start:end]) 02315 self.path_constraints.joint_constraints = [] 02316 for i in range(0, length): 02317 val1 = arm_navigation_msgs.msg.JointConstraint() 02318 start = end 02319 end += 4 02320 (length,) = _struct_I.unpack(str[start:end]) 02321 start = end 02322 end += length 02323 val1.joint_name = str[start:end] 02324 _x = val1 02325 start = end 02326 end += 32 02327 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 02328 self.path_constraints.joint_constraints.append(val1) 02329 start = end 02330 end += 4 02331 (length,) = _struct_I.unpack(str[start:end]) 02332 self.path_constraints.position_constraints = [] 02333 for i in range(0, length): 02334 val1 = arm_navigation_msgs.msg.PositionConstraint() 02335 _v142 = val1.header 02336 start = end 02337 end += 4 02338 (_v142.seq,) = _struct_I.unpack(str[start:end]) 02339 _v143 = _v142.stamp 02340 _x = _v143 02341 start = end 02342 end += 8 02343 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02344 start = end 02345 end += 4 02346 (length,) = _struct_I.unpack(str[start:end]) 02347 start = end 02348 end += length 02349 _v142.frame_id = str[start:end] 02350 start = end 02351 end += 4 02352 (length,) = _struct_I.unpack(str[start:end]) 02353 start = end 02354 end += length 02355 val1.link_name = str[start:end] 02356 _v144 = val1.target_point_offset 02357 _x = _v144 02358 start = end 02359 end += 24 02360 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02361 _v145 = val1.position 02362 _x = _v145 02363 start = end 02364 end += 24 02365 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02366 _v146 = val1.constraint_region_shape 02367 start = end 02368 end += 1 02369 (_v146.type,) = _struct_b.unpack(str[start:end]) 02370 start = end 02371 end += 4 02372 (length,) = _struct_I.unpack(str[start:end]) 02373 pattern = '<%sd'%length 02374 start = end 02375 end += struct.calcsize(pattern) 02376 _v146.dimensions = struct.unpack(pattern, str[start:end]) 02377 start = end 02378 end += 4 02379 (length,) = _struct_I.unpack(str[start:end]) 02380 pattern = '<%si'%length 02381 start = end 02382 end += struct.calcsize(pattern) 02383 _v146.triangles = struct.unpack(pattern, str[start:end]) 02384 start = end 02385 end += 4 02386 (length,) = _struct_I.unpack(str[start:end]) 02387 _v146.vertices = [] 02388 for i in range(0, length): 02389 val3 = geometry_msgs.msg.Point() 02390 _x = val3 02391 start = end 02392 end += 24 02393 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02394 _v146.vertices.append(val3) 02395 _v147 = val1.constraint_region_orientation 02396 _x = _v147 02397 start = end 02398 end += 32 02399 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02400 start = end 02401 end += 8 02402 (val1.weight,) = _struct_d.unpack(str[start:end]) 02403 self.path_constraints.position_constraints.append(val1) 02404 start = end 02405 end += 4 02406 (length,) = _struct_I.unpack(str[start:end]) 02407 self.path_constraints.orientation_constraints = [] 02408 for i in range(0, length): 02409 val1 = arm_navigation_msgs.msg.OrientationConstraint() 02410 _v148 = val1.header 02411 start = end 02412 end += 4 02413 (_v148.seq,) = _struct_I.unpack(str[start:end]) 02414 _v149 = _v148.stamp 02415 _x = _v149 02416 start = end 02417 end += 8 02418 (_x.secs, _x.nsecs,) = _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 _v148.frame_id = str[start:end] 02425 start = end 02426 end += 4 02427 (length,) = _struct_I.unpack(str[start:end]) 02428 start = end 02429 end += length 02430 val1.link_name = str[start:end] 02431 start = end 02432 end += 4 02433 (val1.type,) = _struct_i.unpack(str[start:end]) 02434 _v150 = val1.orientation 02435 _x = _v150 02436 start = end 02437 end += 32 02438 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02439 _x = val1 02440 start = end 02441 end += 32 02442 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 02443 self.path_constraints.orientation_constraints.append(val1) 02444 start = end 02445 end += 4 02446 (length,) = _struct_I.unpack(str[start:end]) 02447 self.path_constraints.visibility_constraints = [] 02448 for i in range(0, length): 02449 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 02450 _v151 = val1.header 02451 start = end 02452 end += 4 02453 (_v151.seq,) = _struct_I.unpack(str[start:end]) 02454 _v152 = _v151.stamp 02455 _x = _v152 02456 start = end 02457 end += 8 02458 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02459 start = end 02460 end += 4 02461 (length,) = _struct_I.unpack(str[start:end]) 02462 start = end 02463 end += length 02464 _v151.frame_id = str[start:end] 02465 _v153 = val1.target 02466 _v154 = _v153.header 02467 start = end 02468 end += 4 02469 (_v154.seq,) = _struct_I.unpack(str[start:end]) 02470 _v155 = _v154.stamp 02471 _x = _v155 02472 start = end 02473 end += 8 02474 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02475 start = end 02476 end += 4 02477 (length,) = _struct_I.unpack(str[start:end]) 02478 start = end 02479 end += length 02480 _v154.frame_id = str[start:end] 02481 _v156 = _v153.point 02482 _x = _v156 02483 start = end 02484 end += 24 02485 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02486 _v157 = val1.sensor_pose 02487 _v158 = _v157.header 02488 start = end 02489 end += 4 02490 (_v158.seq,) = _struct_I.unpack(str[start:end]) 02491 _v159 = _v158.stamp 02492 _x = _v159 02493 start = end 02494 end += 8 02495 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02496 start = end 02497 end += 4 02498 (length,) = _struct_I.unpack(str[start:end]) 02499 start = end 02500 end += length 02501 _v158.frame_id = str[start:end] 02502 _v160 = _v157.pose 02503 _v161 = _v160.position 02504 _x = _v161 02505 start = end 02506 end += 24 02507 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02508 _v162 = _v160.orientation 02509 _x = _v162 02510 start = end 02511 end += 32 02512 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02513 start = end 02514 end += 8 02515 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 02516 self.path_constraints.visibility_constraints.append(val1) 02517 start = end 02518 end += 4 02519 (length,) = _struct_I.unpack(str[start:end]) 02520 self.additional_collision_operations.collision_operations = [] 02521 for i in range(0, length): 02522 val1 = arm_navigation_msgs.msg.CollisionOperation() 02523 start = end 02524 end += 4 02525 (length,) = _struct_I.unpack(str[start:end]) 02526 start = end 02527 end += length 02528 val1.object1 = str[start:end] 02529 start = end 02530 end += 4 02531 (length,) = _struct_I.unpack(str[start:end]) 02532 start = end 02533 end += length 02534 val1.object2 = str[start:end] 02535 _x = val1 02536 start = end 02537 end += 12 02538 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 02539 self.additional_collision_operations.collision_operations.append(val1) 02540 start = end 02541 end += 4 02542 (length,) = _struct_I.unpack(str[start:end]) 02543 self.additional_link_padding = [] 02544 for i in range(0, length): 02545 val1 = arm_navigation_msgs.msg.LinkPadding() 02546 start = end 02547 end += 4 02548 (length,) = _struct_I.unpack(str[start:end]) 02549 start = end 02550 end += length 02551 val1.link_name = str[start:end] 02552 start = end 02553 end += 8 02554 (val1.padding,) = _struct_d.unpack(str[start:end]) 02555 self.additional_link_padding.append(val1) 02556 start = end 02557 end += 4 02558 (length,) = _struct_I.unpack(str[start:end]) 02559 self.movable_obstacles = [] 02560 for i in range(0, length): 02561 val1 = object_manipulation_msgs.msg.GraspableObject() 02562 start = end 02563 end += 4 02564 (length,) = _struct_I.unpack(str[start:end]) 02565 start = end 02566 end += length 02567 val1.reference_frame_id = str[start:end] 02568 start = end 02569 end += 4 02570 (length,) = _struct_I.unpack(str[start:end]) 02571 val1.potential_models = [] 02572 for i in range(0, length): 02573 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 02574 start = end 02575 end += 4 02576 (val2.model_id,) = _struct_i.unpack(str[start:end]) 02577 _v163 = val2.pose 02578 _v164 = _v163.header 02579 start = end 02580 end += 4 02581 (_v164.seq,) = _struct_I.unpack(str[start:end]) 02582 _v165 = _v164.stamp 02583 _x = _v165 02584 start = end 02585 end += 8 02586 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02587 start = end 02588 end += 4 02589 (length,) = _struct_I.unpack(str[start:end]) 02590 start = end 02591 end += length 02592 _v164.frame_id = str[start:end] 02593 _v166 = _v163.pose 02594 _v167 = _v166.position 02595 _x = _v167 02596 start = end 02597 end += 24 02598 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02599 _v168 = _v166.orientation 02600 _x = _v168 02601 start = end 02602 end += 32 02603 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02604 start = end 02605 end += 4 02606 (val2.confidence,) = _struct_f.unpack(str[start:end]) 02607 start = end 02608 end += 4 02609 (length,) = _struct_I.unpack(str[start:end]) 02610 start = end 02611 end += length 02612 val2.detector_name = str[start:end] 02613 val1.potential_models.append(val2) 02614 _v169 = val1.cluster 02615 _v170 = _v169.header 02616 start = end 02617 end += 4 02618 (_v170.seq,) = _struct_I.unpack(str[start:end]) 02619 _v171 = _v170.stamp 02620 _x = _v171 02621 start = end 02622 end += 8 02623 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02624 start = end 02625 end += 4 02626 (length,) = _struct_I.unpack(str[start:end]) 02627 start = end 02628 end += length 02629 _v170.frame_id = str[start:end] 02630 start = end 02631 end += 4 02632 (length,) = _struct_I.unpack(str[start:end]) 02633 _v169.points = [] 02634 for i in range(0, length): 02635 val3 = geometry_msgs.msg.Point32() 02636 _x = val3 02637 start = end 02638 end += 12 02639 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 02640 _v169.points.append(val3) 02641 start = end 02642 end += 4 02643 (length,) = _struct_I.unpack(str[start:end]) 02644 _v169.channels = [] 02645 for i in range(0, length): 02646 val3 = sensor_msgs.msg.ChannelFloat32() 02647 start = end 02648 end += 4 02649 (length,) = _struct_I.unpack(str[start:end]) 02650 start = end 02651 end += length 02652 val3.name = str[start:end] 02653 start = end 02654 end += 4 02655 (length,) = _struct_I.unpack(str[start:end]) 02656 pattern = '<%sf'%length 02657 start = end 02658 end += struct.calcsize(pattern) 02659 val3.values = struct.unpack(pattern, str[start:end]) 02660 _v169.channels.append(val3) 02661 _v172 = val1.region 02662 _v173 = _v172.cloud 02663 _v174 = _v173.header 02664 start = end 02665 end += 4 02666 (_v174.seq,) = _struct_I.unpack(str[start:end]) 02667 _v175 = _v174.stamp 02668 _x = _v175 02669 start = end 02670 end += 8 02671 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02672 start = end 02673 end += 4 02674 (length,) = _struct_I.unpack(str[start:end]) 02675 start = end 02676 end += length 02677 _v174.frame_id = str[start:end] 02678 _x = _v173 02679 start = end 02680 end += 8 02681 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02682 start = end 02683 end += 4 02684 (length,) = _struct_I.unpack(str[start:end]) 02685 _v173.fields = [] 02686 for i in range(0, length): 02687 val4 = sensor_msgs.msg.PointField() 02688 start = end 02689 end += 4 02690 (length,) = _struct_I.unpack(str[start:end]) 02691 start = end 02692 end += length 02693 val4.name = str[start:end] 02694 _x = val4 02695 start = end 02696 end += 9 02697 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 02698 _v173.fields.append(val4) 02699 _x = _v173 02700 start = end 02701 end += 9 02702 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 02703 _v173.is_bigendian = bool(_v173.is_bigendian) 02704 start = end 02705 end += 4 02706 (length,) = _struct_I.unpack(str[start:end]) 02707 start = end 02708 end += length 02709 _v173.data = str[start:end] 02710 start = end 02711 end += 1 02712 (_v173.is_dense,) = _struct_B.unpack(str[start:end]) 02713 _v173.is_dense = bool(_v173.is_dense) 02714 start = end 02715 end += 4 02716 (length,) = _struct_I.unpack(str[start:end]) 02717 pattern = '<%si'%length 02718 start = end 02719 end += struct.calcsize(pattern) 02720 _v172.mask = struct.unpack(pattern, str[start:end]) 02721 _v176 = _v172.image 02722 _v177 = _v176.header 02723 start = end 02724 end += 4 02725 (_v177.seq,) = _struct_I.unpack(str[start:end]) 02726 _v178 = _v177.stamp 02727 _x = _v178 02728 start = end 02729 end += 8 02730 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02731 start = end 02732 end += 4 02733 (length,) = _struct_I.unpack(str[start:end]) 02734 start = end 02735 end += length 02736 _v177.frame_id = str[start:end] 02737 _x = _v176 02738 start = end 02739 end += 8 02740 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02741 start = end 02742 end += 4 02743 (length,) = _struct_I.unpack(str[start:end]) 02744 start = end 02745 end += length 02746 _v176.encoding = str[start:end] 02747 _x = _v176 02748 start = end 02749 end += 5 02750 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 02751 start = end 02752 end += 4 02753 (length,) = _struct_I.unpack(str[start:end]) 02754 start = end 02755 end += length 02756 _v176.data = str[start:end] 02757 _v179 = _v172.disparity_image 02758 _v180 = _v179.header 02759 start = end 02760 end += 4 02761 (_v180.seq,) = _struct_I.unpack(str[start:end]) 02762 _v181 = _v180.stamp 02763 _x = _v181 02764 start = end 02765 end += 8 02766 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02767 start = end 02768 end += 4 02769 (length,) = _struct_I.unpack(str[start:end]) 02770 start = end 02771 end += length 02772 _v180.frame_id = str[start:end] 02773 _x = _v179 02774 start = end 02775 end += 8 02776 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02777 start = end 02778 end += 4 02779 (length,) = _struct_I.unpack(str[start:end]) 02780 start = end 02781 end += length 02782 _v179.encoding = str[start:end] 02783 _x = _v179 02784 start = end 02785 end += 5 02786 (_x.is_bigendian, _x.step,) = _struct_BI.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 _v179.data = str[start:end] 02793 _v182 = _v172.cam_info 02794 _v183 = _v182.header 02795 start = end 02796 end += 4 02797 (_v183.seq,) = _struct_I.unpack(str[start:end]) 02798 _v184 = _v183.stamp 02799 _x = _v184 02800 start = end 02801 end += 8 02802 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02803 start = end 02804 end += 4 02805 (length,) = _struct_I.unpack(str[start:end]) 02806 start = end 02807 end += length 02808 _v183.frame_id = str[start:end] 02809 _x = _v182 02810 start = end 02811 end += 8 02812 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 02813 start = end 02814 end += 4 02815 (length,) = _struct_I.unpack(str[start:end]) 02816 start = end 02817 end += length 02818 _v182.distortion_model = str[start:end] 02819 start = end 02820 end += 4 02821 (length,) = _struct_I.unpack(str[start:end]) 02822 pattern = '<%sd'%length 02823 start = end 02824 end += struct.calcsize(pattern) 02825 _v182.D = struct.unpack(pattern, str[start:end]) 02826 start = end 02827 end += 72 02828 _v182.K = _struct_9d.unpack(str[start:end]) 02829 start = end 02830 end += 72 02831 _v182.R = _struct_9d.unpack(str[start:end]) 02832 start = end 02833 end += 96 02834 _v182.P = _struct_12d.unpack(str[start:end]) 02835 _x = _v182 02836 start = end 02837 end += 8 02838 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 02839 _v185 = _v182.roi 02840 _x = _v185 02841 start = end 02842 end += 17 02843 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 02844 _v185.do_rectify = bool(_v185.do_rectify) 02845 _v186 = _v172.roi_box_pose 02846 _v187 = _v186.header 02847 start = end 02848 end += 4 02849 (_v187.seq,) = _struct_I.unpack(str[start:end]) 02850 _v188 = _v187.stamp 02851 _x = _v188 02852 start = end 02853 end += 8 02854 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02855 start = end 02856 end += 4 02857 (length,) = _struct_I.unpack(str[start:end]) 02858 start = end 02859 end += length 02860 _v187.frame_id = str[start:end] 02861 _v189 = _v186.pose 02862 _v190 = _v189.position 02863 _x = _v190 02864 start = end 02865 end += 24 02866 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02867 _v191 = _v189.orientation 02868 _x = _v191 02869 start = end 02870 end += 32 02871 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02872 _v192 = _v172.roi_box_dims 02873 _x = _v192 02874 start = end 02875 end += 24 02876 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02877 start = end 02878 end += 4 02879 (length,) = _struct_I.unpack(str[start:end]) 02880 start = end 02881 end += length 02882 val1.collision_name = str[start:end] 02883 self.movable_obstacles.append(val1) 02884 start = end 02885 end += 4 02886 (self.max_contact_force,) = _struct_f.unpack(str[start:end]) 02887 return self 02888 except struct.error as e: 02889 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02890 02891 02892 def serialize_numpy(self, buff, numpy): 02893 """ 02894 serialize message with numpy array types into buffer 02895 @param buff: buffer 02896 @type buff: StringIO 02897 @param numpy: numpy python module 02898 @type numpy module 02899 """ 02900 try: 02901 _x = self.arm_name 02902 length = len(_x) 02903 buff.write(struct.pack('<I%ss'%length, length, _x)) 02904 _x = self.target.reference_frame_id 02905 length = len(_x) 02906 buff.write(struct.pack('<I%ss'%length, length, _x)) 02907 length = len(self.target.potential_models) 02908 buff.write(_struct_I.pack(length)) 02909 for val1 in self.target.potential_models: 02910 buff.write(_struct_i.pack(val1.model_id)) 02911 _v193 = val1.pose 02912 _v194 = _v193.header 02913 buff.write(_struct_I.pack(_v194.seq)) 02914 _v195 = _v194.stamp 02915 _x = _v195 02916 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02917 _x = _v194.frame_id 02918 length = len(_x) 02919 buff.write(struct.pack('<I%ss'%length, length, _x)) 02920 _v196 = _v193.pose 02921 _v197 = _v196.position 02922 _x = _v197 02923 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02924 _v198 = _v196.orientation 02925 _x = _v198 02926 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02927 buff.write(_struct_f.pack(val1.confidence)) 02928 _x = val1.detector_name 02929 length = len(_x) 02930 buff.write(struct.pack('<I%ss'%length, length, _x)) 02931 _x = self 02932 buff.write(_struct_3I.pack(_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs)) 02933 _x = self.target.cluster.header.frame_id 02934 length = len(_x) 02935 buff.write(struct.pack('<I%ss'%length, length, _x)) 02936 length = len(self.target.cluster.points) 02937 buff.write(_struct_I.pack(length)) 02938 for val1 in self.target.cluster.points: 02939 _x = val1 02940 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02941 length = len(self.target.cluster.channels) 02942 buff.write(_struct_I.pack(length)) 02943 for val1 in self.target.cluster.channels: 02944 _x = val1.name 02945 length = len(_x) 02946 buff.write(struct.pack('<I%ss'%length, length, _x)) 02947 length = len(val1.values) 02948 buff.write(_struct_I.pack(length)) 02949 pattern = '<%sf'%length 02950 buff.write(val1.values.tostring()) 02951 _x = self 02952 buff.write(_struct_3I.pack(_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs)) 02953 _x = self.target.region.cloud.header.frame_id 02954 length = len(_x) 02955 buff.write(struct.pack('<I%ss'%length, length, _x)) 02956 _x = self 02957 buff.write(_struct_2I.pack(_x.target.region.cloud.height, _x.target.region.cloud.width)) 02958 length = len(self.target.region.cloud.fields) 02959 buff.write(_struct_I.pack(length)) 02960 for val1 in self.target.region.cloud.fields: 02961 _x = val1.name 02962 length = len(_x) 02963 buff.write(struct.pack('<I%ss'%length, length, _x)) 02964 _x = val1 02965 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02966 _x = self 02967 buff.write(_struct_B2I.pack(_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step)) 02968 _x = self.target.region.cloud.data 02969 length = len(_x) 02970 # - if encoded as a list instead, serialize as bytes instead of string 02971 if type(_x) in [list, tuple]: 02972 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02973 else: 02974 buff.write(struct.pack('<I%ss'%length, length, _x)) 02975 buff.write(_struct_B.pack(self.target.region.cloud.is_dense)) 02976 length = len(self.target.region.mask) 02977 buff.write(_struct_I.pack(length)) 02978 pattern = '<%si'%length 02979 buff.write(self.target.region.mask.tostring()) 02980 _x = self 02981 buff.write(_struct_3I.pack(_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs)) 02982 _x = self.target.region.image.header.frame_id 02983 length = len(_x) 02984 buff.write(struct.pack('<I%ss'%length, length, _x)) 02985 _x = self 02986 buff.write(_struct_2I.pack(_x.target.region.image.height, _x.target.region.image.width)) 02987 _x = self.target.region.image.encoding 02988 length = len(_x) 02989 buff.write(struct.pack('<I%ss'%length, length, _x)) 02990 _x = self 02991 buff.write(_struct_BI.pack(_x.target.region.image.is_bigendian, _x.target.region.image.step)) 02992 _x = self.target.region.image.data 02993 length = len(_x) 02994 # - if encoded as a list instead, serialize as bytes instead of string 02995 if type(_x) in [list, tuple]: 02996 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02997 else: 02998 buff.write(struct.pack('<I%ss'%length, length, _x)) 02999 _x = self 03000 buff.write(_struct_3I.pack(_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs)) 03001 _x = self.target.region.disparity_image.header.frame_id 03002 length = len(_x) 03003 buff.write(struct.pack('<I%ss'%length, length, _x)) 03004 _x = self 03005 buff.write(_struct_2I.pack(_x.target.region.disparity_image.height, _x.target.region.disparity_image.width)) 03006 _x = self.target.region.disparity_image.encoding 03007 length = len(_x) 03008 buff.write(struct.pack('<I%ss'%length, length, _x)) 03009 _x = self 03010 buff.write(_struct_BI.pack(_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step)) 03011 _x = self.target.region.disparity_image.data 03012 length = len(_x) 03013 # - if encoded as a list instead, serialize as bytes instead of string 03014 if type(_x) in [list, tuple]: 03015 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03016 else: 03017 buff.write(struct.pack('<I%ss'%length, length, _x)) 03018 _x = self 03019 buff.write(_struct_3I.pack(_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs)) 03020 _x = self.target.region.cam_info.header.frame_id 03021 length = len(_x) 03022 buff.write(struct.pack('<I%ss'%length, length, _x)) 03023 _x = self 03024 buff.write(_struct_2I.pack(_x.target.region.cam_info.height, _x.target.region.cam_info.width)) 03025 _x = self.target.region.cam_info.distortion_model 03026 length = len(_x) 03027 buff.write(struct.pack('<I%ss'%length, length, _x)) 03028 length = len(self.target.region.cam_info.D) 03029 buff.write(_struct_I.pack(length)) 03030 pattern = '<%sd'%length 03031 buff.write(self.target.region.cam_info.D.tostring()) 03032 buff.write(self.target.region.cam_info.K.tostring()) 03033 buff.write(self.target.region.cam_info.R.tostring()) 03034 buff.write(self.target.region.cam_info.P.tostring()) 03035 _x = self 03036 buff.write(_struct_6IB3I.pack(_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs)) 03037 _x = self.target.region.roi_box_pose.header.frame_id 03038 length = len(_x) 03039 buff.write(struct.pack('<I%ss'%length, length, _x)) 03040 _x = self 03041 buff.write(_struct_10d.pack(_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z)) 03042 _x = self.target.collision_name 03043 length = len(_x) 03044 buff.write(struct.pack('<I%ss'%length, length, _x)) 03045 length = len(self.desired_grasps) 03046 buff.write(_struct_I.pack(length)) 03047 for val1 in self.desired_grasps: 03048 _v199 = val1.pre_grasp_posture 03049 _v200 = _v199.header 03050 buff.write(_struct_I.pack(_v200.seq)) 03051 _v201 = _v200.stamp 03052 _x = _v201 03053 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03054 _x = _v200.frame_id 03055 length = len(_x) 03056 buff.write(struct.pack('<I%ss'%length, length, _x)) 03057 length = len(_v199.name) 03058 buff.write(_struct_I.pack(length)) 03059 for val3 in _v199.name: 03060 length = len(val3) 03061 buff.write(struct.pack('<I%ss'%length, length, val3)) 03062 length = len(_v199.position) 03063 buff.write(_struct_I.pack(length)) 03064 pattern = '<%sd'%length 03065 buff.write(_v199.position.tostring()) 03066 length = len(_v199.velocity) 03067 buff.write(_struct_I.pack(length)) 03068 pattern = '<%sd'%length 03069 buff.write(_v199.velocity.tostring()) 03070 length = len(_v199.effort) 03071 buff.write(_struct_I.pack(length)) 03072 pattern = '<%sd'%length 03073 buff.write(_v199.effort.tostring()) 03074 _v202 = val1.grasp_posture 03075 _v203 = _v202.header 03076 buff.write(_struct_I.pack(_v203.seq)) 03077 _v204 = _v203.stamp 03078 _x = _v204 03079 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03080 _x = _v203.frame_id 03081 length = len(_x) 03082 buff.write(struct.pack('<I%ss'%length, length, _x)) 03083 length = len(_v202.name) 03084 buff.write(_struct_I.pack(length)) 03085 for val3 in _v202.name: 03086 length = len(val3) 03087 buff.write(struct.pack('<I%ss'%length, length, val3)) 03088 length = len(_v202.position) 03089 buff.write(_struct_I.pack(length)) 03090 pattern = '<%sd'%length 03091 buff.write(_v202.position.tostring()) 03092 length = len(_v202.velocity) 03093 buff.write(_struct_I.pack(length)) 03094 pattern = '<%sd'%length 03095 buff.write(_v202.velocity.tostring()) 03096 length = len(_v202.effort) 03097 buff.write(_struct_I.pack(length)) 03098 pattern = '<%sd'%length 03099 buff.write(_v202.effort.tostring()) 03100 _v205 = val1.grasp_pose 03101 _v206 = _v205.position 03102 _x = _v206 03103 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03104 _v207 = _v205.orientation 03105 _x = _v207 03106 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03107 _x = val1 03108 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance)) 03109 length = len(val1.moved_obstacles) 03110 buff.write(_struct_I.pack(length)) 03111 for val2 in val1.moved_obstacles: 03112 _x = val2.reference_frame_id 03113 length = len(_x) 03114 buff.write(struct.pack('<I%ss'%length, length, _x)) 03115 length = len(val2.potential_models) 03116 buff.write(_struct_I.pack(length)) 03117 for val3 in val2.potential_models: 03118 buff.write(_struct_i.pack(val3.model_id)) 03119 _v208 = val3.pose 03120 _v209 = _v208.header 03121 buff.write(_struct_I.pack(_v209.seq)) 03122 _v210 = _v209.stamp 03123 _x = _v210 03124 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03125 _x = _v209.frame_id 03126 length = len(_x) 03127 buff.write(struct.pack('<I%ss'%length, length, _x)) 03128 _v211 = _v208.pose 03129 _v212 = _v211.position 03130 _x = _v212 03131 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03132 _v213 = _v211.orientation 03133 _x = _v213 03134 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03135 buff.write(_struct_f.pack(val3.confidence)) 03136 _x = val3.detector_name 03137 length = len(_x) 03138 buff.write(struct.pack('<I%ss'%length, length, _x)) 03139 _v214 = val2.cluster 03140 _v215 = _v214.header 03141 buff.write(_struct_I.pack(_v215.seq)) 03142 _v216 = _v215.stamp 03143 _x = _v216 03144 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03145 _x = _v215.frame_id 03146 length = len(_x) 03147 buff.write(struct.pack('<I%ss'%length, length, _x)) 03148 length = len(_v214.points) 03149 buff.write(_struct_I.pack(length)) 03150 for val4 in _v214.points: 03151 _x = val4 03152 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 03153 length = len(_v214.channels) 03154 buff.write(_struct_I.pack(length)) 03155 for val4 in _v214.channels: 03156 _x = val4.name 03157 length = len(_x) 03158 buff.write(struct.pack('<I%ss'%length, length, _x)) 03159 length = len(val4.values) 03160 buff.write(_struct_I.pack(length)) 03161 pattern = '<%sf'%length 03162 buff.write(val4.values.tostring()) 03163 _v217 = val2.region 03164 _v218 = _v217.cloud 03165 _v219 = _v218.header 03166 buff.write(_struct_I.pack(_v219.seq)) 03167 _v220 = _v219.stamp 03168 _x = _v220 03169 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03170 _x = _v219.frame_id 03171 length = len(_x) 03172 buff.write(struct.pack('<I%ss'%length, length, _x)) 03173 _x = _v218 03174 buff.write(_struct_2I.pack(_x.height, _x.width)) 03175 length = len(_v218.fields) 03176 buff.write(_struct_I.pack(length)) 03177 for val5 in _v218.fields: 03178 _x = val5.name 03179 length = len(_x) 03180 buff.write(struct.pack('<I%ss'%length, length, _x)) 03181 _x = val5 03182 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 03183 _x = _v218 03184 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 03185 _x = _v218.data 03186 length = len(_x) 03187 # - if encoded as a list instead, serialize as bytes instead of string 03188 if type(_x) in [list, tuple]: 03189 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03190 else: 03191 buff.write(struct.pack('<I%ss'%length, length, _x)) 03192 buff.write(_struct_B.pack(_v218.is_dense)) 03193 length = len(_v217.mask) 03194 buff.write(_struct_I.pack(length)) 03195 pattern = '<%si'%length 03196 buff.write(_v217.mask.tostring()) 03197 _v221 = _v217.image 03198 _v222 = _v221.header 03199 buff.write(_struct_I.pack(_v222.seq)) 03200 _v223 = _v222.stamp 03201 _x = _v223 03202 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03203 _x = _v222.frame_id 03204 length = len(_x) 03205 buff.write(struct.pack('<I%ss'%length, length, _x)) 03206 _x = _v221 03207 buff.write(_struct_2I.pack(_x.height, _x.width)) 03208 _x = _v221.encoding 03209 length = len(_x) 03210 buff.write(struct.pack('<I%ss'%length, length, _x)) 03211 _x = _v221 03212 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 03213 _x = _v221.data 03214 length = len(_x) 03215 # - if encoded as a list instead, serialize as bytes instead of string 03216 if type(_x) in [list, tuple]: 03217 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03218 else: 03219 buff.write(struct.pack('<I%ss'%length, length, _x)) 03220 _v224 = _v217.disparity_image 03221 _v225 = _v224.header 03222 buff.write(_struct_I.pack(_v225.seq)) 03223 _v226 = _v225.stamp 03224 _x = _v226 03225 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03226 _x = _v225.frame_id 03227 length = len(_x) 03228 buff.write(struct.pack('<I%ss'%length, length, _x)) 03229 _x = _v224 03230 buff.write(_struct_2I.pack(_x.height, _x.width)) 03231 _x = _v224.encoding 03232 length = len(_x) 03233 buff.write(struct.pack('<I%ss'%length, length, _x)) 03234 _x = _v224 03235 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 03236 _x = _v224.data 03237 length = len(_x) 03238 # - if encoded as a list instead, serialize as bytes instead of string 03239 if type(_x) in [list, tuple]: 03240 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03241 else: 03242 buff.write(struct.pack('<I%ss'%length, length, _x)) 03243 _v227 = _v217.cam_info 03244 _v228 = _v227.header 03245 buff.write(_struct_I.pack(_v228.seq)) 03246 _v229 = _v228.stamp 03247 _x = _v229 03248 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03249 _x = _v228.frame_id 03250 length = len(_x) 03251 buff.write(struct.pack('<I%ss'%length, length, _x)) 03252 _x = _v227 03253 buff.write(_struct_2I.pack(_x.height, _x.width)) 03254 _x = _v227.distortion_model 03255 length = len(_x) 03256 buff.write(struct.pack('<I%ss'%length, length, _x)) 03257 length = len(_v227.D) 03258 buff.write(_struct_I.pack(length)) 03259 pattern = '<%sd'%length 03260 buff.write(_v227.D.tostring()) 03261 buff.write(_v227.K.tostring()) 03262 buff.write(_v227.R.tostring()) 03263 buff.write(_v227.P.tostring()) 03264 _x = _v227 03265 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 03266 _v230 = _v227.roi 03267 _x = _v230 03268 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 03269 _v231 = _v217.roi_box_pose 03270 _v232 = _v231.header 03271 buff.write(_struct_I.pack(_v232.seq)) 03272 _v233 = _v232.stamp 03273 _x = _v233 03274 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03275 _x = _v232.frame_id 03276 length = len(_x) 03277 buff.write(struct.pack('<I%ss'%length, length, _x)) 03278 _v234 = _v231.pose 03279 _v235 = _v234.position 03280 _x = _v235 03281 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03282 _v236 = _v234.orientation 03283 _x = _v236 03284 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03285 _v237 = _v217.roi_box_dims 03286 _x = _v237 03287 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03288 _x = val2.collision_name 03289 length = len(_x) 03290 buff.write(struct.pack('<I%ss'%length, length, _x)) 03291 _x = self 03292 buff.write(_struct_3I.pack(_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs)) 03293 _x = self.lift.direction.header.frame_id 03294 length = len(_x) 03295 buff.write(struct.pack('<I%ss'%length, length, _x)) 03296 _x = self 03297 buff.write(_struct_3d2f.pack(_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance)) 03298 _x = self.collision_object_name 03299 length = len(_x) 03300 buff.write(struct.pack('<I%ss'%length, length, _x)) 03301 _x = self.collision_support_surface_name 03302 length = len(_x) 03303 buff.write(struct.pack('<I%ss'%length, length, _x)) 03304 _x = self 03305 buff.write(_struct_5B.pack(_x.allow_gripper_support_collision, _x.use_reactive_execution, _x.use_reactive_lift, _x.only_perform_feasibility_test, _x.ignore_collisions)) 03306 length = len(self.path_constraints.joint_constraints) 03307 buff.write(_struct_I.pack(length)) 03308 for val1 in self.path_constraints.joint_constraints: 03309 _x = val1.joint_name 03310 length = len(_x) 03311 buff.write(struct.pack('<I%ss'%length, length, _x)) 03312 _x = val1 03313 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 03314 length = len(self.path_constraints.position_constraints) 03315 buff.write(_struct_I.pack(length)) 03316 for val1 in self.path_constraints.position_constraints: 03317 _v238 = val1.header 03318 buff.write(_struct_I.pack(_v238.seq)) 03319 _v239 = _v238.stamp 03320 _x = _v239 03321 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03322 _x = _v238.frame_id 03323 length = len(_x) 03324 buff.write(struct.pack('<I%ss'%length, length, _x)) 03325 _x = val1.link_name 03326 length = len(_x) 03327 buff.write(struct.pack('<I%ss'%length, length, _x)) 03328 _v240 = val1.target_point_offset 03329 _x = _v240 03330 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03331 _v241 = val1.position 03332 _x = _v241 03333 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03334 _v242 = val1.constraint_region_shape 03335 buff.write(_struct_b.pack(_v242.type)) 03336 length = len(_v242.dimensions) 03337 buff.write(_struct_I.pack(length)) 03338 pattern = '<%sd'%length 03339 buff.write(_v242.dimensions.tostring()) 03340 length = len(_v242.triangles) 03341 buff.write(_struct_I.pack(length)) 03342 pattern = '<%si'%length 03343 buff.write(_v242.triangles.tostring()) 03344 length = len(_v242.vertices) 03345 buff.write(_struct_I.pack(length)) 03346 for val3 in _v242.vertices: 03347 _x = val3 03348 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03349 _v243 = val1.constraint_region_orientation 03350 _x = _v243 03351 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03352 buff.write(_struct_d.pack(val1.weight)) 03353 length = len(self.path_constraints.orientation_constraints) 03354 buff.write(_struct_I.pack(length)) 03355 for val1 in self.path_constraints.orientation_constraints: 03356 _v244 = val1.header 03357 buff.write(_struct_I.pack(_v244.seq)) 03358 _v245 = _v244.stamp 03359 _x = _v245 03360 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03361 _x = _v244.frame_id 03362 length = len(_x) 03363 buff.write(struct.pack('<I%ss'%length, length, _x)) 03364 _x = val1.link_name 03365 length = len(_x) 03366 buff.write(struct.pack('<I%ss'%length, length, _x)) 03367 buff.write(_struct_i.pack(val1.type)) 03368 _v246 = val1.orientation 03369 _x = _v246 03370 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03371 _x = val1 03372 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 03373 length = len(self.path_constraints.visibility_constraints) 03374 buff.write(_struct_I.pack(length)) 03375 for val1 in self.path_constraints.visibility_constraints: 03376 _v247 = val1.header 03377 buff.write(_struct_I.pack(_v247.seq)) 03378 _v248 = _v247.stamp 03379 _x = _v248 03380 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03381 _x = _v247.frame_id 03382 length = len(_x) 03383 buff.write(struct.pack('<I%ss'%length, length, _x)) 03384 _v249 = val1.target 03385 _v250 = _v249.header 03386 buff.write(_struct_I.pack(_v250.seq)) 03387 _v251 = _v250.stamp 03388 _x = _v251 03389 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03390 _x = _v250.frame_id 03391 length = len(_x) 03392 buff.write(struct.pack('<I%ss'%length, length, _x)) 03393 _v252 = _v249.point 03394 _x = _v252 03395 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03396 _v253 = val1.sensor_pose 03397 _v254 = _v253.header 03398 buff.write(_struct_I.pack(_v254.seq)) 03399 _v255 = _v254.stamp 03400 _x = _v255 03401 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03402 _x = _v254.frame_id 03403 length = len(_x) 03404 buff.write(struct.pack('<I%ss'%length, length, _x)) 03405 _v256 = _v253.pose 03406 _v257 = _v256.position 03407 _x = _v257 03408 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03409 _v258 = _v256.orientation 03410 _x = _v258 03411 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03412 buff.write(_struct_d.pack(val1.absolute_tolerance)) 03413 length = len(self.additional_collision_operations.collision_operations) 03414 buff.write(_struct_I.pack(length)) 03415 for val1 in self.additional_collision_operations.collision_operations: 03416 _x = val1.object1 03417 length = len(_x) 03418 buff.write(struct.pack('<I%ss'%length, length, _x)) 03419 _x = val1.object2 03420 length = len(_x) 03421 buff.write(struct.pack('<I%ss'%length, length, _x)) 03422 _x = val1 03423 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 03424 length = len(self.additional_link_padding) 03425 buff.write(_struct_I.pack(length)) 03426 for val1 in self.additional_link_padding: 03427 _x = val1.link_name 03428 length = len(_x) 03429 buff.write(struct.pack('<I%ss'%length, length, _x)) 03430 buff.write(_struct_d.pack(val1.padding)) 03431 length = len(self.movable_obstacles) 03432 buff.write(_struct_I.pack(length)) 03433 for val1 in self.movable_obstacles: 03434 _x = val1.reference_frame_id 03435 length = len(_x) 03436 buff.write(struct.pack('<I%ss'%length, length, _x)) 03437 length = len(val1.potential_models) 03438 buff.write(_struct_I.pack(length)) 03439 for val2 in val1.potential_models: 03440 buff.write(_struct_i.pack(val2.model_id)) 03441 _v259 = val2.pose 03442 _v260 = _v259.header 03443 buff.write(_struct_I.pack(_v260.seq)) 03444 _v261 = _v260.stamp 03445 _x = _v261 03446 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03447 _x = _v260.frame_id 03448 length = len(_x) 03449 buff.write(struct.pack('<I%ss'%length, length, _x)) 03450 _v262 = _v259.pose 03451 _v263 = _v262.position 03452 _x = _v263 03453 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03454 _v264 = _v262.orientation 03455 _x = _v264 03456 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03457 buff.write(_struct_f.pack(val2.confidence)) 03458 _x = val2.detector_name 03459 length = len(_x) 03460 buff.write(struct.pack('<I%ss'%length, length, _x)) 03461 _v265 = val1.cluster 03462 _v266 = _v265.header 03463 buff.write(_struct_I.pack(_v266.seq)) 03464 _v267 = _v266.stamp 03465 _x = _v267 03466 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03467 _x = _v266.frame_id 03468 length = len(_x) 03469 buff.write(struct.pack('<I%ss'%length, length, _x)) 03470 length = len(_v265.points) 03471 buff.write(_struct_I.pack(length)) 03472 for val3 in _v265.points: 03473 _x = val3 03474 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 03475 length = len(_v265.channels) 03476 buff.write(_struct_I.pack(length)) 03477 for val3 in _v265.channels: 03478 _x = val3.name 03479 length = len(_x) 03480 buff.write(struct.pack('<I%ss'%length, length, _x)) 03481 length = len(val3.values) 03482 buff.write(_struct_I.pack(length)) 03483 pattern = '<%sf'%length 03484 buff.write(val3.values.tostring()) 03485 _v268 = val1.region 03486 _v269 = _v268.cloud 03487 _v270 = _v269.header 03488 buff.write(_struct_I.pack(_v270.seq)) 03489 _v271 = _v270.stamp 03490 _x = _v271 03491 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03492 _x = _v270.frame_id 03493 length = len(_x) 03494 buff.write(struct.pack('<I%ss'%length, length, _x)) 03495 _x = _v269 03496 buff.write(_struct_2I.pack(_x.height, _x.width)) 03497 length = len(_v269.fields) 03498 buff.write(_struct_I.pack(length)) 03499 for val4 in _v269.fields: 03500 _x = val4.name 03501 length = len(_x) 03502 buff.write(struct.pack('<I%ss'%length, length, _x)) 03503 _x = val4 03504 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 03505 _x = _v269 03506 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 03507 _x = _v269.data 03508 length = len(_x) 03509 # - if encoded as a list instead, serialize as bytes instead of string 03510 if type(_x) in [list, tuple]: 03511 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03512 else: 03513 buff.write(struct.pack('<I%ss'%length, length, _x)) 03514 buff.write(_struct_B.pack(_v269.is_dense)) 03515 length = len(_v268.mask) 03516 buff.write(_struct_I.pack(length)) 03517 pattern = '<%si'%length 03518 buff.write(_v268.mask.tostring()) 03519 _v272 = _v268.image 03520 _v273 = _v272.header 03521 buff.write(_struct_I.pack(_v273.seq)) 03522 _v274 = _v273.stamp 03523 _x = _v274 03524 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03525 _x = _v273.frame_id 03526 length = len(_x) 03527 buff.write(struct.pack('<I%ss'%length, length, _x)) 03528 _x = _v272 03529 buff.write(_struct_2I.pack(_x.height, _x.width)) 03530 _x = _v272.encoding 03531 length = len(_x) 03532 buff.write(struct.pack('<I%ss'%length, length, _x)) 03533 _x = _v272 03534 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 03535 _x = _v272.data 03536 length = len(_x) 03537 # - if encoded as a list instead, serialize as bytes instead of string 03538 if type(_x) in [list, tuple]: 03539 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03540 else: 03541 buff.write(struct.pack('<I%ss'%length, length, _x)) 03542 _v275 = _v268.disparity_image 03543 _v276 = _v275.header 03544 buff.write(_struct_I.pack(_v276.seq)) 03545 _v277 = _v276.stamp 03546 _x = _v277 03547 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03548 _x = _v276.frame_id 03549 length = len(_x) 03550 buff.write(struct.pack('<I%ss'%length, length, _x)) 03551 _x = _v275 03552 buff.write(_struct_2I.pack(_x.height, _x.width)) 03553 _x = _v275.encoding 03554 length = len(_x) 03555 buff.write(struct.pack('<I%ss'%length, length, _x)) 03556 _x = _v275 03557 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 03558 _x = _v275.data 03559 length = len(_x) 03560 # - if encoded as a list instead, serialize as bytes instead of string 03561 if type(_x) in [list, tuple]: 03562 buff.write(struct.pack('<I%sB'%length, length, *_x)) 03563 else: 03564 buff.write(struct.pack('<I%ss'%length, length, _x)) 03565 _v278 = _v268.cam_info 03566 _v279 = _v278.header 03567 buff.write(_struct_I.pack(_v279.seq)) 03568 _v280 = _v279.stamp 03569 _x = _v280 03570 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03571 _x = _v279.frame_id 03572 length = len(_x) 03573 buff.write(struct.pack('<I%ss'%length, length, _x)) 03574 _x = _v278 03575 buff.write(_struct_2I.pack(_x.height, _x.width)) 03576 _x = _v278.distortion_model 03577 length = len(_x) 03578 buff.write(struct.pack('<I%ss'%length, length, _x)) 03579 length = len(_v278.D) 03580 buff.write(_struct_I.pack(length)) 03581 pattern = '<%sd'%length 03582 buff.write(_v278.D.tostring()) 03583 buff.write(_v278.K.tostring()) 03584 buff.write(_v278.R.tostring()) 03585 buff.write(_v278.P.tostring()) 03586 _x = _v278 03587 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 03588 _v281 = _v278.roi 03589 _x = _v281 03590 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 03591 _v282 = _v268.roi_box_pose 03592 _v283 = _v282.header 03593 buff.write(_struct_I.pack(_v283.seq)) 03594 _v284 = _v283.stamp 03595 _x = _v284 03596 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 03597 _x = _v283.frame_id 03598 length = len(_x) 03599 buff.write(struct.pack('<I%ss'%length, length, _x)) 03600 _v285 = _v282.pose 03601 _v286 = _v285.position 03602 _x = _v286 03603 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03604 _v287 = _v285.orientation 03605 _x = _v287 03606 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 03607 _v288 = _v268.roi_box_dims 03608 _x = _v288 03609 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 03610 _x = val1.collision_name 03611 length = len(_x) 03612 buff.write(struct.pack('<I%ss'%length, length, _x)) 03613 buff.write(_struct_f.pack(self.max_contact_force)) 03614 except struct.error as se: self._check_types(se) 03615 except TypeError as te: self._check_types(te) 03616 03617 def deserialize_numpy(self, str, numpy): 03618 """ 03619 unpack serialized message in str into this message instance using numpy for array types 03620 @param str: byte array of serialized message 03621 @type str: str 03622 @param numpy: numpy python module 03623 @type numpy: module 03624 """ 03625 try: 03626 if self.target is None: 03627 self.target = object_manipulation_msgs.msg.GraspableObject() 03628 if self.lift is None: 03629 self.lift = object_manipulation_msgs.msg.GripperTranslation() 03630 if self.path_constraints is None: 03631 self.path_constraints = arm_navigation_msgs.msg.Constraints() 03632 if self.additional_collision_operations is None: 03633 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations() 03634 end = 0 03635 start = end 03636 end += 4 03637 (length,) = _struct_I.unpack(str[start:end]) 03638 start = end 03639 end += length 03640 self.arm_name = str[start:end] 03641 start = end 03642 end += 4 03643 (length,) = _struct_I.unpack(str[start:end]) 03644 start = end 03645 end += length 03646 self.target.reference_frame_id = str[start:end] 03647 start = end 03648 end += 4 03649 (length,) = _struct_I.unpack(str[start:end]) 03650 self.target.potential_models = [] 03651 for i in range(0, length): 03652 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 03653 start = end 03654 end += 4 03655 (val1.model_id,) = _struct_i.unpack(str[start:end]) 03656 _v289 = val1.pose 03657 _v290 = _v289.header 03658 start = end 03659 end += 4 03660 (_v290.seq,) = _struct_I.unpack(str[start:end]) 03661 _v291 = _v290.stamp 03662 _x = _v291 03663 start = end 03664 end += 8 03665 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03666 start = end 03667 end += 4 03668 (length,) = _struct_I.unpack(str[start:end]) 03669 start = end 03670 end += length 03671 _v290.frame_id = str[start:end] 03672 _v292 = _v289.pose 03673 _v293 = _v292.position 03674 _x = _v293 03675 start = end 03676 end += 24 03677 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03678 _v294 = _v292.orientation 03679 _x = _v294 03680 start = end 03681 end += 32 03682 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03683 start = end 03684 end += 4 03685 (val1.confidence,) = _struct_f.unpack(str[start:end]) 03686 start = end 03687 end += 4 03688 (length,) = _struct_I.unpack(str[start:end]) 03689 start = end 03690 end += length 03691 val1.detector_name = str[start:end] 03692 self.target.potential_models.append(val1) 03693 _x = self 03694 start = end 03695 end += 12 03696 (_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03697 start = end 03698 end += 4 03699 (length,) = _struct_I.unpack(str[start:end]) 03700 start = end 03701 end += length 03702 self.target.cluster.header.frame_id = str[start:end] 03703 start = end 03704 end += 4 03705 (length,) = _struct_I.unpack(str[start:end]) 03706 self.target.cluster.points = [] 03707 for i in range(0, length): 03708 val1 = geometry_msgs.msg.Point32() 03709 _x = val1 03710 start = end 03711 end += 12 03712 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03713 self.target.cluster.points.append(val1) 03714 start = end 03715 end += 4 03716 (length,) = _struct_I.unpack(str[start:end]) 03717 self.target.cluster.channels = [] 03718 for i in range(0, length): 03719 val1 = sensor_msgs.msg.ChannelFloat32() 03720 start = end 03721 end += 4 03722 (length,) = _struct_I.unpack(str[start:end]) 03723 start = end 03724 end += length 03725 val1.name = str[start:end] 03726 start = end 03727 end += 4 03728 (length,) = _struct_I.unpack(str[start:end]) 03729 pattern = '<%sf'%length 03730 start = end 03731 end += struct.calcsize(pattern) 03732 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 03733 self.target.cluster.channels.append(val1) 03734 _x = self 03735 start = end 03736 end += 12 03737 (_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03738 start = end 03739 end += 4 03740 (length,) = _struct_I.unpack(str[start:end]) 03741 start = end 03742 end += length 03743 self.target.region.cloud.header.frame_id = str[start:end] 03744 _x = self 03745 start = end 03746 end += 8 03747 (_x.target.region.cloud.height, _x.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 03748 start = end 03749 end += 4 03750 (length,) = _struct_I.unpack(str[start:end]) 03751 self.target.region.cloud.fields = [] 03752 for i in range(0, length): 03753 val1 = sensor_msgs.msg.PointField() 03754 start = end 03755 end += 4 03756 (length,) = _struct_I.unpack(str[start:end]) 03757 start = end 03758 end += length 03759 val1.name = str[start:end] 03760 _x = val1 03761 start = end 03762 end += 9 03763 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03764 self.target.region.cloud.fields.append(val1) 03765 _x = self 03766 start = end 03767 end += 9 03768 (_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 03769 self.target.region.cloud.is_bigendian = bool(self.target.region.cloud.is_bigendian) 03770 start = end 03771 end += 4 03772 (length,) = _struct_I.unpack(str[start:end]) 03773 start = end 03774 end += length 03775 self.target.region.cloud.data = str[start:end] 03776 start = end 03777 end += 1 03778 (self.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 03779 self.target.region.cloud.is_dense = bool(self.target.region.cloud.is_dense) 03780 start = end 03781 end += 4 03782 (length,) = _struct_I.unpack(str[start:end]) 03783 pattern = '<%si'%length 03784 start = end 03785 end += struct.calcsize(pattern) 03786 self.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03787 _x = self 03788 start = end 03789 end += 12 03790 (_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03791 start = end 03792 end += 4 03793 (length,) = _struct_I.unpack(str[start:end]) 03794 start = end 03795 end += length 03796 self.target.region.image.header.frame_id = str[start:end] 03797 _x = self 03798 start = end 03799 end += 8 03800 (_x.target.region.image.height, _x.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 03801 start = end 03802 end += 4 03803 (length,) = _struct_I.unpack(str[start:end]) 03804 start = end 03805 end += length 03806 self.target.region.image.encoding = str[start:end] 03807 _x = self 03808 start = end 03809 end += 5 03810 (_x.target.region.image.is_bigendian, _x.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 03811 start = end 03812 end += 4 03813 (length,) = _struct_I.unpack(str[start:end]) 03814 start = end 03815 end += length 03816 self.target.region.image.data = str[start:end] 03817 _x = self 03818 start = end 03819 end += 12 03820 (_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03821 start = end 03822 end += 4 03823 (length,) = _struct_I.unpack(str[start:end]) 03824 start = end 03825 end += length 03826 self.target.region.disparity_image.header.frame_id = str[start:end] 03827 _x = self 03828 start = end 03829 end += 8 03830 (_x.target.region.disparity_image.height, _x.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 03831 start = end 03832 end += 4 03833 (length,) = _struct_I.unpack(str[start:end]) 03834 start = end 03835 end += length 03836 self.target.region.disparity_image.encoding = str[start:end] 03837 _x = self 03838 start = end 03839 end += 5 03840 (_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 03841 start = end 03842 end += 4 03843 (length,) = _struct_I.unpack(str[start:end]) 03844 start = end 03845 end += length 03846 self.target.region.disparity_image.data = str[start:end] 03847 _x = self 03848 start = end 03849 end += 12 03850 (_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03851 start = end 03852 end += 4 03853 (length,) = _struct_I.unpack(str[start:end]) 03854 start = end 03855 end += length 03856 self.target.region.cam_info.header.frame_id = str[start:end] 03857 _x = self 03858 start = end 03859 end += 8 03860 (_x.target.region.cam_info.height, _x.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 03861 start = end 03862 end += 4 03863 (length,) = _struct_I.unpack(str[start:end]) 03864 start = end 03865 end += length 03866 self.target.region.cam_info.distortion_model = str[start:end] 03867 start = end 03868 end += 4 03869 (length,) = _struct_I.unpack(str[start:end]) 03870 pattern = '<%sd'%length 03871 start = end 03872 end += struct.calcsize(pattern) 03873 self.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03874 start = end 03875 end += 72 03876 self.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03877 start = end 03878 end += 72 03879 self.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03880 start = end 03881 end += 96 03882 self.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 03883 _x = self 03884 start = end 03885 end += 37 03886 (_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 03887 self.target.region.cam_info.roi.do_rectify = bool(self.target.region.cam_info.roi.do_rectify) 03888 start = end 03889 end += 4 03890 (length,) = _struct_I.unpack(str[start:end]) 03891 start = end 03892 end += length 03893 self.target.region.roi_box_pose.header.frame_id = str[start:end] 03894 _x = self 03895 start = end 03896 end += 80 03897 (_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 03898 start = end 03899 end += 4 03900 (length,) = _struct_I.unpack(str[start:end]) 03901 start = end 03902 end += length 03903 self.target.collision_name = str[start:end] 03904 start = end 03905 end += 4 03906 (length,) = _struct_I.unpack(str[start:end]) 03907 self.desired_grasps = [] 03908 for i in range(0, length): 03909 val1 = object_manipulation_msgs.msg.Grasp() 03910 _v295 = val1.pre_grasp_posture 03911 _v296 = _v295.header 03912 start = end 03913 end += 4 03914 (_v296.seq,) = _struct_I.unpack(str[start:end]) 03915 _v297 = _v296.stamp 03916 _x = _v297 03917 start = end 03918 end += 8 03919 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03920 start = end 03921 end += 4 03922 (length,) = _struct_I.unpack(str[start:end]) 03923 start = end 03924 end += length 03925 _v296.frame_id = str[start:end] 03926 start = end 03927 end += 4 03928 (length,) = _struct_I.unpack(str[start:end]) 03929 _v295.name = [] 03930 for i in range(0, length): 03931 start = end 03932 end += 4 03933 (length,) = _struct_I.unpack(str[start:end]) 03934 start = end 03935 end += length 03936 val3 = str[start:end] 03937 _v295.name.append(val3) 03938 start = end 03939 end += 4 03940 (length,) = _struct_I.unpack(str[start:end]) 03941 pattern = '<%sd'%length 03942 start = end 03943 end += struct.calcsize(pattern) 03944 _v295.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03945 start = end 03946 end += 4 03947 (length,) = _struct_I.unpack(str[start:end]) 03948 pattern = '<%sd'%length 03949 start = end 03950 end += struct.calcsize(pattern) 03951 _v295.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03952 start = end 03953 end += 4 03954 (length,) = _struct_I.unpack(str[start:end]) 03955 pattern = '<%sd'%length 03956 start = end 03957 end += struct.calcsize(pattern) 03958 _v295.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03959 _v298 = val1.grasp_posture 03960 _v299 = _v298.header 03961 start = end 03962 end += 4 03963 (_v299.seq,) = _struct_I.unpack(str[start:end]) 03964 _v300 = _v299.stamp 03965 _x = _v300 03966 start = end 03967 end += 8 03968 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03969 start = end 03970 end += 4 03971 (length,) = _struct_I.unpack(str[start:end]) 03972 start = end 03973 end += length 03974 _v299.frame_id = str[start:end] 03975 start = end 03976 end += 4 03977 (length,) = _struct_I.unpack(str[start:end]) 03978 _v298.name = [] 03979 for i in range(0, length): 03980 start = end 03981 end += 4 03982 (length,) = _struct_I.unpack(str[start:end]) 03983 start = end 03984 end += length 03985 val3 = str[start:end] 03986 _v298.name.append(val3) 03987 start = end 03988 end += 4 03989 (length,) = _struct_I.unpack(str[start:end]) 03990 pattern = '<%sd'%length 03991 start = end 03992 end += struct.calcsize(pattern) 03993 _v298.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03994 start = end 03995 end += 4 03996 (length,) = _struct_I.unpack(str[start:end]) 03997 pattern = '<%sd'%length 03998 start = end 03999 end += struct.calcsize(pattern) 04000 _v298.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 04001 start = end 04002 end += 4 04003 (length,) = _struct_I.unpack(str[start:end]) 04004 pattern = '<%sd'%length 04005 start = end 04006 end += struct.calcsize(pattern) 04007 _v298.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 04008 _v301 = val1.grasp_pose 04009 _v302 = _v301.position 04010 _x = _v302 04011 start = end 04012 end += 24 04013 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04014 _v303 = _v301.orientation 04015 _x = _v303 04016 start = end 04017 end += 32 04018 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04019 _x = val1 04020 start = end 04021 end += 17 04022 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end]) 04023 val1.cluster_rep = bool(val1.cluster_rep) 04024 start = end 04025 end += 4 04026 (length,) = _struct_I.unpack(str[start:end]) 04027 val1.moved_obstacles = [] 04028 for i in range(0, length): 04029 val2 = object_manipulation_msgs.msg.GraspableObject() 04030 start = end 04031 end += 4 04032 (length,) = _struct_I.unpack(str[start:end]) 04033 start = end 04034 end += length 04035 val2.reference_frame_id = str[start:end] 04036 start = end 04037 end += 4 04038 (length,) = _struct_I.unpack(str[start:end]) 04039 val2.potential_models = [] 04040 for i in range(0, length): 04041 val3 = household_objects_database_msgs.msg.DatabaseModelPose() 04042 start = end 04043 end += 4 04044 (val3.model_id,) = _struct_i.unpack(str[start:end]) 04045 _v304 = val3.pose 04046 _v305 = _v304.header 04047 start = end 04048 end += 4 04049 (_v305.seq,) = _struct_I.unpack(str[start:end]) 04050 _v306 = _v305.stamp 04051 _x = _v306 04052 start = end 04053 end += 8 04054 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04055 start = end 04056 end += 4 04057 (length,) = _struct_I.unpack(str[start:end]) 04058 start = end 04059 end += length 04060 _v305.frame_id = str[start:end] 04061 _v307 = _v304.pose 04062 _v308 = _v307.position 04063 _x = _v308 04064 start = end 04065 end += 24 04066 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04067 _v309 = _v307.orientation 04068 _x = _v309 04069 start = end 04070 end += 32 04071 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04072 start = end 04073 end += 4 04074 (val3.confidence,) = _struct_f.unpack(str[start:end]) 04075 start = end 04076 end += 4 04077 (length,) = _struct_I.unpack(str[start:end]) 04078 start = end 04079 end += length 04080 val3.detector_name = str[start:end] 04081 val2.potential_models.append(val3) 04082 _v310 = val2.cluster 04083 _v311 = _v310.header 04084 start = end 04085 end += 4 04086 (_v311.seq,) = _struct_I.unpack(str[start:end]) 04087 _v312 = _v311.stamp 04088 _x = _v312 04089 start = end 04090 end += 8 04091 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04092 start = end 04093 end += 4 04094 (length,) = _struct_I.unpack(str[start:end]) 04095 start = end 04096 end += length 04097 _v311.frame_id = str[start:end] 04098 start = end 04099 end += 4 04100 (length,) = _struct_I.unpack(str[start:end]) 04101 _v310.points = [] 04102 for i in range(0, length): 04103 val4 = geometry_msgs.msg.Point32() 04104 _x = val4 04105 start = end 04106 end += 12 04107 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 04108 _v310.points.append(val4) 04109 start = end 04110 end += 4 04111 (length,) = _struct_I.unpack(str[start:end]) 04112 _v310.channels = [] 04113 for i in range(0, length): 04114 val4 = sensor_msgs.msg.ChannelFloat32() 04115 start = end 04116 end += 4 04117 (length,) = _struct_I.unpack(str[start:end]) 04118 start = end 04119 end += length 04120 val4.name = str[start:end] 04121 start = end 04122 end += 4 04123 (length,) = _struct_I.unpack(str[start:end]) 04124 pattern = '<%sf'%length 04125 start = end 04126 end += struct.calcsize(pattern) 04127 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 04128 _v310.channels.append(val4) 04129 _v313 = val2.region 04130 _v314 = _v313.cloud 04131 _v315 = _v314.header 04132 start = end 04133 end += 4 04134 (_v315.seq,) = _struct_I.unpack(str[start:end]) 04135 _v316 = _v315.stamp 04136 _x = _v316 04137 start = end 04138 end += 8 04139 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04140 start = end 04141 end += 4 04142 (length,) = _struct_I.unpack(str[start:end]) 04143 start = end 04144 end += length 04145 _v315.frame_id = str[start:end] 04146 _x = _v314 04147 start = end 04148 end += 8 04149 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04150 start = end 04151 end += 4 04152 (length,) = _struct_I.unpack(str[start:end]) 04153 _v314.fields = [] 04154 for i in range(0, length): 04155 val5 = sensor_msgs.msg.PointField() 04156 start = end 04157 end += 4 04158 (length,) = _struct_I.unpack(str[start:end]) 04159 start = end 04160 end += length 04161 val5.name = str[start:end] 04162 _x = val5 04163 start = end 04164 end += 9 04165 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 04166 _v314.fields.append(val5) 04167 _x = _v314 04168 start = end 04169 end += 9 04170 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 04171 _v314.is_bigendian = bool(_v314.is_bigendian) 04172 start = end 04173 end += 4 04174 (length,) = _struct_I.unpack(str[start:end]) 04175 start = end 04176 end += length 04177 _v314.data = str[start:end] 04178 start = end 04179 end += 1 04180 (_v314.is_dense,) = _struct_B.unpack(str[start:end]) 04181 _v314.is_dense = bool(_v314.is_dense) 04182 start = end 04183 end += 4 04184 (length,) = _struct_I.unpack(str[start:end]) 04185 pattern = '<%si'%length 04186 start = end 04187 end += struct.calcsize(pattern) 04188 _v313.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 04189 _v317 = _v313.image 04190 _v318 = _v317.header 04191 start = end 04192 end += 4 04193 (_v318.seq,) = _struct_I.unpack(str[start:end]) 04194 _v319 = _v318.stamp 04195 _x = _v319 04196 start = end 04197 end += 8 04198 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04199 start = end 04200 end += 4 04201 (length,) = _struct_I.unpack(str[start:end]) 04202 start = end 04203 end += length 04204 _v318.frame_id = str[start:end] 04205 _x = _v317 04206 start = end 04207 end += 8 04208 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04209 start = end 04210 end += 4 04211 (length,) = _struct_I.unpack(str[start:end]) 04212 start = end 04213 end += length 04214 _v317.encoding = str[start:end] 04215 _x = _v317 04216 start = end 04217 end += 5 04218 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 04219 start = end 04220 end += 4 04221 (length,) = _struct_I.unpack(str[start:end]) 04222 start = end 04223 end += length 04224 _v317.data = str[start:end] 04225 _v320 = _v313.disparity_image 04226 _v321 = _v320.header 04227 start = end 04228 end += 4 04229 (_v321.seq,) = _struct_I.unpack(str[start:end]) 04230 _v322 = _v321.stamp 04231 _x = _v322 04232 start = end 04233 end += 8 04234 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04235 start = end 04236 end += 4 04237 (length,) = _struct_I.unpack(str[start:end]) 04238 start = end 04239 end += length 04240 _v321.frame_id = str[start:end] 04241 _x = _v320 04242 start = end 04243 end += 8 04244 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04245 start = end 04246 end += 4 04247 (length,) = _struct_I.unpack(str[start:end]) 04248 start = end 04249 end += length 04250 _v320.encoding = str[start:end] 04251 _x = _v320 04252 start = end 04253 end += 5 04254 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 04255 start = end 04256 end += 4 04257 (length,) = _struct_I.unpack(str[start:end]) 04258 start = end 04259 end += length 04260 _v320.data = str[start:end] 04261 _v323 = _v313.cam_info 04262 _v324 = _v323.header 04263 start = end 04264 end += 4 04265 (_v324.seq,) = _struct_I.unpack(str[start:end]) 04266 _v325 = _v324.stamp 04267 _x = _v325 04268 start = end 04269 end += 8 04270 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04271 start = end 04272 end += 4 04273 (length,) = _struct_I.unpack(str[start:end]) 04274 start = end 04275 end += length 04276 _v324.frame_id = str[start:end] 04277 _x = _v323 04278 start = end 04279 end += 8 04280 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04281 start = end 04282 end += 4 04283 (length,) = _struct_I.unpack(str[start:end]) 04284 start = end 04285 end += length 04286 _v323.distortion_model = str[start:end] 04287 start = end 04288 end += 4 04289 (length,) = _struct_I.unpack(str[start:end]) 04290 pattern = '<%sd'%length 04291 start = end 04292 end += struct.calcsize(pattern) 04293 _v323.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 04294 start = end 04295 end += 72 04296 _v323.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 04297 start = end 04298 end += 72 04299 _v323.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 04300 start = end 04301 end += 96 04302 _v323.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 04303 _x = _v323 04304 start = end 04305 end += 8 04306 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 04307 _v326 = _v323.roi 04308 _x = _v326 04309 start = end 04310 end += 17 04311 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 04312 _v326.do_rectify = bool(_v326.do_rectify) 04313 _v327 = _v313.roi_box_pose 04314 _v328 = _v327.header 04315 start = end 04316 end += 4 04317 (_v328.seq,) = _struct_I.unpack(str[start:end]) 04318 _v329 = _v328.stamp 04319 _x = _v329 04320 start = end 04321 end += 8 04322 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04323 start = end 04324 end += 4 04325 (length,) = _struct_I.unpack(str[start:end]) 04326 start = end 04327 end += length 04328 _v328.frame_id = str[start:end] 04329 _v330 = _v327.pose 04330 _v331 = _v330.position 04331 _x = _v331 04332 start = end 04333 end += 24 04334 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04335 _v332 = _v330.orientation 04336 _x = _v332 04337 start = end 04338 end += 32 04339 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04340 _v333 = _v313.roi_box_dims 04341 _x = _v333 04342 start = end 04343 end += 24 04344 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04345 start = end 04346 end += 4 04347 (length,) = _struct_I.unpack(str[start:end]) 04348 start = end 04349 end += length 04350 val2.collision_name = str[start:end] 04351 val1.moved_obstacles.append(val2) 04352 self.desired_grasps.append(val1) 04353 _x = self 04354 start = end 04355 end += 12 04356 (_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 04357 start = end 04358 end += 4 04359 (length,) = _struct_I.unpack(str[start:end]) 04360 start = end 04361 end += length 04362 self.lift.direction.header.frame_id = str[start:end] 04363 _x = self 04364 start = end 04365 end += 32 04366 (_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance,) = _struct_3d2f.unpack(str[start:end]) 04367 start = end 04368 end += 4 04369 (length,) = _struct_I.unpack(str[start:end]) 04370 start = end 04371 end += length 04372 self.collision_object_name = str[start:end] 04373 start = end 04374 end += 4 04375 (length,) = _struct_I.unpack(str[start:end]) 04376 start = end 04377 end += length 04378 self.collision_support_surface_name = str[start:end] 04379 _x = self 04380 start = end 04381 end += 5 04382 (_x.allow_gripper_support_collision, _x.use_reactive_execution, _x.use_reactive_lift, _x.only_perform_feasibility_test, _x.ignore_collisions,) = _struct_5B.unpack(str[start:end]) 04383 self.allow_gripper_support_collision = bool(self.allow_gripper_support_collision) 04384 self.use_reactive_execution = bool(self.use_reactive_execution) 04385 self.use_reactive_lift = bool(self.use_reactive_lift) 04386 self.only_perform_feasibility_test = bool(self.only_perform_feasibility_test) 04387 self.ignore_collisions = bool(self.ignore_collisions) 04388 start = end 04389 end += 4 04390 (length,) = _struct_I.unpack(str[start:end]) 04391 self.path_constraints.joint_constraints = [] 04392 for i in range(0, length): 04393 val1 = arm_navigation_msgs.msg.JointConstraint() 04394 start = end 04395 end += 4 04396 (length,) = _struct_I.unpack(str[start:end]) 04397 start = end 04398 end += length 04399 val1.joint_name = str[start:end] 04400 _x = val1 04401 start = end 04402 end += 32 04403 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 04404 self.path_constraints.joint_constraints.append(val1) 04405 start = end 04406 end += 4 04407 (length,) = _struct_I.unpack(str[start:end]) 04408 self.path_constraints.position_constraints = [] 04409 for i in range(0, length): 04410 val1 = arm_navigation_msgs.msg.PositionConstraint() 04411 _v334 = val1.header 04412 start = end 04413 end += 4 04414 (_v334.seq,) = _struct_I.unpack(str[start:end]) 04415 _v335 = _v334.stamp 04416 _x = _v335 04417 start = end 04418 end += 8 04419 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04420 start = end 04421 end += 4 04422 (length,) = _struct_I.unpack(str[start:end]) 04423 start = end 04424 end += length 04425 _v334.frame_id = str[start:end] 04426 start = end 04427 end += 4 04428 (length,) = _struct_I.unpack(str[start:end]) 04429 start = end 04430 end += length 04431 val1.link_name = str[start:end] 04432 _v336 = val1.target_point_offset 04433 _x = _v336 04434 start = end 04435 end += 24 04436 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04437 _v337 = val1.position 04438 _x = _v337 04439 start = end 04440 end += 24 04441 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04442 _v338 = val1.constraint_region_shape 04443 start = end 04444 end += 1 04445 (_v338.type,) = _struct_b.unpack(str[start:end]) 04446 start = end 04447 end += 4 04448 (length,) = _struct_I.unpack(str[start:end]) 04449 pattern = '<%sd'%length 04450 start = end 04451 end += struct.calcsize(pattern) 04452 _v338.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 04453 start = end 04454 end += 4 04455 (length,) = _struct_I.unpack(str[start:end]) 04456 pattern = '<%si'%length 04457 start = end 04458 end += struct.calcsize(pattern) 04459 _v338.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 04460 start = end 04461 end += 4 04462 (length,) = _struct_I.unpack(str[start:end]) 04463 _v338.vertices = [] 04464 for i in range(0, length): 04465 val3 = geometry_msgs.msg.Point() 04466 _x = val3 04467 start = end 04468 end += 24 04469 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04470 _v338.vertices.append(val3) 04471 _v339 = val1.constraint_region_orientation 04472 _x = _v339 04473 start = end 04474 end += 32 04475 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04476 start = end 04477 end += 8 04478 (val1.weight,) = _struct_d.unpack(str[start:end]) 04479 self.path_constraints.position_constraints.append(val1) 04480 start = end 04481 end += 4 04482 (length,) = _struct_I.unpack(str[start:end]) 04483 self.path_constraints.orientation_constraints = [] 04484 for i in range(0, length): 04485 val1 = arm_navigation_msgs.msg.OrientationConstraint() 04486 _v340 = val1.header 04487 start = end 04488 end += 4 04489 (_v340.seq,) = _struct_I.unpack(str[start:end]) 04490 _v341 = _v340.stamp 04491 _x = _v341 04492 start = end 04493 end += 8 04494 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04495 start = end 04496 end += 4 04497 (length,) = _struct_I.unpack(str[start:end]) 04498 start = end 04499 end += length 04500 _v340.frame_id = str[start:end] 04501 start = end 04502 end += 4 04503 (length,) = _struct_I.unpack(str[start:end]) 04504 start = end 04505 end += length 04506 val1.link_name = str[start:end] 04507 start = end 04508 end += 4 04509 (val1.type,) = _struct_i.unpack(str[start:end]) 04510 _v342 = val1.orientation 04511 _x = _v342 04512 start = end 04513 end += 32 04514 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04515 _x = val1 04516 start = end 04517 end += 32 04518 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 04519 self.path_constraints.orientation_constraints.append(val1) 04520 start = end 04521 end += 4 04522 (length,) = _struct_I.unpack(str[start:end]) 04523 self.path_constraints.visibility_constraints = [] 04524 for i in range(0, length): 04525 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 04526 _v343 = val1.header 04527 start = end 04528 end += 4 04529 (_v343.seq,) = _struct_I.unpack(str[start:end]) 04530 _v344 = _v343.stamp 04531 _x = _v344 04532 start = end 04533 end += 8 04534 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04535 start = end 04536 end += 4 04537 (length,) = _struct_I.unpack(str[start:end]) 04538 start = end 04539 end += length 04540 _v343.frame_id = str[start:end] 04541 _v345 = val1.target 04542 _v346 = _v345.header 04543 start = end 04544 end += 4 04545 (_v346.seq,) = _struct_I.unpack(str[start:end]) 04546 _v347 = _v346.stamp 04547 _x = _v347 04548 start = end 04549 end += 8 04550 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04551 start = end 04552 end += 4 04553 (length,) = _struct_I.unpack(str[start:end]) 04554 start = end 04555 end += length 04556 _v346.frame_id = str[start:end] 04557 _v348 = _v345.point 04558 _x = _v348 04559 start = end 04560 end += 24 04561 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04562 _v349 = val1.sensor_pose 04563 _v350 = _v349.header 04564 start = end 04565 end += 4 04566 (_v350.seq,) = _struct_I.unpack(str[start:end]) 04567 _v351 = _v350.stamp 04568 _x = _v351 04569 start = end 04570 end += 8 04571 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04572 start = end 04573 end += 4 04574 (length,) = _struct_I.unpack(str[start:end]) 04575 start = end 04576 end += length 04577 _v350.frame_id = str[start:end] 04578 _v352 = _v349.pose 04579 _v353 = _v352.position 04580 _x = _v353 04581 start = end 04582 end += 24 04583 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04584 _v354 = _v352.orientation 04585 _x = _v354 04586 start = end 04587 end += 32 04588 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04589 start = end 04590 end += 8 04591 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 04592 self.path_constraints.visibility_constraints.append(val1) 04593 start = end 04594 end += 4 04595 (length,) = _struct_I.unpack(str[start:end]) 04596 self.additional_collision_operations.collision_operations = [] 04597 for i in range(0, length): 04598 val1 = arm_navigation_msgs.msg.CollisionOperation() 04599 start = end 04600 end += 4 04601 (length,) = _struct_I.unpack(str[start:end]) 04602 start = end 04603 end += length 04604 val1.object1 = str[start:end] 04605 start = end 04606 end += 4 04607 (length,) = _struct_I.unpack(str[start:end]) 04608 start = end 04609 end += length 04610 val1.object2 = str[start:end] 04611 _x = val1 04612 start = end 04613 end += 12 04614 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 04615 self.additional_collision_operations.collision_operations.append(val1) 04616 start = end 04617 end += 4 04618 (length,) = _struct_I.unpack(str[start:end]) 04619 self.additional_link_padding = [] 04620 for i in range(0, length): 04621 val1 = arm_navigation_msgs.msg.LinkPadding() 04622 start = end 04623 end += 4 04624 (length,) = _struct_I.unpack(str[start:end]) 04625 start = end 04626 end += length 04627 val1.link_name = str[start:end] 04628 start = end 04629 end += 8 04630 (val1.padding,) = _struct_d.unpack(str[start:end]) 04631 self.additional_link_padding.append(val1) 04632 start = end 04633 end += 4 04634 (length,) = _struct_I.unpack(str[start:end]) 04635 self.movable_obstacles = [] 04636 for i in range(0, length): 04637 val1 = object_manipulation_msgs.msg.GraspableObject() 04638 start = end 04639 end += 4 04640 (length,) = _struct_I.unpack(str[start:end]) 04641 start = end 04642 end += length 04643 val1.reference_frame_id = str[start:end] 04644 start = end 04645 end += 4 04646 (length,) = _struct_I.unpack(str[start:end]) 04647 val1.potential_models = [] 04648 for i in range(0, length): 04649 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 04650 start = end 04651 end += 4 04652 (val2.model_id,) = _struct_i.unpack(str[start:end]) 04653 _v355 = val2.pose 04654 _v356 = _v355.header 04655 start = end 04656 end += 4 04657 (_v356.seq,) = _struct_I.unpack(str[start:end]) 04658 _v357 = _v356.stamp 04659 _x = _v357 04660 start = end 04661 end += 8 04662 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04663 start = end 04664 end += 4 04665 (length,) = _struct_I.unpack(str[start:end]) 04666 start = end 04667 end += length 04668 _v356.frame_id = str[start:end] 04669 _v358 = _v355.pose 04670 _v359 = _v358.position 04671 _x = _v359 04672 start = end 04673 end += 24 04674 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04675 _v360 = _v358.orientation 04676 _x = _v360 04677 start = end 04678 end += 32 04679 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04680 start = end 04681 end += 4 04682 (val2.confidence,) = _struct_f.unpack(str[start:end]) 04683 start = end 04684 end += 4 04685 (length,) = _struct_I.unpack(str[start:end]) 04686 start = end 04687 end += length 04688 val2.detector_name = str[start:end] 04689 val1.potential_models.append(val2) 04690 _v361 = val1.cluster 04691 _v362 = _v361.header 04692 start = end 04693 end += 4 04694 (_v362.seq,) = _struct_I.unpack(str[start:end]) 04695 _v363 = _v362.stamp 04696 _x = _v363 04697 start = end 04698 end += 8 04699 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04700 start = end 04701 end += 4 04702 (length,) = _struct_I.unpack(str[start:end]) 04703 start = end 04704 end += length 04705 _v362.frame_id = str[start:end] 04706 start = end 04707 end += 4 04708 (length,) = _struct_I.unpack(str[start:end]) 04709 _v361.points = [] 04710 for i in range(0, length): 04711 val3 = geometry_msgs.msg.Point32() 04712 _x = val3 04713 start = end 04714 end += 12 04715 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 04716 _v361.points.append(val3) 04717 start = end 04718 end += 4 04719 (length,) = _struct_I.unpack(str[start:end]) 04720 _v361.channels = [] 04721 for i in range(0, length): 04722 val3 = sensor_msgs.msg.ChannelFloat32() 04723 start = end 04724 end += 4 04725 (length,) = _struct_I.unpack(str[start:end]) 04726 start = end 04727 end += length 04728 val3.name = str[start:end] 04729 start = end 04730 end += 4 04731 (length,) = _struct_I.unpack(str[start:end]) 04732 pattern = '<%sf'%length 04733 start = end 04734 end += struct.calcsize(pattern) 04735 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 04736 _v361.channels.append(val3) 04737 _v364 = val1.region 04738 _v365 = _v364.cloud 04739 _v366 = _v365.header 04740 start = end 04741 end += 4 04742 (_v366.seq,) = _struct_I.unpack(str[start:end]) 04743 _v367 = _v366.stamp 04744 _x = _v367 04745 start = end 04746 end += 8 04747 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04748 start = end 04749 end += 4 04750 (length,) = _struct_I.unpack(str[start:end]) 04751 start = end 04752 end += length 04753 _v366.frame_id = str[start:end] 04754 _x = _v365 04755 start = end 04756 end += 8 04757 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04758 start = end 04759 end += 4 04760 (length,) = _struct_I.unpack(str[start:end]) 04761 _v365.fields = [] 04762 for i in range(0, length): 04763 val4 = sensor_msgs.msg.PointField() 04764 start = end 04765 end += 4 04766 (length,) = _struct_I.unpack(str[start:end]) 04767 start = end 04768 end += length 04769 val4.name = str[start:end] 04770 _x = val4 04771 start = end 04772 end += 9 04773 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 04774 _v365.fields.append(val4) 04775 _x = _v365 04776 start = end 04777 end += 9 04778 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 04779 _v365.is_bigendian = bool(_v365.is_bigendian) 04780 start = end 04781 end += 4 04782 (length,) = _struct_I.unpack(str[start:end]) 04783 start = end 04784 end += length 04785 _v365.data = str[start:end] 04786 start = end 04787 end += 1 04788 (_v365.is_dense,) = _struct_B.unpack(str[start:end]) 04789 _v365.is_dense = bool(_v365.is_dense) 04790 start = end 04791 end += 4 04792 (length,) = _struct_I.unpack(str[start:end]) 04793 pattern = '<%si'%length 04794 start = end 04795 end += struct.calcsize(pattern) 04796 _v364.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 04797 _v368 = _v364.image 04798 _v369 = _v368.header 04799 start = end 04800 end += 4 04801 (_v369.seq,) = _struct_I.unpack(str[start:end]) 04802 _v370 = _v369.stamp 04803 _x = _v370 04804 start = end 04805 end += 8 04806 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04807 start = end 04808 end += 4 04809 (length,) = _struct_I.unpack(str[start:end]) 04810 start = end 04811 end += length 04812 _v369.frame_id = str[start:end] 04813 _x = _v368 04814 start = end 04815 end += 8 04816 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04817 start = end 04818 end += 4 04819 (length,) = _struct_I.unpack(str[start:end]) 04820 start = end 04821 end += length 04822 _v368.encoding = str[start:end] 04823 _x = _v368 04824 start = end 04825 end += 5 04826 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 04827 start = end 04828 end += 4 04829 (length,) = _struct_I.unpack(str[start:end]) 04830 start = end 04831 end += length 04832 _v368.data = str[start:end] 04833 _v371 = _v364.disparity_image 04834 _v372 = _v371.header 04835 start = end 04836 end += 4 04837 (_v372.seq,) = _struct_I.unpack(str[start:end]) 04838 _v373 = _v372.stamp 04839 _x = _v373 04840 start = end 04841 end += 8 04842 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04843 start = end 04844 end += 4 04845 (length,) = _struct_I.unpack(str[start:end]) 04846 start = end 04847 end += length 04848 _v372.frame_id = str[start:end] 04849 _x = _v371 04850 start = end 04851 end += 8 04852 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04853 start = end 04854 end += 4 04855 (length,) = _struct_I.unpack(str[start:end]) 04856 start = end 04857 end += length 04858 _v371.encoding = str[start:end] 04859 _x = _v371 04860 start = end 04861 end += 5 04862 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 04863 start = end 04864 end += 4 04865 (length,) = _struct_I.unpack(str[start:end]) 04866 start = end 04867 end += length 04868 _v371.data = str[start:end] 04869 _v374 = _v364.cam_info 04870 _v375 = _v374.header 04871 start = end 04872 end += 4 04873 (_v375.seq,) = _struct_I.unpack(str[start:end]) 04874 _v376 = _v375.stamp 04875 _x = _v376 04876 start = end 04877 end += 8 04878 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04879 start = end 04880 end += 4 04881 (length,) = _struct_I.unpack(str[start:end]) 04882 start = end 04883 end += length 04884 _v375.frame_id = str[start:end] 04885 _x = _v374 04886 start = end 04887 end += 8 04888 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 04889 start = end 04890 end += 4 04891 (length,) = _struct_I.unpack(str[start:end]) 04892 start = end 04893 end += length 04894 _v374.distortion_model = str[start:end] 04895 start = end 04896 end += 4 04897 (length,) = _struct_I.unpack(str[start:end]) 04898 pattern = '<%sd'%length 04899 start = end 04900 end += struct.calcsize(pattern) 04901 _v374.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 04902 start = end 04903 end += 72 04904 _v374.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 04905 start = end 04906 end += 72 04907 _v374.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 04908 start = end 04909 end += 96 04910 _v374.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 04911 _x = _v374 04912 start = end 04913 end += 8 04914 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 04915 _v377 = _v374.roi 04916 _x = _v377 04917 start = end 04918 end += 17 04919 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 04920 _v377.do_rectify = bool(_v377.do_rectify) 04921 _v378 = _v364.roi_box_pose 04922 _v379 = _v378.header 04923 start = end 04924 end += 4 04925 (_v379.seq,) = _struct_I.unpack(str[start:end]) 04926 _v380 = _v379.stamp 04927 _x = _v380 04928 start = end 04929 end += 8 04930 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 04931 start = end 04932 end += 4 04933 (length,) = _struct_I.unpack(str[start:end]) 04934 start = end 04935 end += length 04936 _v379.frame_id = str[start:end] 04937 _v381 = _v378.pose 04938 _v382 = _v381.position 04939 _x = _v382 04940 start = end 04941 end += 24 04942 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04943 _v383 = _v381.orientation 04944 _x = _v383 04945 start = end 04946 end += 32 04947 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 04948 _v384 = _v364.roi_box_dims 04949 _x = _v384 04950 start = end 04951 end += 24 04952 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 04953 start = end 04954 end += 4 04955 (length,) = _struct_I.unpack(str[start:end]) 04956 start = end 04957 end += length 04958 val1.collision_name = str[start:end] 04959 self.movable_obstacles.append(val1) 04960 start = end 04961 end += 4 04962 (self.max_contact_force,) = _struct_f.unpack(str[start:end]) 04963 return self 04964 except struct.error as e: 04965 raise roslib.message.DeserializationError(e) #most likely buffer underfill 04966 04967 _struct_I = roslib.message.struct_I 04968 _struct_IBI = struct.Struct("<IBI") 04969 _struct_12d = struct.Struct("<12d") 04970 _struct_dB2f = struct.Struct("<dB2f") 04971 _struct_BI = struct.Struct("<BI") 04972 _struct_10d = struct.Struct("<10d") 04973 _struct_3I = struct.Struct("<3I") 04974 _struct_B2I = struct.Struct("<B2I") 04975 _struct_5B = struct.Struct("<5B") 04976 _struct_3f = struct.Struct("<3f") 04977 _struct_3d = struct.Struct("<3d") 04978 _struct_B = struct.Struct("<B") 04979 _struct_di = struct.Struct("<di") 04980 _struct_9d = struct.Struct("<9d") 04981 _struct_2I = struct.Struct("<2I") 04982 _struct_6IB3I = struct.Struct("<6IB3I") 04983 _struct_b = struct.Struct("<b") 04984 _struct_d = struct.Struct("<d") 04985 _struct_f = struct.Struct("<f") 04986 _struct_i = struct.Struct("<i") 04987 _struct_3d2f = struct.Struct("<3d2f") 04988 _struct_4d = struct.Struct("<4d") 04989 _struct_4IB = struct.Struct("<4IB")