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