00001 """autogenerated by genpy from object_manipulation_msgs/PlaceGoal.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import arm_navigation_msgs.msg
00008 import object_manipulation_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 PlaceGoal(genpy.Message):
00015 _md5sum = "6313a88996ba81ffc91a8fc15ca327b6"
00016 _type = "object_manipulation_msgs/PlaceGoal"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019 # An action for placing an object
00020
00021 # which arm to be used for grasping
00022 string arm_name
00023
00024 # a list of possible transformations for placing the object
00025 geometry_msgs/PoseStamped[] place_locations
00026
00027 # the grasp that has been executed on this object
00028 Grasp grasp
00029
00030 # Note that, in general place_location is intended to show where you want the object
00031 # to go in the world, while grasp is meant to show how the object is grasped
00032 # (where the hand is relative to the object).
00033
00034 # Ultimately, what matters is where the hand goes when you place the object; this
00035 # is computed internally by simply multiplying the two transforms above.
00036
00037 # If you already know where you want the hand to go in the world when placing, feel
00038 # free to put that in the place_location, and make the transform in grasp equal to
00039 # identity.
00040
00041 # how far the retreat should ideally be away from the place location
00042 float32 desired_retreat_distance
00043
00044 # the min distance between the retreat and the place location that must actually be feasible
00045 # for the place not to be rejected
00046 float32 min_retreat_distance
00047
00048 # how the place location should be approached
00049 # the frame_id that this lift is specified in MUST be either the robot_frame
00050 # or the gripper_frame specified in your hand description file
00051 GripperTranslation approach
00052
00053 # the name that the target object has in the collision map
00054 # can be left empty if no name is available
00055 string collision_object_name
00056
00057 # the name that the support surface (e.g. table) has in the collision map
00058 # can be left empty if no name is available
00059 string collision_support_surface_name
00060
00061 # whether collisions between the gripper and the support surface should be acceptable
00062 # during move from pre-place to place and during retreat. Collisions when moving to the
00063 # pre-place location are still not allowed even if this is set to true.
00064 bool allow_gripper_support_collision
00065
00066 # whether reactive placing based on tactile sensors should be used
00067 bool use_reactive_place
00068
00069 # how much the object should be padded by when deciding if the grasp
00070 # location is freasible or not
00071 float64 place_padding
00072
00073 # set this to true if you only want to query the manipulation pipeline as to what
00074 # place locations it thinks are feasible, without actually executing them. If this is set to
00075 # true, the atempted_location_results field of the result will be populated, but no arm
00076 # movement will be attempted
00077 bool only_perform_feasibility_test
00078
00079 # OPTIONAL (These will not have to be filled out most of the time)
00080 # constraints to be imposed on every point in the motion of the arm
00081 arm_navigation_msgs/Constraints path_constraints
00082
00083 # OPTIONAL (These will not have to be filled out most of the time)
00084 # additional collision operations to be used for every arm movement performed
00085 # during placing. Note that these will be added on top of (and thus overide) other
00086 # collision operations that the grasping pipeline deems necessary. Should be used
00087 # with care and only if special behaviors are desired.
00088 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations
00089
00090 # OPTIONAL (These will not have to be filled out most of the time)
00091 # additional link paddings to be used for every arm movement performed
00092 # during placing. Note that these will be added on top of (and thus overide) other
00093 # link paddings that the grasping pipeline deems necessary. Should be used
00094 # with care and only if special behaviors are desired.
00095 arm_navigation_msgs/LinkPadding[] additional_link_padding
00096
00097
00098 ================================================================================
00099 MSG: geometry_msgs/PoseStamped
00100 # A Pose with reference coordinate frame and timestamp
00101 Header header
00102 Pose pose
00103
00104 ================================================================================
00105 MSG: std_msgs/Header
00106 # Standard metadata for higher-level stamped data types.
00107 # This is generally used to communicate timestamped data
00108 # in a particular coordinate frame.
00109 #
00110 # sequence ID: consecutively increasing ID
00111 uint32 seq
00112 #Two-integer timestamp that is expressed as:
00113 # * stamp.secs: seconds (stamp_secs) since epoch
00114 # * stamp.nsecs: nanoseconds since stamp_secs
00115 # time-handling sugar is provided by the client library
00116 time stamp
00117 #Frame this data is associated with
00118 # 0: no frame
00119 # 1: global frame
00120 string frame_id
00121
00122 ================================================================================
00123 MSG: geometry_msgs/Pose
00124 # A representation of pose in free space, composed of postion and orientation.
00125 Point position
00126 Quaternion orientation
00127
00128 ================================================================================
00129 MSG: geometry_msgs/Point
00130 # This contains the position of a point in free space
00131 float64 x
00132 float64 y
00133 float64 z
00134
00135 ================================================================================
00136 MSG: geometry_msgs/Quaternion
00137 # This represents an orientation in free space in quaternion form.
00138
00139 float64 x
00140 float64 y
00141 float64 z
00142 float64 w
00143
00144 ================================================================================
00145 MSG: object_manipulation_msgs/Grasp
00146
00147 # The internal posture of the hand for the pre-grasp
00148 # only positions are used
00149 sensor_msgs/JointState pre_grasp_posture
00150
00151 # The internal posture of the hand for the grasp
00152 # positions and efforts are used
00153 sensor_msgs/JointState grasp_posture
00154
00155 # The position of the end-effector for the grasp relative to a reference frame
00156 # (that is always specified elsewhere, not in this message)
00157 geometry_msgs/Pose grasp_pose
00158
00159 # The estimated probability of success for this grasp
00160 float64 success_probability
00161
00162 # Debug flag to indicate that this grasp would be the best in its cluster
00163 bool cluster_rep
00164
00165 # how far the pre-grasp should ideally be away from the grasp
00166 float32 desired_approach_distance
00167
00168 # how much distance between pre-grasp and grasp must actually be feasible
00169 # for the grasp not to be rejected
00170 float32 min_approach_distance
00171
00172 # an optional list of obstacles that we have semantic information about
00173 # and that we expect might move in the course of executing this grasp
00174 # the grasp planner is expected to make sure they move in an OK way; during
00175 # execution, grasp executors will not check for collisions against these objects
00176 GraspableObject[] moved_obstacles
00177
00178 ================================================================================
00179 MSG: sensor_msgs/JointState
00180 # This is a message that holds data to describe the state of a set of torque controlled joints.
00181 #
00182 # The state of each joint (revolute or prismatic) is defined by:
00183 # * the position of the joint (rad or m),
00184 # * the velocity of the joint (rad/s or m/s) and
00185 # * the effort that is applied in the joint (Nm or N).
00186 #
00187 # Each joint is uniquely identified by its name
00188 # The header specifies the time at which the joint states were recorded. All the joint states
00189 # in one message have to be recorded at the same time.
00190 #
00191 # This message consists of a multiple arrays, one for each part of the joint state.
00192 # The goal is to make each of the fields optional. When e.g. your joints have no
00193 # effort associated with them, you can leave the effort array empty.
00194 #
00195 # All arrays in this message should have the same size, or be empty.
00196 # This is the only way to uniquely associate the joint name with the correct
00197 # states.
00198
00199
00200 Header header
00201
00202 string[] name
00203 float64[] position
00204 float64[] velocity
00205 float64[] effort
00206
00207 ================================================================================
00208 MSG: object_manipulation_msgs/GraspableObject
00209 # an object that the object_manipulator can work on
00210
00211 # a graspable object can be represented in multiple ways. This message
00212 # can contain all of them. Which one is actually used is up to the receiver
00213 # of this message. When adding new representations, one must be careful that
00214 # they have reasonable lightweight defaults indicating that that particular
00215 # representation is not available.
00216
00217 # the tf frame to be used as a reference frame when combining information from
00218 # the different representations below
00219 string reference_frame_id
00220
00221 # potential recognition results from a database of models
00222 # all poses are relative to the object reference pose
00223 household_objects_database_msgs/DatabaseModelPose[] potential_models
00224
00225 # the point cloud itself
00226 sensor_msgs/PointCloud cluster
00227
00228 # a region of a PointCloud2 of interest
00229 object_manipulation_msgs/SceneRegion region
00230
00231 # the name that this object has in the collision environment
00232 string collision_name
00233 ================================================================================
00234 MSG: household_objects_database_msgs/DatabaseModelPose
00235 # Informs that a specific model from the Model Database has been
00236 # identified at a certain location
00237
00238 # the database id of the model
00239 int32 model_id
00240
00241 # the pose that it can be found in
00242 geometry_msgs/PoseStamped pose
00243
00244 # a measure of the confidence level in this detection result
00245 float32 confidence
00246
00247 # the name of the object detector that generated this detection result
00248 string detector_name
00249
00250 ================================================================================
00251 MSG: sensor_msgs/PointCloud
00252 # This message holds a collection of 3d points, plus optional additional
00253 # information about each point.
00254
00255 # Time of sensor data acquisition, coordinate frame ID.
00256 Header header
00257
00258 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00259 # in the frame given in the header.
00260 geometry_msgs/Point32[] points
00261
00262 # Each channel should have the same number of elements as points array,
00263 # and the data in each channel should correspond 1:1 with each point.
00264 # Channel names in common practice are listed in ChannelFloat32.msg.
00265 ChannelFloat32[] channels
00266
00267 ================================================================================
00268 MSG: geometry_msgs/Point32
00269 # This contains the position of a point in free space(with 32 bits of precision).
00270 # It is recommeded to use Point wherever possible instead of Point32.
00271 #
00272 # This recommendation is to promote interoperability.
00273 #
00274 # This message is designed to take up less space when sending
00275 # lots of points at once, as in the case of a PointCloud.
00276
00277 float32 x
00278 float32 y
00279 float32 z
00280 ================================================================================
00281 MSG: sensor_msgs/ChannelFloat32
00282 # This message is used by the PointCloud message to hold optional data
00283 # associated with each point in the cloud. The length of the values
00284 # array should be the same as the length of the points array in the
00285 # PointCloud, and each value should be associated with the corresponding
00286 # point.
00287
00288 # Channel names in existing practice include:
00289 # "u", "v" - row and column (respectively) in the left stereo image.
00290 # This is opposite to usual conventions but remains for
00291 # historical reasons. The newer PointCloud2 message has no
00292 # such problem.
00293 # "rgb" - For point clouds produced by color stereo cameras. uint8
00294 # (R,G,B) values packed into the least significant 24 bits,
00295 # in order.
00296 # "intensity" - laser or pixel intensity.
00297 # "distance"
00298
00299 # The channel name should give semantics of the channel (e.g.
00300 # "intensity" instead of "value").
00301 string name
00302
00303 # The values array should be 1-1 with the elements of the associated
00304 # PointCloud.
00305 float32[] values
00306
00307 ================================================================================
00308 MSG: object_manipulation_msgs/SceneRegion
00309 # Point cloud
00310 sensor_msgs/PointCloud2 cloud
00311
00312 # Indices for the region of interest
00313 int32[] mask
00314
00315 # One of the corresponding 2D images, if applicable
00316 sensor_msgs/Image image
00317
00318 # The disparity image, if applicable
00319 sensor_msgs/Image disparity_image
00320
00321 # Camera info for the camera that took the image
00322 sensor_msgs/CameraInfo cam_info
00323
00324 # a 3D region of interest for grasp planning
00325 geometry_msgs/PoseStamped roi_box_pose
00326 geometry_msgs/Vector3 roi_box_dims
00327
00328 ================================================================================
00329 MSG: sensor_msgs/PointCloud2
00330 # This message holds a collection of N-dimensional points, which may
00331 # contain additional information such as normals, intensity, etc. The
00332 # point data is stored as a binary blob, its layout described by the
00333 # contents of the "fields" array.
00334
00335 # The point cloud data may be organized 2d (image-like) or 1d
00336 # (unordered). Point clouds organized as 2d images may be produced by
00337 # camera depth sensors such as stereo or time-of-flight.
00338
00339 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00340 # points).
00341 Header header
00342
00343 # 2D structure of the point cloud. If the cloud is unordered, height is
00344 # 1 and width is the length of the point cloud.
00345 uint32 height
00346 uint32 width
00347
00348 # Describes the channels and their layout in the binary data blob.
00349 PointField[] fields
00350
00351 bool is_bigendian # Is this data bigendian?
00352 uint32 point_step # Length of a point in bytes
00353 uint32 row_step # Length of a row in bytes
00354 uint8[] data # Actual point data, size is (row_step*height)
00355
00356 bool is_dense # True if there are no invalid points
00357
00358 ================================================================================
00359 MSG: sensor_msgs/PointField
00360 # This message holds the description of one point entry in the
00361 # PointCloud2 message format.
00362 uint8 INT8 = 1
00363 uint8 UINT8 = 2
00364 uint8 INT16 = 3
00365 uint8 UINT16 = 4
00366 uint8 INT32 = 5
00367 uint8 UINT32 = 6
00368 uint8 FLOAT32 = 7
00369 uint8 FLOAT64 = 8
00370
00371 string name # Name of field
00372 uint32 offset # Offset from start of point struct
00373 uint8 datatype # Datatype enumeration, see above
00374 uint32 count # How many elements in the field
00375
00376 ================================================================================
00377 MSG: sensor_msgs/Image
00378 # This message contains an uncompressed image
00379 # (0, 0) is at top-left corner of image
00380 #
00381
00382 Header header # Header timestamp should be acquisition time of image
00383 # Header frame_id should be optical frame of camera
00384 # origin of frame should be optical center of cameara
00385 # +x should point to the right in the image
00386 # +y should point down in the image
00387 # +z should point into to plane of the image
00388 # If the frame_id here and the frame_id of the CameraInfo
00389 # message associated with the image conflict
00390 # the behavior is undefined
00391
00392 uint32 height # image height, that is, number of rows
00393 uint32 width # image width, that is, number of columns
00394
00395 # The legal values for encoding are in file src/image_encodings.cpp
00396 # If you want to standardize a new string format, join
00397 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00398
00399 string encoding # Encoding of pixels -- channel meaning, ordering, size
00400 # taken from the list of strings in src/image_encodings.cpp
00401
00402 uint8 is_bigendian # is this data bigendian?
00403 uint32 step # Full row length in bytes
00404 uint8[] data # actual matrix data, size is (step * rows)
00405
00406 ================================================================================
00407 MSG: sensor_msgs/CameraInfo
00408 # This message defines meta information for a camera. It should be in a
00409 # camera namespace on topic "camera_info" and accompanied by up to five
00410 # image topics named:
00411 #
00412 # image_raw - raw data from the camera driver, possibly Bayer encoded
00413 # image - monochrome, distorted
00414 # image_color - color, distorted
00415 # image_rect - monochrome, rectified
00416 # image_rect_color - color, rectified
00417 #
00418 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00419 # for producing the four processed image topics from image_raw and
00420 # camera_info. The meaning of the camera parameters are described in
00421 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00422 #
00423 # The image_geometry package provides a user-friendly interface to
00424 # common operations using this meta information. If you want to, e.g.,
00425 # project a 3d point into image coordinates, we strongly recommend
00426 # using image_geometry.
00427 #
00428 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00429 # zeroed out. In particular, clients may assume that K[0] == 0.0
00430 # indicates an uncalibrated camera.
00431
00432 #######################################################################
00433 # Image acquisition info #
00434 #######################################################################
00435
00436 # Time of image acquisition, camera coordinate frame ID
00437 Header header # Header timestamp should be acquisition time of image
00438 # Header frame_id should be optical frame of camera
00439 # origin of frame should be optical center of camera
00440 # +x should point to the right in the image
00441 # +y should point down in the image
00442 # +z should point into the plane of the image
00443
00444
00445 #######################################################################
00446 # Calibration Parameters #
00447 #######################################################################
00448 # These are fixed during camera calibration. Their values will be the #
00449 # same in all messages until the camera is recalibrated. Note that #
00450 # self-calibrating systems may "recalibrate" frequently. #
00451 # #
00452 # The internal parameters can be used to warp a raw (distorted) image #
00453 # to: #
00454 # 1. An undistorted image (requires D and K) #
00455 # 2. A rectified image (requires D, K, R) #
00456 # The projection matrix P projects 3D points into the rectified image.#
00457 #######################################################################
00458
00459 # The image dimensions with which the camera was calibrated. Normally
00460 # this will be the full camera resolution in pixels.
00461 uint32 height
00462 uint32 width
00463
00464 # The distortion model used. Supported models are listed in
00465 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00466 # simple model of radial and tangential distortion - is sufficent.
00467 string distortion_model
00468
00469 # The distortion parameters, size depending on the distortion model.
00470 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00471 float64[] D
00472
00473 # Intrinsic camera matrix for the raw (distorted) images.
00474 # [fx 0 cx]
00475 # K = [ 0 fy cy]
00476 # [ 0 0 1]
00477 # Projects 3D points in the camera coordinate frame to 2D pixel
00478 # coordinates using the focal lengths (fx, fy) and principal point
00479 # (cx, cy).
00480 float64[9] K # 3x3 row-major matrix
00481
00482 # Rectification matrix (stereo cameras only)
00483 # A rotation matrix aligning the camera coordinate system to the ideal
00484 # stereo image plane so that epipolar lines in both stereo images are
00485 # parallel.
00486 float64[9] R # 3x3 row-major matrix
00487
00488 # Projection/camera matrix
00489 # [fx' 0 cx' Tx]
00490 # P = [ 0 fy' cy' Ty]
00491 # [ 0 0 1 0]
00492 # By convention, this matrix specifies the intrinsic (camera) matrix
00493 # of the processed (rectified) image. That is, the left 3x3 portion
00494 # is the normal camera intrinsic matrix for the rectified image.
00495 # It projects 3D points in the camera coordinate frame to 2D pixel
00496 # coordinates using the focal lengths (fx', fy') and principal point
00497 # (cx', cy') - these may differ from the values in K.
00498 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00499 # also have R = the identity and P[1:3,1:3] = K.
00500 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00501 # position of the optical center of the second camera in the first
00502 # camera's frame. We assume Tz = 0 so both cameras are in the same
00503 # stereo image plane. The first camera always has Tx = Ty = 0. For
00504 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00505 # Tx = -fx' * B, where B is the baseline between the cameras.
00506 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00507 # the rectified image is given by:
00508 # [u v w]' = P * [X Y Z 1]'
00509 # x = u / w
00510 # y = v / w
00511 # This holds for both images of a stereo pair.
00512 float64[12] P # 3x4 row-major matrix
00513
00514
00515 #######################################################################
00516 # Operational Parameters #
00517 #######################################################################
00518 # These define the image region actually captured by the camera #
00519 # driver. Although they affect the geometry of the output image, they #
00520 # may be changed freely without recalibrating the camera. #
00521 #######################################################################
00522
00523 # Binning refers here to any camera setting which combines rectangular
00524 # neighborhoods of pixels into larger "super-pixels." It reduces the
00525 # resolution of the output image to
00526 # (width / binning_x) x (height / binning_y).
00527 # The default values binning_x = binning_y = 0 is considered the same
00528 # as binning_x = binning_y = 1 (no subsampling).
00529 uint32 binning_x
00530 uint32 binning_y
00531
00532 # Region of interest (subwindow of full camera resolution), given in
00533 # full resolution (unbinned) image coordinates. A particular ROI
00534 # always denotes the same window of pixels on the camera sensor,
00535 # regardless of binning settings.
00536 # The default setting of roi (all values 0) is considered the same as
00537 # full resolution (roi.width = width, roi.height = height).
00538 RegionOfInterest roi
00539
00540 ================================================================================
00541 MSG: sensor_msgs/RegionOfInterest
00542 # This message is used to specify a region of interest within an image.
00543 #
00544 # When used to specify the ROI setting of the camera when the image was
00545 # taken, the height and width fields should either match the height and
00546 # width fields for the associated image; or height = width = 0
00547 # indicates that the full resolution image was captured.
00548
00549 uint32 x_offset # Leftmost pixel of the ROI
00550 # (0 if the ROI includes the left edge of the image)
00551 uint32 y_offset # Topmost pixel of the ROI
00552 # (0 if the ROI includes the top edge of the image)
00553 uint32 height # Height of ROI
00554 uint32 width # Width of ROI
00555
00556 # True if a distinct rectified ROI should be calculated from the "raw"
00557 # ROI in this message. Typically this should be False if the full image
00558 # is captured (ROI not used), and True if a subwindow is captured (ROI
00559 # used).
00560 bool do_rectify
00561
00562 ================================================================================
00563 MSG: geometry_msgs/Vector3
00564 # This represents a vector in free space.
00565
00566 float64 x
00567 float64 y
00568 float64 z
00569 ================================================================================
00570 MSG: object_manipulation_msgs/GripperTranslation
00571 # defines a translation for the gripper, used in pickup or place tasks
00572 # for example for lifting an object off a table or approaching the table for placing
00573
00574 # the direction of the translation
00575 geometry_msgs/Vector3Stamped direction
00576
00577 # the desired translation distance
00578 float32 desired_distance
00579
00580 # the min distance that must be considered feasible before the
00581 # grasp is even attempted
00582 float32 min_distance
00583 ================================================================================
00584 MSG: geometry_msgs/Vector3Stamped
00585 # This represents a Vector3 with reference coordinate frame and timestamp
00586 Header header
00587 Vector3 vector
00588
00589 ================================================================================
00590 MSG: arm_navigation_msgs/Constraints
00591 # This message contains a list of motion planning constraints.
00592
00593 arm_navigation_msgs/JointConstraint[] joint_constraints
00594 arm_navigation_msgs/PositionConstraint[] position_constraints
00595 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00596 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00597
00598 ================================================================================
00599 MSG: arm_navigation_msgs/JointConstraint
00600 # Constrain the position of a joint to be within a certain bound
00601 string joint_name
00602
00603 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00604 float64 position
00605 float64 tolerance_above
00606 float64 tolerance_below
00607
00608 # A weighting factor for this constraint
00609 float64 weight
00610 ================================================================================
00611 MSG: arm_navigation_msgs/PositionConstraint
00612 # This message contains the definition of a position constraint.
00613 Header header
00614
00615 # The robot link this constraint refers to
00616 string link_name
00617
00618 # The offset (in the link frame) for the target point on the link we are planning for
00619 geometry_msgs/Point target_point_offset
00620
00621 # The nominal/target position for the point we are planning for
00622 geometry_msgs/Point position
00623
00624 # The shape of the bounded region that constrains the position of the end-effector
00625 # This region is always centered at the position defined above
00626 arm_navigation_msgs/Shape constraint_region_shape
00627
00628 # The orientation of the bounded region that constrains the position of the end-effector.
00629 # This allows the specification of non-axis aligned constraints
00630 geometry_msgs/Quaternion constraint_region_orientation
00631
00632 # Constraint weighting factor - a weight for this constraint
00633 float64 weight
00634
00635 ================================================================================
00636 MSG: arm_navigation_msgs/Shape
00637 byte SPHERE=0
00638 byte BOX=1
00639 byte CYLINDER=2
00640 byte MESH=3
00641
00642 byte type
00643
00644
00645 #### define sphere, box, cylinder ####
00646 # the origin of each shape is considered at the shape's center
00647
00648 # for sphere
00649 # radius := dimensions[0]
00650
00651 # for cylinder
00652 # radius := dimensions[0]
00653 # length := dimensions[1]
00654 # the length is along the Z axis
00655
00656 # for box
00657 # size_x := dimensions[0]
00658 # size_y := dimensions[1]
00659 # size_z := dimensions[2]
00660 float64[] dimensions
00661
00662
00663 #### define mesh ####
00664
00665 # list of triangles; triangle k is defined by tre vertices located
00666 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00667 int32[] triangles
00668 geometry_msgs/Point[] vertices
00669
00670 ================================================================================
00671 MSG: arm_navigation_msgs/OrientationConstraint
00672 # This message contains the definition of an orientation constraint.
00673 Header header
00674
00675 # The robot link this constraint refers to
00676 string link_name
00677
00678 # The type of the constraint
00679 int32 type
00680 int32 LINK_FRAME=0
00681 int32 HEADER_FRAME=1
00682
00683 # The desired orientation of the robot link specified as a quaternion
00684 geometry_msgs/Quaternion orientation
00685
00686 # optional RPY error tolerances specified if
00687 float64 absolute_roll_tolerance
00688 float64 absolute_pitch_tolerance
00689 float64 absolute_yaw_tolerance
00690
00691 # Constraint weighting factor - a weight for this constraint
00692 float64 weight
00693
00694 ================================================================================
00695 MSG: arm_navigation_msgs/VisibilityConstraint
00696 # This message contains the definition of a visibility constraint.
00697 Header header
00698
00699 # The point stamped target that needs to be kept within view of the sensor
00700 geometry_msgs/PointStamped target
00701
00702 # The local pose of the frame in which visibility is to be maintained
00703 # The frame id should represent the robot link to which the sensor is attached
00704 # The visual axis of the sensor is assumed to be along the X axis of this frame
00705 geometry_msgs/PoseStamped sensor_pose
00706
00707 # The deviation (in radians) that will be tolerated
00708 # Constraint error will be measured as the solid angle between the
00709 # X axis of the frame defined above and the vector between the origin
00710 # of the frame defined above and the target location
00711 float64 absolute_tolerance
00712
00713
00714 ================================================================================
00715 MSG: geometry_msgs/PointStamped
00716 # This represents a Point with reference coordinate frame and timestamp
00717 Header header
00718 Point point
00719
00720 ================================================================================
00721 MSG: arm_navigation_msgs/OrderedCollisionOperations
00722 # A set of collision operations that will be performed in the order they are specified
00723 CollisionOperation[] collision_operations
00724 ================================================================================
00725 MSG: arm_navigation_msgs/CollisionOperation
00726 # A definition of a collision operation
00727 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions
00728 # between the gripper and all objects in the collision space
00729
00730 string object1
00731 string object2
00732 string COLLISION_SET_ALL="all"
00733 string COLLISION_SET_OBJECTS="objects"
00734 string COLLISION_SET_ATTACHED_OBJECTS="attached"
00735
00736 # The penetration distance to which collisions are allowed. This is 0.0 by default.
00737 float64 penetration_distance
00738
00739 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above
00740 int32 operation
00741 int32 DISABLE=0
00742 int32 ENABLE=1
00743
00744 ================================================================================
00745 MSG: arm_navigation_msgs/LinkPadding
00746 #name for the link
00747 string link_name
00748
00749 # padding to apply to the link
00750 float64 padding
00751
00752 """
00753 __slots__ = ['arm_name','place_locations','grasp','desired_retreat_distance','min_retreat_distance','approach','collision_object_name','collision_support_surface_name','allow_gripper_support_collision','use_reactive_place','place_padding','only_perform_feasibility_test','path_constraints','additional_collision_operations','additional_link_padding']
00754 _slot_types = ['string','geometry_msgs/PoseStamped[]','object_manipulation_msgs/Grasp','float32','float32','object_manipulation_msgs/GripperTranslation','string','string','bool','bool','float64','bool','arm_navigation_msgs/Constraints','arm_navigation_msgs/OrderedCollisionOperations','arm_navigation_msgs/LinkPadding[]']
00755
00756 def __init__(self, *args, **kwds):
00757 """
00758 Constructor. Any message fields that are implicitly/explicitly
00759 set to None will be assigned a default value. The recommend
00760 use is keyword arguments as this is more robust to future message
00761 changes. You cannot mix in-order arguments and keyword arguments.
00762
00763 The available fields are:
00764 arm_name,place_locations,grasp,desired_retreat_distance,min_retreat_distance,approach,collision_object_name,collision_support_surface_name,allow_gripper_support_collision,use_reactive_place,place_padding,only_perform_feasibility_test,path_constraints,additional_collision_operations,additional_link_padding
00765
00766 :param args: complete set of field values, in .msg order
00767 :param kwds: use keyword arguments corresponding to message field names
00768 to set specific fields.
00769 """
00770 if args or kwds:
00771 super(PlaceGoal, self).__init__(*args, **kwds)
00772 #message fields cannot be None, assign default values for those that are
00773 if self.arm_name is None:
00774 self.arm_name = ''
00775 if self.place_locations is None:
00776 self.place_locations = []
00777 if self.grasp is None:
00778 self.grasp = object_manipulation_msgs.msg.Grasp()
00779 if self.desired_retreat_distance is None:
00780 self.desired_retreat_distance = 0.
00781 if self.min_retreat_distance is None:
00782 self.min_retreat_distance = 0.
00783 if self.approach is None:
00784 self.approach = object_manipulation_msgs.msg.GripperTranslation()
00785 if self.collision_object_name is None:
00786 self.collision_object_name = ''
00787 if self.collision_support_surface_name is None:
00788 self.collision_support_surface_name = ''
00789 if self.allow_gripper_support_collision is None:
00790 self.allow_gripper_support_collision = False
00791 if self.use_reactive_place is None:
00792 self.use_reactive_place = False
00793 if self.place_padding is None:
00794 self.place_padding = 0.
00795 if self.only_perform_feasibility_test is None:
00796 self.only_perform_feasibility_test = False
00797 if self.path_constraints is None:
00798 self.path_constraints = arm_navigation_msgs.msg.Constraints()
00799 if self.additional_collision_operations is None:
00800 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00801 if self.additional_link_padding is None:
00802 self.additional_link_padding = []
00803 else:
00804 self.arm_name = ''
00805 self.place_locations = []
00806 self.grasp = object_manipulation_msgs.msg.Grasp()
00807 self.desired_retreat_distance = 0.
00808 self.min_retreat_distance = 0.
00809 self.approach = object_manipulation_msgs.msg.GripperTranslation()
00810 self.collision_object_name = ''
00811 self.collision_support_surface_name = ''
00812 self.allow_gripper_support_collision = False
00813 self.use_reactive_place = False
00814 self.place_padding = 0.
00815 self.only_perform_feasibility_test = False
00816 self.path_constraints = arm_navigation_msgs.msg.Constraints()
00817 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
00818 self.additional_link_padding = []
00819
00820 def _get_types(self):
00821 """
00822 internal API method
00823 """
00824 return self._slot_types
00825
00826 def serialize(self, buff):
00827 """
00828 serialize message into buffer
00829 :param buff: buffer, ``StringIO``
00830 """
00831 try:
00832 _x = self.arm_name
00833 length = len(_x)
00834 if python3 or type(_x) == unicode:
00835 _x = _x.encode('utf-8')
00836 length = len(_x)
00837 buff.write(struct.pack('<I%ss'%length, length, _x))
00838 length = len(self.place_locations)
00839 buff.write(_struct_I.pack(length))
00840 for val1 in self.place_locations:
00841 _v1 = val1.header
00842 buff.write(_struct_I.pack(_v1.seq))
00843 _v2 = _v1.stamp
00844 _x = _v2
00845 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00846 _x = _v1.frame_id
00847 length = len(_x)
00848 if python3 or type(_x) == unicode:
00849 _x = _x.encode('utf-8')
00850 length = len(_x)
00851 buff.write(struct.pack('<I%ss'%length, length, _x))
00852 _v3 = val1.pose
00853 _v4 = _v3.position
00854 _x = _v4
00855 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00856 _v5 = _v3.orientation
00857 _x = _v5
00858 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00859 _x = self
00860 buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00861 _x = self.grasp.pre_grasp_posture.header.frame_id
00862 length = len(_x)
00863 if python3 or type(_x) == unicode:
00864 _x = _x.encode('utf-8')
00865 length = len(_x)
00866 buff.write(struct.pack('<I%ss'%length, length, _x))
00867 length = len(self.grasp.pre_grasp_posture.name)
00868 buff.write(_struct_I.pack(length))
00869 for val1 in self.grasp.pre_grasp_posture.name:
00870 length = len(val1)
00871 if python3 or type(val1) == unicode:
00872 val1 = val1.encode('utf-8')
00873 length = len(val1)
00874 buff.write(struct.pack('<I%ss'%length, length, val1))
00875 length = len(self.grasp.pre_grasp_posture.position)
00876 buff.write(_struct_I.pack(length))
00877 pattern = '<%sd'%length
00878 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00879 length = len(self.grasp.pre_grasp_posture.velocity)
00880 buff.write(_struct_I.pack(length))
00881 pattern = '<%sd'%length
00882 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00883 length = len(self.grasp.pre_grasp_posture.effort)
00884 buff.write(_struct_I.pack(length))
00885 pattern = '<%sd'%length
00886 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00887 _x = self
00888 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00889 _x = self.grasp.grasp_posture.header.frame_id
00890 length = len(_x)
00891 if python3 or type(_x) == unicode:
00892 _x = _x.encode('utf-8')
00893 length = len(_x)
00894 buff.write(struct.pack('<I%ss'%length, length, _x))
00895 length = len(self.grasp.grasp_posture.name)
00896 buff.write(_struct_I.pack(length))
00897 for val1 in self.grasp.grasp_posture.name:
00898 length = len(val1)
00899 if python3 or type(val1) == unicode:
00900 val1 = val1.encode('utf-8')
00901 length = len(val1)
00902 buff.write(struct.pack('<I%ss'%length, length, val1))
00903 length = len(self.grasp.grasp_posture.position)
00904 buff.write(_struct_I.pack(length))
00905 pattern = '<%sd'%length
00906 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00907 length = len(self.grasp.grasp_posture.velocity)
00908 buff.write(_struct_I.pack(length))
00909 pattern = '<%sd'%length
00910 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00911 length = len(self.grasp.grasp_posture.effort)
00912 buff.write(_struct_I.pack(length))
00913 pattern = '<%sd'%length
00914 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00915 _x = self
00916 buff.write(_struct_8dB2f.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance))
00917 length = len(self.grasp.moved_obstacles)
00918 buff.write(_struct_I.pack(length))
00919 for val1 in self.grasp.moved_obstacles:
00920 _x = val1.reference_frame_id
00921 length = len(_x)
00922 if python3 or type(_x) == unicode:
00923 _x = _x.encode('utf-8')
00924 length = len(_x)
00925 buff.write(struct.pack('<I%ss'%length, length, _x))
00926 length = len(val1.potential_models)
00927 buff.write(_struct_I.pack(length))
00928 for val2 in val1.potential_models:
00929 buff.write(_struct_i.pack(val2.model_id))
00930 _v6 = val2.pose
00931 _v7 = _v6.header
00932 buff.write(_struct_I.pack(_v7.seq))
00933 _v8 = _v7.stamp
00934 _x = _v8
00935 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00936 _x = _v7.frame_id
00937 length = len(_x)
00938 if python3 or type(_x) == unicode:
00939 _x = _x.encode('utf-8')
00940 length = len(_x)
00941 buff.write(struct.pack('<I%ss'%length, length, _x))
00942 _v9 = _v6.pose
00943 _v10 = _v9.position
00944 _x = _v10
00945 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00946 _v11 = _v9.orientation
00947 _x = _v11
00948 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00949 buff.write(_struct_f.pack(val2.confidence))
00950 _x = val2.detector_name
00951 length = len(_x)
00952 if python3 or type(_x) == unicode:
00953 _x = _x.encode('utf-8')
00954 length = len(_x)
00955 buff.write(struct.pack('<I%ss'%length, length, _x))
00956 _v12 = val1.cluster
00957 _v13 = _v12.header
00958 buff.write(_struct_I.pack(_v13.seq))
00959 _v14 = _v13.stamp
00960 _x = _v14
00961 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00962 _x = _v13.frame_id
00963 length = len(_x)
00964 if python3 or type(_x) == unicode:
00965 _x = _x.encode('utf-8')
00966 length = len(_x)
00967 buff.write(struct.pack('<I%ss'%length, length, _x))
00968 length = len(_v12.points)
00969 buff.write(_struct_I.pack(length))
00970 for val3 in _v12.points:
00971 _x = val3
00972 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00973 length = len(_v12.channels)
00974 buff.write(_struct_I.pack(length))
00975 for val3 in _v12.channels:
00976 _x = val3.name
00977 length = len(_x)
00978 if python3 or type(_x) == unicode:
00979 _x = _x.encode('utf-8')
00980 length = len(_x)
00981 buff.write(struct.pack('<I%ss'%length, length, _x))
00982 length = len(val3.values)
00983 buff.write(_struct_I.pack(length))
00984 pattern = '<%sf'%length
00985 buff.write(struct.pack(pattern, *val3.values))
00986 _v15 = val1.region
00987 _v16 = _v15.cloud
00988 _v17 = _v16.header
00989 buff.write(_struct_I.pack(_v17.seq))
00990 _v18 = _v17.stamp
00991 _x = _v18
00992 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00993 _x = _v17.frame_id
00994 length = len(_x)
00995 if python3 or type(_x) == unicode:
00996 _x = _x.encode('utf-8')
00997 length = len(_x)
00998 buff.write(struct.pack('<I%ss'%length, length, _x))
00999 _x = _v16
01000 buff.write(_struct_2I.pack(_x.height, _x.width))
01001 length = len(_v16.fields)
01002 buff.write(_struct_I.pack(length))
01003 for val4 in _v16.fields:
01004 _x = val4.name
01005 length = len(_x)
01006 if python3 or type(_x) == unicode:
01007 _x = _x.encode('utf-8')
01008 length = len(_x)
01009 buff.write(struct.pack('<I%ss'%length, length, _x))
01010 _x = val4
01011 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01012 _x = _v16
01013 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01014 _x = _v16.data
01015 length = len(_x)
01016 # - if encoded as a list instead, serialize as bytes instead of string
01017 if type(_x) in [list, tuple]:
01018 buff.write(struct.pack('<I%sB'%length, length, *_x))
01019 else:
01020 buff.write(struct.pack('<I%ss'%length, length, _x))
01021 buff.write(_struct_B.pack(_v16.is_dense))
01022 length = len(_v15.mask)
01023 buff.write(_struct_I.pack(length))
01024 pattern = '<%si'%length
01025 buff.write(struct.pack(pattern, *_v15.mask))
01026 _v19 = _v15.image
01027 _v20 = _v19.header
01028 buff.write(_struct_I.pack(_v20.seq))
01029 _v21 = _v20.stamp
01030 _x = _v21
01031 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01032 _x = _v20.frame_id
01033 length = len(_x)
01034 if python3 or type(_x) == unicode:
01035 _x = _x.encode('utf-8')
01036 length = len(_x)
01037 buff.write(struct.pack('<I%ss'%length, length, _x))
01038 _x = _v19
01039 buff.write(_struct_2I.pack(_x.height, _x.width))
01040 _x = _v19.encoding
01041 length = len(_x)
01042 if python3 or type(_x) == unicode:
01043 _x = _x.encode('utf-8')
01044 length = len(_x)
01045 buff.write(struct.pack('<I%ss'%length, length, _x))
01046 _x = _v19
01047 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01048 _x = _v19.data
01049 length = len(_x)
01050 # - if encoded as a list instead, serialize as bytes instead of string
01051 if type(_x) in [list, tuple]:
01052 buff.write(struct.pack('<I%sB'%length, length, *_x))
01053 else:
01054 buff.write(struct.pack('<I%ss'%length, length, _x))
01055 _v22 = _v15.disparity_image
01056 _v23 = _v22.header
01057 buff.write(_struct_I.pack(_v23.seq))
01058 _v24 = _v23.stamp
01059 _x = _v24
01060 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01061 _x = _v23.frame_id
01062 length = len(_x)
01063 if python3 or type(_x) == unicode:
01064 _x = _x.encode('utf-8')
01065 length = len(_x)
01066 buff.write(struct.pack('<I%ss'%length, length, _x))
01067 _x = _v22
01068 buff.write(_struct_2I.pack(_x.height, _x.width))
01069 _x = _v22.encoding
01070 length = len(_x)
01071 if python3 or type(_x) == unicode:
01072 _x = _x.encode('utf-8')
01073 length = len(_x)
01074 buff.write(struct.pack('<I%ss'%length, length, _x))
01075 _x = _v22
01076 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01077 _x = _v22.data
01078 length = len(_x)
01079 # - if encoded as a list instead, serialize as bytes instead of string
01080 if type(_x) in [list, tuple]:
01081 buff.write(struct.pack('<I%sB'%length, length, *_x))
01082 else:
01083 buff.write(struct.pack('<I%ss'%length, length, _x))
01084 _v25 = _v15.cam_info
01085 _v26 = _v25.header
01086 buff.write(_struct_I.pack(_v26.seq))
01087 _v27 = _v26.stamp
01088 _x = _v27
01089 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01090 _x = _v26.frame_id
01091 length = len(_x)
01092 if python3 or type(_x) == unicode:
01093 _x = _x.encode('utf-8')
01094 length = len(_x)
01095 buff.write(struct.pack('<I%ss'%length, length, _x))
01096 _x = _v25
01097 buff.write(_struct_2I.pack(_x.height, _x.width))
01098 _x = _v25.distortion_model
01099 length = len(_x)
01100 if python3 or type(_x) == unicode:
01101 _x = _x.encode('utf-8')
01102 length = len(_x)
01103 buff.write(struct.pack('<I%ss'%length, length, _x))
01104 length = len(_v25.D)
01105 buff.write(_struct_I.pack(length))
01106 pattern = '<%sd'%length
01107 buff.write(struct.pack(pattern, *_v25.D))
01108 buff.write(_struct_9d.pack(*_v25.K))
01109 buff.write(_struct_9d.pack(*_v25.R))
01110 buff.write(_struct_12d.pack(*_v25.P))
01111 _x = _v25
01112 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01113 _v28 = _v25.roi
01114 _x = _v28
01115 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01116 _v29 = _v15.roi_box_pose
01117 _v30 = _v29.header
01118 buff.write(_struct_I.pack(_v30.seq))
01119 _v31 = _v30.stamp
01120 _x = _v31
01121 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01122 _x = _v30.frame_id
01123 length = len(_x)
01124 if python3 or type(_x) == unicode:
01125 _x = _x.encode('utf-8')
01126 length = len(_x)
01127 buff.write(struct.pack('<I%ss'%length, length, _x))
01128 _v32 = _v29.pose
01129 _v33 = _v32.position
01130 _x = _v33
01131 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01132 _v34 = _v32.orientation
01133 _x = _v34
01134 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01135 _v35 = _v15.roi_box_dims
01136 _x = _v35
01137 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01138 _x = val1.collision_name
01139 length = len(_x)
01140 if python3 or type(_x) == unicode:
01141 _x = _x.encode('utf-8')
01142 length = len(_x)
01143 buff.write(struct.pack('<I%ss'%length, length, _x))
01144 _x = self
01145 buff.write(_struct_2f3I.pack(_x.desired_retreat_distance, _x.min_retreat_distance, _x.approach.direction.header.seq, _x.approach.direction.header.stamp.secs, _x.approach.direction.header.stamp.nsecs))
01146 _x = self.approach.direction.header.frame_id
01147 length = len(_x)
01148 if python3 or type(_x) == unicode:
01149 _x = _x.encode('utf-8')
01150 length = len(_x)
01151 buff.write(struct.pack('<I%ss'%length, length, _x))
01152 _x = self
01153 buff.write(_struct_3d2f.pack(_x.approach.direction.vector.x, _x.approach.direction.vector.y, _x.approach.direction.vector.z, _x.approach.desired_distance, _x.approach.min_distance))
01154 _x = self.collision_object_name
01155 length = len(_x)
01156 if python3 or type(_x) == unicode:
01157 _x = _x.encode('utf-8')
01158 length = len(_x)
01159 buff.write(struct.pack('<I%ss'%length, length, _x))
01160 _x = self.collision_support_surface_name
01161 length = len(_x)
01162 if python3 or type(_x) == unicode:
01163 _x = _x.encode('utf-8')
01164 length = len(_x)
01165 buff.write(struct.pack('<I%ss'%length, length, _x))
01166 _x = self
01167 buff.write(_struct_2BdB.pack(_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test))
01168 length = len(self.path_constraints.joint_constraints)
01169 buff.write(_struct_I.pack(length))
01170 for val1 in self.path_constraints.joint_constraints:
01171 _x = val1.joint_name
01172 length = len(_x)
01173 if python3 or type(_x) == unicode:
01174 _x = _x.encode('utf-8')
01175 length = len(_x)
01176 buff.write(struct.pack('<I%ss'%length, length, _x))
01177 _x = val1
01178 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01179 length = len(self.path_constraints.position_constraints)
01180 buff.write(_struct_I.pack(length))
01181 for val1 in self.path_constraints.position_constraints:
01182 _v36 = val1.header
01183 buff.write(_struct_I.pack(_v36.seq))
01184 _v37 = _v36.stamp
01185 _x = _v37
01186 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01187 _x = _v36.frame_id
01188 length = len(_x)
01189 if python3 or type(_x) == unicode:
01190 _x = _x.encode('utf-8')
01191 length = len(_x)
01192 buff.write(struct.pack('<I%ss'%length, length, _x))
01193 _x = val1.link_name
01194 length = len(_x)
01195 if python3 or type(_x) == unicode:
01196 _x = _x.encode('utf-8')
01197 length = len(_x)
01198 buff.write(struct.pack('<I%ss'%length, length, _x))
01199 _v38 = val1.target_point_offset
01200 _x = _v38
01201 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01202 _v39 = val1.position
01203 _x = _v39
01204 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01205 _v40 = val1.constraint_region_shape
01206 buff.write(_struct_b.pack(_v40.type))
01207 length = len(_v40.dimensions)
01208 buff.write(_struct_I.pack(length))
01209 pattern = '<%sd'%length
01210 buff.write(struct.pack(pattern, *_v40.dimensions))
01211 length = len(_v40.triangles)
01212 buff.write(_struct_I.pack(length))
01213 pattern = '<%si'%length
01214 buff.write(struct.pack(pattern, *_v40.triangles))
01215 length = len(_v40.vertices)
01216 buff.write(_struct_I.pack(length))
01217 for val3 in _v40.vertices:
01218 _x = val3
01219 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01220 _v41 = val1.constraint_region_orientation
01221 _x = _v41
01222 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01223 buff.write(_struct_d.pack(val1.weight))
01224 length = len(self.path_constraints.orientation_constraints)
01225 buff.write(_struct_I.pack(length))
01226 for val1 in self.path_constraints.orientation_constraints:
01227 _v42 = val1.header
01228 buff.write(_struct_I.pack(_v42.seq))
01229 _v43 = _v42.stamp
01230 _x = _v43
01231 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01232 _x = _v42.frame_id
01233 length = len(_x)
01234 if python3 or type(_x) == unicode:
01235 _x = _x.encode('utf-8')
01236 length = len(_x)
01237 buff.write(struct.pack('<I%ss'%length, length, _x))
01238 _x = val1.link_name
01239 length = len(_x)
01240 if python3 or type(_x) == unicode:
01241 _x = _x.encode('utf-8')
01242 length = len(_x)
01243 buff.write(struct.pack('<I%ss'%length, length, _x))
01244 buff.write(_struct_i.pack(val1.type))
01245 _v44 = val1.orientation
01246 _x = _v44
01247 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01248 _x = val1
01249 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01250 length = len(self.path_constraints.visibility_constraints)
01251 buff.write(_struct_I.pack(length))
01252 for val1 in self.path_constraints.visibility_constraints:
01253 _v45 = val1.header
01254 buff.write(_struct_I.pack(_v45.seq))
01255 _v46 = _v45.stamp
01256 _x = _v46
01257 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01258 _x = _v45.frame_id
01259 length = len(_x)
01260 if python3 or type(_x) == unicode:
01261 _x = _x.encode('utf-8')
01262 length = len(_x)
01263 buff.write(struct.pack('<I%ss'%length, length, _x))
01264 _v47 = val1.target
01265 _v48 = _v47.header
01266 buff.write(_struct_I.pack(_v48.seq))
01267 _v49 = _v48.stamp
01268 _x = _v49
01269 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01270 _x = _v48.frame_id
01271 length = len(_x)
01272 if python3 or type(_x) == unicode:
01273 _x = _x.encode('utf-8')
01274 length = len(_x)
01275 buff.write(struct.pack('<I%ss'%length, length, _x))
01276 _v50 = _v47.point
01277 _x = _v50
01278 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01279 _v51 = val1.sensor_pose
01280 _v52 = _v51.header
01281 buff.write(_struct_I.pack(_v52.seq))
01282 _v53 = _v52.stamp
01283 _x = _v53
01284 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01285 _x = _v52.frame_id
01286 length = len(_x)
01287 if python3 or type(_x) == unicode:
01288 _x = _x.encode('utf-8')
01289 length = len(_x)
01290 buff.write(struct.pack('<I%ss'%length, length, _x))
01291 _v54 = _v51.pose
01292 _v55 = _v54.position
01293 _x = _v55
01294 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01295 _v56 = _v54.orientation
01296 _x = _v56
01297 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01298 buff.write(_struct_d.pack(val1.absolute_tolerance))
01299 length = len(self.additional_collision_operations.collision_operations)
01300 buff.write(_struct_I.pack(length))
01301 for val1 in self.additional_collision_operations.collision_operations:
01302 _x = val1.object1
01303 length = len(_x)
01304 if python3 or type(_x) == unicode:
01305 _x = _x.encode('utf-8')
01306 length = len(_x)
01307 buff.write(struct.pack('<I%ss'%length, length, _x))
01308 _x = val1.object2
01309 length = len(_x)
01310 if python3 or type(_x) == unicode:
01311 _x = _x.encode('utf-8')
01312 length = len(_x)
01313 buff.write(struct.pack('<I%ss'%length, length, _x))
01314 _x = val1
01315 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
01316 length = len(self.additional_link_padding)
01317 buff.write(_struct_I.pack(length))
01318 for val1 in self.additional_link_padding:
01319 _x = val1.link_name
01320 length = len(_x)
01321 if python3 or type(_x) == unicode:
01322 _x = _x.encode('utf-8')
01323 length = len(_x)
01324 buff.write(struct.pack('<I%ss'%length, length, _x))
01325 buff.write(_struct_d.pack(val1.padding))
01326 except struct.error as se: self._check_types(se)
01327 except TypeError as te: self._check_types(te)
01328
01329 def deserialize(self, str):
01330 """
01331 unpack serialized message in str into this message instance
01332 :param str: byte array of serialized message, ``str``
01333 """
01334 try:
01335 if self.place_locations is None:
01336 self.place_locations = None
01337 if self.grasp is None:
01338 self.grasp = object_manipulation_msgs.msg.Grasp()
01339 if self.approach is None:
01340 self.approach = object_manipulation_msgs.msg.GripperTranslation()
01341 if self.path_constraints is None:
01342 self.path_constraints = arm_navigation_msgs.msg.Constraints()
01343 if self.additional_collision_operations is None:
01344 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
01345 if self.additional_link_padding is None:
01346 self.additional_link_padding = None
01347 end = 0
01348 start = end
01349 end += 4
01350 (length,) = _struct_I.unpack(str[start:end])
01351 start = end
01352 end += length
01353 if python3:
01354 self.arm_name = str[start:end].decode('utf-8')
01355 else:
01356 self.arm_name = str[start:end]
01357 start = end
01358 end += 4
01359 (length,) = _struct_I.unpack(str[start:end])
01360 self.place_locations = []
01361 for i in range(0, length):
01362 val1 = geometry_msgs.msg.PoseStamped()
01363 _v57 = val1.header
01364 start = end
01365 end += 4
01366 (_v57.seq,) = _struct_I.unpack(str[start:end])
01367 _v58 = _v57.stamp
01368 _x = _v58
01369 start = end
01370 end += 8
01371 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01372 start = end
01373 end += 4
01374 (length,) = _struct_I.unpack(str[start:end])
01375 start = end
01376 end += length
01377 if python3:
01378 _v57.frame_id = str[start:end].decode('utf-8')
01379 else:
01380 _v57.frame_id = str[start:end]
01381 _v59 = val1.pose
01382 _v60 = _v59.position
01383 _x = _v60
01384 start = end
01385 end += 24
01386 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01387 _v61 = _v59.orientation
01388 _x = _v61
01389 start = end
01390 end += 32
01391 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01392 self.place_locations.append(val1)
01393 _x = self
01394 start = end
01395 end += 12
01396 (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01397 start = end
01398 end += 4
01399 (length,) = _struct_I.unpack(str[start:end])
01400 start = end
01401 end += length
01402 if python3:
01403 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01404 else:
01405 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01406 start = end
01407 end += 4
01408 (length,) = _struct_I.unpack(str[start:end])
01409 self.grasp.pre_grasp_posture.name = []
01410 for i in range(0, length):
01411 start = end
01412 end += 4
01413 (length,) = _struct_I.unpack(str[start:end])
01414 start = end
01415 end += length
01416 if python3:
01417 val1 = str[start:end].decode('utf-8')
01418 else:
01419 val1 = str[start:end]
01420 self.grasp.pre_grasp_posture.name.append(val1)
01421 start = end
01422 end += 4
01423 (length,) = _struct_I.unpack(str[start:end])
01424 pattern = '<%sd'%length
01425 start = end
01426 end += struct.calcsize(pattern)
01427 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01428 start = end
01429 end += 4
01430 (length,) = _struct_I.unpack(str[start:end])
01431 pattern = '<%sd'%length
01432 start = end
01433 end += struct.calcsize(pattern)
01434 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01435 start = end
01436 end += 4
01437 (length,) = _struct_I.unpack(str[start:end])
01438 pattern = '<%sd'%length
01439 start = end
01440 end += struct.calcsize(pattern)
01441 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01442 _x = self
01443 start = end
01444 end += 12
01445 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01446 start = end
01447 end += 4
01448 (length,) = _struct_I.unpack(str[start:end])
01449 start = end
01450 end += length
01451 if python3:
01452 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01453 else:
01454 self.grasp.grasp_posture.header.frame_id = str[start:end]
01455 start = end
01456 end += 4
01457 (length,) = _struct_I.unpack(str[start:end])
01458 self.grasp.grasp_posture.name = []
01459 for i in range(0, length):
01460 start = end
01461 end += 4
01462 (length,) = _struct_I.unpack(str[start:end])
01463 start = end
01464 end += length
01465 if python3:
01466 val1 = str[start:end].decode('utf-8')
01467 else:
01468 val1 = str[start:end]
01469 self.grasp.grasp_posture.name.append(val1)
01470 start = end
01471 end += 4
01472 (length,) = _struct_I.unpack(str[start:end])
01473 pattern = '<%sd'%length
01474 start = end
01475 end += struct.calcsize(pattern)
01476 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
01477 start = end
01478 end += 4
01479 (length,) = _struct_I.unpack(str[start:end])
01480 pattern = '<%sd'%length
01481 start = end
01482 end += struct.calcsize(pattern)
01483 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01484 start = end
01485 end += 4
01486 (length,) = _struct_I.unpack(str[start:end])
01487 pattern = '<%sd'%length
01488 start = end
01489 end += struct.calcsize(pattern)
01490 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01491 _x = self
01492 start = end
01493 end += 73
01494 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end])
01495 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
01496 start = end
01497 end += 4
01498 (length,) = _struct_I.unpack(str[start:end])
01499 self.grasp.moved_obstacles = []
01500 for i in range(0, length):
01501 val1 = object_manipulation_msgs.msg.GraspableObject()
01502 start = end
01503 end += 4
01504 (length,) = _struct_I.unpack(str[start:end])
01505 start = end
01506 end += length
01507 if python3:
01508 val1.reference_frame_id = str[start:end].decode('utf-8')
01509 else:
01510 val1.reference_frame_id = str[start:end]
01511 start = end
01512 end += 4
01513 (length,) = _struct_I.unpack(str[start:end])
01514 val1.potential_models = []
01515 for i in range(0, length):
01516 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
01517 start = end
01518 end += 4
01519 (val2.model_id,) = _struct_i.unpack(str[start:end])
01520 _v62 = val2.pose
01521 _v63 = _v62.header
01522 start = end
01523 end += 4
01524 (_v63.seq,) = _struct_I.unpack(str[start:end])
01525 _v64 = _v63.stamp
01526 _x = _v64
01527 start = end
01528 end += 8
01529 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01530 start = end
01531 end += 4
01532 (length,) = _struct_I.unpack(str[start:end])
01533 start = end
01534 end += length
01535 if python3:
01536 _v63.frame_id = str[start:end].decode('utf-8')
01537 else:
01538 _v63.frame_id = str[start:end]
01539 _v65 = _v62.pose
01540 _v66 = _v65.position
01541 _x = _v66
01542 start = end
01543 end += 24
01544 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01545 _v67 = _v65.orientation
01546 _x = _v67
01547 start = end
01548 end += 32
01549 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01550 start = end
01551 end += 4
01552 (val2.confidence,) = _struct_f.unpack(str[start:end])
01553 start = end
01554 end += 4
01555 (length,) = _struct_I.unpack(str[start:end])
01556 start = end
01557 end += length
01558 if python3:
01559 val2.detector_name = str[start:end].decode('utf-8')
01560 else:
01561 val2.detector_name = str[start:end]
01562 val1.potential_models.append(val2)
01563 _v68 = val1.cluster
01564 _v69 = _v68.header
01565 start = end
01566 end += 4
01567 (_v69.seq,) = _struct_I.unpack(str[start:end])
01568 _v70 = _v69.stamp
01569 _x = _v70
01570 start = end
01571 end += 8
01572 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01573 start = end
01574 end += 4
01575 (length,) = _struct_I.unpack(str[start:end])
01576 start = end
01577 end += length
01578 if python3:
01579 _v69.frame_id = str[start:end].decode('utf-8')
01580 else:
01581 _v69.frame_id = str[start:end]
01582 start = end
01583 end += 4
01584 (length,) = _struct_I.unpack(str[start:end])
01585 _v68.points = []
01586 for i in range(0, length):
01587 val3 = geometry_msgs.msg.Point32()
01588 _x = val3
01589 start = end
01590 end += 12
01591 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01592 _v68.points.append(val3)
01593 start = end
01594 end += 4
01595 (length,) = _struct_I.unpack(str[start:end])
01596 _v68.channels = []
01597 for i in range(0, length):
01598 val3 = sensor_msgs.msg.ChannelFloat32()
01599 start = end
01600 end += 4
01601 (length,) = _struct_I.unpack(str[start:end])
01602 start = end
01603 end += length
01604 if python3:
01605 val3.name = str[start:end].decode('utf-8')
01606 else:
01607 val3.name = str[start:end]
01608 start = end
01609 end += 4
01610 (length,) = _struct_I.unpack(str[start:end])
01611 pattern = '<%sf'%length
01612 start = end
01613 end += struct.calcsize(pattern)
01614 val3.values = struct.unpack(pattern, str[start:end])
01615 _v68.channels.append(val3)
01616 _v71 = val1.region
01617 _v72 = _v71.cloud
01618 _v73 = _v72.header
01619 start = end
01620 end += 4
01621 (_v73.seq,) = _struct_I.unpack(str[start:end])
01622 _v74 = _v73.stamp
01623 _x = _v74
01624 start = end
01625 end += 8
01626 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01627 start = end
01628 end += 4
01629 (length,) = _struct_I.unpack(str[start:end])
01630 start = end
01631 end += length
01632 if python3:
01633 _v73.frame_id = str[start:end].decode('utf-8')
01634 else:
01635 _v73.frame_id = str[start:end]
01636 _x = _v72
01637 start = end
01638 end += 8
01639 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01640 start = end
01641 end += 4
01642 (length,) = _struct_I.unpack(str[start:end])
01643 _v72.fields = []
01644 for i in range(0, length):
01645 val4 = sensor_msgs.msg.PointField()
01646 start = end
01647 end += 4
01648 (length,) = _struct_I.unpack(str[start:end])
01649 start = end
01650 end += length
01651 if python3:
01652 val4.name = str[start:end].decode('utf-8')
01653 else:
01654 val4.name = str[start:end]
01655 _x = val4
01656 start = end
01657 end += 9
01658 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01659 _v72.fields.append(val4)
01660 _x = _v72
01661 start = end
01662 end += 9
01663 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
01664 _v72.is_bigendian = bool(_v72.is_bigendian)
01665 start = end
01666 end += 4
01667 (length,) = _struct_I.unpack(str[start:end])
01668 start = end
01669 end += length
01670 if python3:
01671 _v72.data = str[start:end].decode('utf-8')
01672 else:
01673 _v72.data = str[start:end]
01674 start = end
01675 end += 1
01676 (_v72.is_dense,) = _struct_B.unpack(str[start:end])
01677 _v72.is_dense = bool(_v72.is_dense)
01678 start = end
01679 end += 4
01680 (length,) = _struct_I.unpack(str[start:end])
01681 pattern = '<%si'%length
01682 start = end
01683 end += struct.calcsize(pattern)
01684 _v71.mask = struct.unpack(pattern, str[start:end])
01685 _v75 = _v71.image
01686 _v76 = _v75.header
01687 start = end
01688 end += 4
01689 (_v76.seq,) = _struct_I.unpack(str[start:end])
01690 _v77 = _v76.stamp
01691 _x = _v77
01692 start = end
01693 end += 8
01694 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01695 start = end
01696 end += 4
01697 (length,) = _struct_I.unpack(str[start:end])
01698 start = end
01699 end += length
01700 if python3:
01701 _v76.frame_id = str[start:end].decode('utf-8')
01702 else:
01703 _v76.frame_id = str[start:end]
01704 _x = _v75
01705 start = end
01706 end += 8
01707 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01708 start = end
01709 end += 4
01710 (length,) = _struct_I.unpack(str[start:end])
01711 start = end
01712 end += length
01713 if python3:
01714 _v75.encoding = str[start:end].decode('utf-8')
01715 else:
01716 _v75.encoding = str[start:end]
01717 _x = _v75
01718 start = end
01719 end += 5
01720 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01721 start = end
01722 end += 4
01723 (length,) = _struct_I.unpack(str[start:end])
01724 start = end
01725 end += length
01726 if python3:
01727 _v75.data = str[start:end].decode('utf-8')
01728 else:
01729 _v75.data = str[start:end]
01730 _v78 = _v71.disparity_image
01731 _v79 = _v78.header
01732 start = end
01733 end += 4
01734 (_v79.seq,) = _struct_I.unpack(str[start:end])
01735 _v80 = _v79.stamp
01736 _x = _v80
01737 start = end
01738 end += 8
01739 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01740 start = end
01741 end += 4
01742 (length,) = _struct_I.unpack(str[start:end])
01743 start = end
01744 end += length
01745 if python3:
01746 _v79.frame_id = str[start:end].decode('utf-8')
01747 else:
01748 _v79.frame_id = str[start:end]
01749 _x = _v78
01750 start = end
01751 end += 8
01752 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01753 start = end
01754 end += 4
01755 (length,) = _struct_I.unpack(str[start:end])
01756 start = end
01757 end += length
01758 if python3:
01759 _v78.encoding = str[start:end].decode('utf-8')
01760 else:
01761 _v78.encoding = str[start:end]
01762 _x = _v78
01763 start = end
01764 end += 5
01765 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01766 start = end
01767 end += 4
01768 (length,) = _struct_I.unpack(str[start:end])
01769 start = end
01770 end += length
01771 if python3:
01772 _v78.data = str[start:end].decode('utf-8')
01773 else:
01774 _v78.data = str[start:end]
01775 _v81 = _v71.cam_info
01776 _v82 = _v81.header
01777 start = end
01778 end += 4
01779 (_v82.seq,) = _struct_I.unpack(str[start:end])
01780 _v83 = _v82.stamp
01781 _x = _v83
01782 start = end
01783 end += 8
01784 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01785 start = end
01786 end += 4
01787 (length,) = _struct_I.unpack(str[start:end])
01788 start = end
01789 end += length
01790 if python3:
01791 _v82.frame_id = str[start:end].decode('utf-8')
01792 else:
01793 _v82.frame_id = str[start:end]
01794 _x = _v81
01795 start = end
01796 end += 8
01797 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01798 start = end
01799 end += 4
01800 (length,) = _struct_I.unpack(str[start:end])
01801 start = end
01802 end += length
01803 if python3:
01804 _v81.distortion_model = str[start:end].decode('utf-8')
01805 else:
01806 _v81.distortion_model = str[start:end]
01807 start = end
01808 end += 4
01809 (length,) = _struct_I.unpack(str[start:end])
01810 pattern = '<%sd'%length
01811 start = end
01812 end += struct.calcsize(pattern)
01813 _v81.D = struct.unpack(pattern, str[start:end])
01814 start = end
01815 end += 72
01816 _v81.K = _struct_9d.unpack(str[start:end])
01817 start = end
01818 end += 72
01819 _v81.R = _struct_9d.unpack(str[start:end])
01820 start = end
01821 end += 96
01822 _v81.P = _struct_12d.unpack(str[start:end])
01823 _x = _v81
01824 start = end
01825 end += 8
01826 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
01827 _v84 = _v81.roi
01828 _x = _v84
01829 start = end
01830 end += 17
01831 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
01832 _v84.do_rectify = bool(_v84.do_rectify)
01833 _v85 = _v71.roi_box_pose
01834 _v86 = _v85.header
01835 start = end
01836 end += 4
01837 (_v86.seq,) = _struct_I.unpack(str[start:end])
01838 _v87 = _v86.stamp
01839 _x = _v87
01840 start = end
01841 end += 8
01842 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01843 start = end
01844 end += 4
01845 (length,) = _struct_I.unpack(str[start:end])
01846 start = end
01847 end += length
01848 if python3:
01849 _v86.frame_id = str[start:end].decode('utf-8')
01850 else:
01851 _v86.frame_id = str[start:end]
01852 _v88 = _v85.pose
01853 _v89 = _v88.position
01854 _x = _v89
01855 start = end
01856 end += 24
01857 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01858 _v90 = _v88.orientation
01859 _x = _v90
01860 start = end
01861 end += 32
01862 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01863 _v91 = _v71.roi_box_dims
01864 _x = _v91
01865 start = end
01866 end += 24
01867 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01868 start = end
01869 end += 4
01870 (length,) = _struct_I.unpack(str[start:end])
01871 start = end
01872 end += length
01873 if python3:
01874 val1.collision_name = str[start:end].decode('utf-8')
01875 else:
01876 val1.collision_name = str[start:end]
01877 self.grasp.moved_obstacles.append(val1)
01878 _x = self
01879 start = end
01880 end += 20
01881 (_x.desired_retreat_distance, _x.min_retreat_distance, _x.approach.direction.header.seq, _x.approach.direction.header.stamp.secs, _x.approach.direction.header.stamp.nsecs,) = _struct_2f3I.unpack(str[start:end])
01882 start = end
01883 end += 4
01884 (length,) = _struct_I.unpack(str[start:end])
01885 start = end
01886 end += length
01887 if python3:
01888 self.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01889 else:
01890 self.approach.direction.header.frame_id = str[start:end]
01891 _x = self
01892 start = end
01893 end += 32
01894 (_x.approach.direction.vector.x, _x.approach.direction.vector.y, _x.approach.direction.vector.z, _x.approach.desired_distance, _x.approach.min_distance,) = _struct_3d2f.unpack(str[start:end])
01895 start = end
01896 end += 4
01897 (length,) = _struct_I.unpack(str[start:end])
01898 start = end
01899 end += length
01900 if python3:
01901 self.collision_object_name = str[start:end].decode('utf-8')
01902 else:
01903 self.collision_object_name = str[start:end]
01904 start = end
01905 end += 4
01906 (length,) = _struct_I.unpack(str[start:end])
01907 start = end
01908 end += length
01909 if python3:
01910 self.collision_support_surface_name = str[start:end].decode('utf-8')
01911 else:
01912 self.collision_support_surface_name = str[start:end]
01913 _x = self
01914 start = end
01915 end += 11
01916 (_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end])
01917 self.allow_gripper_support_collision = bool(self.allow_gripper_support_collision)
01918 self.use_reactive_place = bool(self.use_reactive_place)
01919 self.only_perform_feasibility_test = bool(self.only_perform_feasibility_test)
01920 start = end
01921 end += 4
01922 (length,) = _struct_I.unpack(str[start:end])
01923 self.path_constraints.joint_constraints = []
01924 for i in range(0, length):
01925 val1 = arm_navigation_msgs.msg.JointConstraint()
01926 start = end
01927 end += 4
01928 (length,) = _struct_I.unpack(str[start:end])
01929 start = end
01930 end += length
01931 if python3:
01932 val1.joint_name = str[start:end].decode('utf-8')
01933 else:
01934 val1.joint_name = str[start:end]
01935 _x = val1
01936 start = end
01937 end += 32
01938 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01939 self.path_constraints.joint_constraints.append(val1)
01940 start = end
01941 end += 4
01942 (length,) = _struct_I.unpack(str[start:end])
01943 self.path_constraints.position_constraints = []
01944 for i in range(0, length):
01945 val1 = arm_navigation_msgs.msg.PositionConstraint()
01946 _v92 = val1.header
01947 start = end
01948 end += 4
01949 (_v92.seq,) = _struct_I.unpack(str[start:end])
01950 _v93 = _v92.stamp
01951 _x = _v93
01952 start = end
01953 end += 8
01954 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01955 start = end
01956 end += 4
01957 (length,) = _struct_I.unpack(str[start:end])
01958 start = end
01959 end += length
01960 if python3:
01961 _v92.frame_id = str[start:end].decode('utf-8')
01962 else:
01963 _v92.frame_id = str[start:end]
01964 start = end
01965 end += 4
01966 (length,) = _struct_I.unpack(str[start:end])
01967 start = end
01968 end += length
01969 if python3:
01970 val1.link_name = str[start:end].decode('utf-8')
01971 else:
01972 val1.link_name = str[start:end]
01973 _v94 = val1.target_point_offset
01974 _x = _v94
01975 start = end
01976 end += 24
01977 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01978 _v95 = val1.position
01979 _x = _v95
01980 start = end
01981 end += 24
01982 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01983 _v96 = val1.constraint_region_shape
01984 start = end
01985 end += 1
01986 (_v96.type,) = _struct_b.unpack(str[start:end])
01987 start = end
01988 end += 4
01989 (length,) = _struct_I.unpack(str[start:end])
01990 pattern = '<%sd'%length
01991 start = end
01992 end += struct.calcsize(pattern)
01993 _v96.dimensions = struct.unpack(pattern, str[start:end])
01994 start = end
01995 end += 4
01996 (length,) = _struct_I.unpack(str[start:end])
01997 pattern = '<%si'%length
01998 start = end
01999 end += struct.calcsize(pattern)
02000 _v96.triangles = struct.unpack(pattern, str[start:end])
02001 start = end
02002 end += 4
02003 (length,) = _struct_I.unpack(str[start:end])
02004 _v96.vertices = []
02005 for i in range(0, length):
02006 val3 = geometry_msgs.msg.Point()
02007 _x = val3
02008 start = end
02009 end += 24
02010 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02011 _v96.vertices.append(val3)
02012 _v97 = val1.constraint_region_orientation
02013 _x = _v97
02014 start = end
02015 end += 32
02016 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02017 start = end
02018 end += 8
02019 (val1.weight,) = _struct_d.unpack(str[start:end])
02020 self.path_constraints.position_constraints.append(val1)
02021 start = end
02022 end += 4
02023 (length,) = _struct_I.unpack(str[start:end])
02024 self.path_constraints.orientation_constraints = []
02025 for i in range(0, length):
02026 val1 = arm_navigation_msgs.msg.OrientationConstraint()
02027 _v98 = val1.header
02028 start = end
02029 end += 4
02030 (_v98.seq,) = _struct_I.unpack(str[start:end])
02031 _v99 = _v98.stamp
02032 _x = _v99
02033 start = end
02034 end += 8
02035 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02036 start = end
02037 end += 4
02038 (length,) = _struct_I.unpack(str[start:end])
02039 start = end
02040 end += length
02041 if python3:
02042 _v98.frame_id = str[start:end].decode('utf-8')
02043 else:
02044 _v98.frame_id = str[start:end]
02045 start = end
02046 end += 4
02047 (length,) = _struct_I.unpack(str[start:end])
02048 start = end
02049 end += length
02050 if python3:
02051 val1.link_name = str[start:end].decode('utf-8')
02052 else:
02053 val1.link_name = str[start:end]
02054 start = end
02055 end += 4
02056 (val1.type,) = _struct_i.unpack(str[start:end])
02057 _v100 = val1.orientation
02058 _x = _v100
02059 start = end
02060 end += 32
02061 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02062 _x = val1
02063 start = end
02064 end += 32
02065 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02066 self.path_constraints.orientation_constraints.append(val1)
02067 start = end
02068 end += 4
02069 (length,) = _struct_I.unpack(str[start:end])
02070 self.path_constraints.visibility_constraints = []
02071 for i in range(0, length):
02072 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02073 _v101 = val1.header
02074 start = end
02075 end += 4
02076 (_v101.seq,) = _struct_I.unpack(str[start:end])
02077 _v102 = _v101.stamp
02078 _x = _v102
02079 start = end
02080 end += 8
02081 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02082 start = end
02083 end += 4
02084 (length,) = _struct_I.unpack(str[start:end])
02085 start = end
02086 end += length
02087 if python3:
02088 _v101.frame_id = str[start:end].decode('utf-8')
02089 else:
02090 _v101.frame_id = str[start:end]
02091 _v103 = val1.target
02092 _v104 = _v103.header
02093 start = end
02094 end += 4
02095 (_v104.seq,) = _struct_I.unpack(str[start:end])
02096 _v105 = _v104.stamp
02097 _x = _v105
02098 start = end
02099 end += 8
02100 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02101 start = end
02102 end += 4
02103 (length,) = _struct_I.unpack(str[start:end])
02104 start = end
02105 end += length
02106 if python3:
02107 _v104.frame_id = str[start:end].decode('utf-8')
02108 else:
02109 _v104.frame_id = str[start:end]
02110 _v106 = _v103.point
02111 _x = _v106
02112 start = end
02113 end += 24
02114 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02115 _v107 = val1.sensor_pose
02116 _v108 = _v107.header
02117 start = end
02118 end += 4
02119 (_v108.seq,) = _struct_I.unpack(str[start:end])
02120 _v109 = _v108.stamp
02121 _x = _v109
02122 start = end
02123 end += 8
02124 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02125 start = end
02126 end += 4
02127 (length,) = _struct_I.unpack(str[start:end])
02128 start = end
02129 end += length
02130 if python3:
02131 _v108.frame_id = str[start:end].decode('utf-8')
02132 else:
02133 _v108.frame_id = str[start:end]
02134 _v110 = _v107.pose
02135 _v111 = _v110.position
02136 _x = _v111
02137 start = end
02138 end += 24
02139 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02140 _v112 = _v110.orientation
02141 _x = _v112
02142 start = end
02143 end += 32
02144 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02145 start = end
02146 end += 8
02147 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02148 self.path_constraints.visibility_constraints.append(val1)
02149 start = end
02150 end += 4
02151 (length,) = _struct_I.unpack(str[start:end])
02152 self.additional_collision_operations.collision_operations = []
02153 for i in range(0, length):
02154 val1 = arm_navigation_msgs.msg.CollisionOperation()
02155 start = end
02156 end += 4
02157 (length,) = _struct_I.unpack(str[start:end])
02158 start = end
02159 end += length
02160 if python3:
02161 val1.object1 = str[start:end].decode('utf-8')
02162 else:
02163 val1.object1 = str[start:end]
02164 start = end
02165 end += 4
02166 (length,) = _struct_I.unpack(str[start:end])
02167 start = end
02168 end += length
02169 if python3:
02170 val1.object2 = str[start:end].decode('utf-8')
02171 else:
02172 val1.object2 = str[start:end]
02173 _x = val1
02174 start = end
02175 end += 12
02176 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
02177 self.additional_collision_operations.collision_operations.append(val1)
02178 start = end
02179 end += 4
02180 (length,) = _struct_I.unpack(str[start:end])
02181 self.additional_link_padding = []
02182 for i in range(0, length):
02183 val1 = arm_navigation_msgs.msg.LinkPadding()
02184 start = end
02185 end += 4
02186 (length,) = _struct_I.unpack(str[start:end])
02187 start = end
02188 end += length
02189 if python3:
02190 val1.link_name = str[start:end].decode('utf-8')
02191 else:
02192 val1.link_name = str[start:end]
02193 start = end
02194 end += 8
02195 (val1.padding,) = _struct_d.unpack(str[start:end])
02196 self.additional_link_padding.append(val1)
02197 return self
02198 except struct.error as e:
02199 raise genpy.DeserializationError(e) #most likely buffer underfill
02200
02201
02202 def serialize_numpy(self, buff, numpy):
02203 """
02204 serialize message with numpy array types into buffer
02205 :param buff: buffer, ``StringIO``
02206 :param numpy: numpy python module
02207 """
02208 try:
02209 _x = self.arm_name
02210 length = len(_x)
02211 if python3 or type(_x) == unicode:
02212 _x = _x.encode('utf-8')
02213 length = len(_x)
02214 buff.write(struct.pack('<I%ss'%length, length, _x))
02215 length = len(self.place_locations)
02216 buff.write(_struct_I.pack(length))
02217 for val1 in self.place_locations:
02218 _v113 = val1.header
02219 buff.write(_struct_I.pack(_v113.seq))
02220 _v114 = _v113.stamp
02221 _x = _v114
02222 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02223 _x = _v113.frame_id
02224 length = len(_x)
02225 if python3 or type(_x) == unicode:
02226 _x = _x.encode('utf-8')
02227 length = len(_x)
02228 buff.write(struct.pack('<I%ss'%length, length, _x))
02229 _v115 = val1.pose
02230 _v116 = _v115.position
02231 _x = _v116
02232 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02233 _v117 = _v115.orientation
02234 _x = _v117
02235 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02236 _x = self
02237 buff.write(_struct_3I.pack(_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
02238 _x = self.grasp.pre_grasp_posture.header.frame_id
02239 length = len(_x)
02240 if python3 or type(_x) == unicode:
02241 _x = _x.encode('utf-8')
02242 length = len(_x)
02243 buff.write(struct.pack('<I%ss'%length, length, _x))
02244 length = len(self.grasp.pre_grasp_posture.name)
02245 buff.write(_struct_I.pack(length))
02246 for val1 in self.grasp.pre_grasp_posture.name:
02247 length = len(val1)
02248 if python3 or type(val1) == unicode:
02249 val1 = val1.encode('utf-8')
02250 length = len(val1)
02251 buff.write(struct.pack('<I%ss'%length, length, val1))
02252 length = len(self.grasp.pre_grasp_posture.position)
02253 buff.write(_struct_I.pack(length))
02254 pattern = '<%sd'%length
02255 buff.write(self.grasp.pre_grasp_posture.position.tostring())
02256 length = len(self.grasp.pre_grasp_posture.velocity)
02257 buff.write(_struct_I.pack(length))
02258 pattern = '<%sd'%length
02259 buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
02260 length = len(self.grasp.pre_grasp_posture.effort)
02261 buff.write(_struct_I.pack(length))
02262 pattern = '<%sd'%length
02263 buff.write(self.grasp.pre_grasp_posture.effort.tostring())
02264 _x = self
02265 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
02266 _x = self.grasp.grasp_posture.header.frame_id
02267 length = len(_x)
02268 if python3 or type(_x) == unicode:
02269 _x = _x.encode('utf-8')
02270 length = len(_x)
02271 buff.write(struct.pack('<I%ss'%length, length, _x))
02272 length = len(self.grasp.grasp_posture.name)
02273 buff.write(_struct_I.pack(length))
02274 for val1 in self.grasp.grasp_posture.name:
02275 length = len(val1)
02276 if python3 or type(val1) == unicode:
02277 val1 = val1.encode('utf-8')
02278 length = len(val1)
02279 buff.write(struct.pack('<I%ss'%length, length, val1))
02280 length = len(self.grasp.grasp_posture.position)
02281 buff.write(_struct_I.pack(length))
02282 pattern = '<%sd'%length
02283 buff.write(self.grasp.grasp_posture.position.tostring())
02284 length = len(self.grasp.grasp_posture.velocity)
02285 buff.write(_struct_I.pack(length))
02286 pattern = '<%sd'%length
02287 buff.write(self.grasp.grasp_posture.velocity.tostring())
02288 length = len(self.grasp.grasp_posture.effort)
02289 buff.write(_struct_I.pack(length))
02290 pattern = '<%sd'%length
02291 buff.write(self.grasp.grasp_posture.effort.tostring())
02292 _x = self
02293 buff.write(_struct_8dB2f.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance))
02294 length = len(self.grasp.moved_obstacles)
02295 buff.write(_struct_I.pack(length))
02296 for val1 in self.grasp.moved_obstacles:
02297 _x = val1.reference_frame_id
02298 length = len(_x)
02299 if python3 or type(_x) == unicode:
02300 _x = _x.encode('utf-8')
02301 length = len(_x)
02302 buff.write(struct.pack('<I%ss'%length, length, _x))
02303 length = len(val1.potential_models)
02304 buff.write(_struct_I.pack(length))
02305 for val2 in val1.potential_models:
02306 buff.write(_struct_i.pack(val2.model_id))
02307 _v118 = val2.pose
02308 _v119 = _v118.header
02309 buff.write(_struct_I.pack(_v119.seq))
02310 _v120 = _v119.stamp
02311 _x = _v120
02312 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02313 _x = _v119.frame_id
02314 length = len(_x)
02315 if python3 or type(_x) == unicode:
02316 _x = _x.encode('utf-8')
02317 length = len(_x)
02318 buff.write(struct.pack('<I%ss'%length, length, _x))
02319 _v121 = _v118.pose
02320 _v122 = _v121.position
02321 _x = _v122
02322 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02323 _v123 = _v121.orientation
02324 _x = _v123
02325 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02326 buff.write(_struct_f.pack(val2.confidence))
02327 _x = val2.detector_name
02328 length = len(_x)
02329 if python3 or type(_x) == unicode:
02330 _x = _x.encode('utf-8')
02331 length = len(_x)
02332 buff.write(struct.pack('<I%ss'%length, length, _x))
02333 _v124 = val1.cluster
02334 _v125 = _v124.header
02335 buff.write(_struct_I.pack(_v125.seq))
02336 _v126 = _v125.stamp
02337 _x = _v126
02338 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02339 _x = _v125.frame_id
02340 length = len(_x)
02341 if python3 or type(_x) == unicode:
02342 _x = _x.encode('utf-8')
02343 length = len(_x)
02344 buff.write(struct.pack('<I%ss'%length, length, _x))
02345 length = len(_v124.points)
02346 buff.write(_struct_I.pack(length))
02347 for val3 in _v124.points:
02348 _x = val3
02349 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02350 length = len(_v124.channels)
02351 buff.write(_struct_I.pack(length))
02352 for val3 in _v124.channels:
02353 _x = val3.name
02354 length = len(_x)
02355 if python3 or type(_x) == unicode:
02356 _x = _x.encode('utf-8')
02357 length = len(_x)
02358 buff.write(struct.pack('<I%ss'%length, length, _x))
02359 length = len(val3.values)
02360 buff.write(_struct_I.pack(length))
02361 pattern = '<%sf'%length
02362 buff.write(val3.values.tostring())
02363 _v127 = val1.region
02364 _v128 = _v127.cloud
02365 _v129 = _v128.header
02366 buff.write(_struct_I.pack(_v129.seq))
02367 _v130 = _v129.stamp
02368 _x = _v130
02369 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02370 _x = _v129.frame_id
02371 length = len(_x)
02372 if python3 or type(_x) == unicode:
02373 _x = _x.encode('utf-8')
02374 length = len(_x)
02375 buff.write(struct.pack('<I%ss'%length, length, _x))
02376 _x = _v128
02377 buff.write(_struct_2I.pack(_x.height, _x.width))
02378 length = len(_v128.fields)
02379 buff.write(_struct_I.pack(length))
02380 for val4 in _v128.fields:
02381 _x = val4.name
02382 length = len(_x)
02383 if python3 or type(_x) == unicode:
02384 _x = _x.encode('utf-8')
02385 length = len(_x)
02386 buff.write(struct.pack('<I%ss'%length, length, _x))
02387 _x = val4
02388 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
02389 _x = _v128
02390 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
02391 _x = _v128.data
02392 length = len(_x)
02393 # - if encoded as a list instead, serialize as bytes instead of string
02394 if type(_x) in [list, tuple]:
02395 buff.write(struct.pack('<I%sB'%length, length, *_x))
02396 else:
02397 buff.write(struct.pack('<I%ss'%length, length, _x))
02398 buff.write(_struct_B.pack(_v128.is_dense))
02399 length = len(_v127.mask)
02400 buff.write(_struct_I.pack(length))
02401 pattern = '<%si'%length
02402 buff.write(_v127.mask.tostring())
02403 _v131 = _v127.image
02404 _v132 = _v131.header
02405 buff.write(_struct_I.pack(_v132.seq))
02406 _v133 = _v132.stamp
02407 _x = _v133
02408 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02409 _x = _v132.frame_id
02410 length = len(_x)
02411 if python3 or type(_x) == unicode:
02412 _x = _x.encode('utf-8')
02413 length = len(_x)
02414 buff.write(struct.pack('<I%ss'%length, length, _x))
02415 _x = _v131
02416 buff.write(_struct_2I.pack(_x.height, _x.width))
02417 _x = _v131.encoding
02418 length = len(_x)
02419 if python3 or type(_x) == unicode:
02420 _x = _x.encode('utf-8')
02421 length = len(_x)
02422 buff.write(struct.pack('<I%ss'%length, length, _x))
02423 _x = _v131
02424 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02425 _x = _v131.data
02426 length = len(_x)
02427 # - if encoded as a list instead, serialize as bytes instead of string
02428 if type(_x) in [list, tuple]:
02429 buff.write(struct.pack('<I%sB'%length, length, *_x))
02430 else:
02431 buff.write(struct.pack('<I%ss'%length, length, _x))
02432 _v134 = _v127.disparity_image
02433 _v135 = _v134.header
02434 buff.write(_struct_I.pack(_v135.seq))
02435 _v136 = _v135.stamp
02436 _x = _v136
02437 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02438 _x = _v135.frame_id
02439 length = len(_x)
02440 if python3 or type(_x) == unicode:
02441 _x = _x.encode('utf-8')
02442 length = len(_x)
02443 buff.write(struct.pack('<I%ss'%length, length, _x))
02444 _x = _v134
02445 buff.write(_struct_2I.pack(_x.height, _x.width))
02446 _x = _v134.encoding
02447 length = len(_x)
02448 if python3 or type(_x) == unicode:
02449 _x = _x.encode('utf-8')
02450 length = len(_x)
02451 buff.write(struct.pack('<I%ss'%length, length, _x))
02452 _x = _v134
02453 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02454 _x = _v134.data
02455 length = len(_x)
02456 # - if encoded as a list instead, serialize as bytes instead of string
02457 if type(_x) in [list, tuple]:
02458 buff.write(struct.pack('<I%sB'%length, length, *_x))
02459 else:
02460 buff.write(struct.pack('<I%ss'%length, length, _x))
02461 _v137 = _v127.cam_info
02462 _v138 = _v137.header
02463 buff.write(_struct_I.pack(_v138.seq))
02464 _v139 = _v138.stamp
02465 _x = _v139
02466 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02467 _x = _v138.frame_id
02468 length = len(_x)
02469 if python3 or type(_x) == unicode:
02470 _x = _x.encode('utf-8')
02471 length = len(_x)
02472 buff.write(struct.pack('<I%ss'%length, length, _x))
02473 _x = _v137
02474 buff.write(_struct_2I.pack(_x.height, _x.width))
02475 _x = _v137.distortion_model
02476 length = len(_x)
02477 if python3 or type(_x) == unicode:
02478 _x = _x.encode('utf-8')
02479 length = len(_x)
02480 buff.write(struct.pack('<I%ss'%length, length, _x))
02481 length = len(_v137.D)
02482 buff.write(_struct_I.pack(length))
02483 pattern = '<%sd'%length
02484 buff.write(_v137.D.tostring())
02485 buff.write(_v137.K.tostring())
02486 buff.write(_v137.R.tostring())
02487 buff.write(_v137.P.tostring())
02488 _x = _v137
02489 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
02490 _v140 = _v137.roi
02491 _x = _v140
02492 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
02493 _v141 = _v127.roi_box_pose
02494 _v142 = _v141.header
02495 buff.write(_struct_I.pack(_v142.seq))
02496 _v143 = _v142.stamp
02497 _x = _v143
02498 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02499 _x = _v142.frame_id
02500 length = len(_x)
02501 if python3 or type(_x) == unicode:
02502 _x = _x.encode('utf-8')
02503 length = len(_x)
02504 buff.write(struct.pack('<I%ss'%length, length, _x))
02505 _v144 = _v141.pose
02506 _v145 = _v144.position
02507 _x = _v145
02508 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02509 _v146 = _v144.orientation
02510 _x = _v146
02511 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02512 _v147 = _v127.roi_box_dims
02513 _x = _v147
02514 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02515 _x = val1.collision_name
02516 length = len(_x)
02517 if python3 or type(_x) == unicode:
02518 _x = _x.encode('utf-8')
02519 length = len(_x)
02520 buff.write(struct.pack('<I%ss'%length, length, _x))
02521 _x = self
02522 buff.write(_struct_2f3I.pack(_x.desired_retreat_distance, _x.min_retreat_distance, _x.approach.direction.header.seq, _x.approach.direction.header.stamp.secs, _x.approach.direction.header.stamp.nsecs))
02523 _x = self.approach.direction.header.frame_id
02524 length = len(_x)
02525 if python3 or type(_x) == unicode:
02526 _x = _x.encode('utf-8')
02527 length = len(_x)
02528 buff.write(struct.pack('<I%ss'%length, length, _x))
02529 _x = self
02530 buff.write(_struct_3d2f.pack(_x.approach.direction.vector.x, _x.approach.direction.vector.y, _x.approach.direction.vector.z, _x.approach.desired_distance, _x.approach.min_distance))
02531 _x = self.collision_object_name
02532 length = len(_x)
02533 if python3 or type(_x) == unicode:
02534 _x = _x.encode('utf-8')
02535 length = len(_x)
02536 buff.write(struct.pack('<I%ss'%length, length, _x))
02537 _x = self.collision_support_surface_name
02538 length = len(_x)
02539 if python3 or type(_x) == unicode:
02540 _x = _x.encode('utf-8')
02541 length = len(_x)
02542 buff.write(struct.pack('<I%ss'%length, length, _x))
02543 _x = self
02544 buff.write(_struct_2BdB.pack(_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test))
02545 length = len(self.path_constraints.joint_constraints)
02546 buff.write(_struct_I.pack(length))
02547 for val1 in self.path_constraints.joint_constraints:
02548 _x = val1.joint_name
02549 length = len(_x)
02550 if python3 or type(_x) == unicode:
02551 _x = _x.encode('utf-8')
02552 length = len(_x)
02553 buff.write(struct.pack('<I%ss'%length, length, _x))
02554 _x = val1
02555 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
02556 length = len(self.path_constraints.position_constraints)
02557 buff.write(_struct_I.pack(length))
02558 for val1 in self.path_constraints.position_constraints:
02559 _v148 = val1.header
02560 buff.write(_struct_I.pack(_v148.seq))
02561 _v149 = _v148.stamp
02562 _x = _v149
02563 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02564 _x = _v148.frame_id
02565 length = len(_x)
02566 if python3 or type(_x) == unicode:
02567 _x = _x.encode('utf-8')
02568 length = len(_x)
02569 buff.write(struct.pack('<I%ss'%length, length, _x))
02570 _x = val1.link_name
02571 length = len(_x)
02572 if python3 or type(_x) == unicode:
02573 _x = _x.encode('utf-8')
02574 length = len(_x)
02575 buff.write(struct.pack('<I%ss'%length, length, _x))
02576 _v150 = val1.target_point_offset
02577 _x = _v150
02578 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02579 _v151 = val1.position
02580 _x = _v151
02581 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02582 _v152 = val1.constraint_region_shape
02583 buff.write(_struct_b.pack(_v152.type))
02584 length = len(_v152.dimensions)
02585 buff.write(_struct_I.pack(length))
02586 pattern = '<%sd'%length
02587 buff.write(_v152.dimensions.tostring())
02588 length = len(_v152.triangles)
02589 buff.write(_struct_I.pack(length))
02590 pattern = '<%si'%length
02591 buff.write(_v152.triangles.tostring())
02592 length = len(_v152.vertices)
02593 buff.write(_struct_I.pack(length))
02594 for val3 in _v152.vertices:
02595 _x = val3
02596 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02597 _v153 = val1.constraint_region_orientation
02598 _x = _v153
02599 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02600 buff.write(_struct_d.pack(val1.weight))
02601 length = len(self.path_constraints.orientation_constraints)
02602 buff.write(_struct_I.pack(length))
02603 for val1 in self.path_constraints.orientation_constraints:
02604 _v154 = val1.header
02605 buff.write(_struct_I.pack(_v154.seq))
02606 _v155 = _v154.stamp
02607 _x = _v155
02608 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02609 _x = _v154.frame_id
02610 length = len(_x)
02611 if python3 or type(_x) == unicode:
02612 _x = _x.encode('utf-8')
02613 length = len(_x)
02614 buff.write(struct.pack('<I%ss'%length, length, _x))
02615 _x = val1.link_name
02616 length = len(_x)
02617 if python3 or type(_x) == unicode:
02618 _x = _x.encode('utf-8')
02619 length = len(_x)
02620 buff.write(struct.pack('<I%ss'%length, length, _x))
02621 buff.write(_struct_i.pack(val1.type))
02622 _v156 = val1.orientation
02623 _x = _v156
02624 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02625 _x = val1
02626 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
02627 length = len(self.path_constraints.visibility_constraints)
02628 buff.write(_struct_I.pack(length))
02629 for val1 in self.path_constraints.visibility_constraints:
02630 _v157 = val1.header
02631 buff.write(_struct_I.pack(_v157.seq))
02632 _v158 = _v157.stamp
02633 _x = _v158
02634 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02635 _x = _v157.frame_id
02636 length = len(_x)
02637 if python3 or type(_x) == unicode:
02638 _x = _x.encode('utf-8')
02639 length = len(_x)
02640 buff.write(struct.pack('<I%ss'%length, length, _x))
02641 _v159 = val1.target
02642 _v160 = _v159.header
02643 buff.write(_struct_I.pack(_v160.seq))
02644 _v161 = _v160.stamp
02645 _x = _v161
02646 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02647 _x = _v160.frame_id
02648 length = len(_x)
02649 if python3 or type(_x) == unicode:
02650 _x = _x.encode('utf-8')
02651 length = len(_x)
02652 buff.write(struct.pack('<I%ss'%length, length, _x))
02653 _v162 = _v159.point
02654 _x = _v162
02655 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02656 _v163 = val1.sensor_pose
02657 _v164 = _v163.header
02658 buff.write(_struct_I.pack(_v164.seq))
02659 _v165 = _v164.stamp
02660 _x = _v165
02661 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02662 _x = _v164.frame_id
02663 length = len(_x)
02664 if python3 or type(_x) == unicode:
02665 _x = _x.encode('utf-8')
02666 length = len(_x)
02667 buff.write(struct.pack('<I%ss'%length, length, _x))
02668 _v166 = _v163.pose
02669 _v167 = _v166.position
02670 _x = _v167
02671 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02672 _v168 = _v166.orientation
02673 _x = _v168
02674 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02675 buff.write(_struct_d.pack(val1.absolute_tolerance))
02676 length = len(self.additional_collision_operations.collision_operations)
02677 buff.write(_struct_I.pack(length))
02678 for val1 in self.additional_collision_operations.collision_operations:
02679 _x = val1.object1
02680 length = len(_x)
02681 if python3 or type(_x) == unicode:
02682 _x = _x.encode('utf-8')
02683 length = len(_x)
02684 buff.write(struct.pack('<I%ss'%length, length, _x))
02685 _x = val1.object2
02686 length = len(_x)
02687 if python3 or type(_x) == unicode:
02688 _x = _x.encode('utf-8')
02689 length = len(_x)
02690 buff.write(struct.pack('<I%ss'%length, length, _x))
02691 _x = val1
02692 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
02693 length = len(self.additional_link_padding)
02694 buff.write(_struct_I.pack(length))
02695 for val1 in self.additional_link_padding:
02696 _x = val1.link_name
02697 length = len(_x)
02698 if python3 or type(_x) == unicode:
02699 _x = _x.encode('utf-8')
02700 length = len(_x)
02701 buff.write(struct.pack('<I%ss'%length, length, _x))
02702 buff.write(_struct_d.pack(val1.padding))
02703 except struct.error as se: self._check_types(se)
02704 except TypeError as te: self._check_types(te)
02705
02706 def deserialize_numpy(self, str, numpy):
02707 """
02708 unpack serialized message in str into this message instance using numpy for array types
02709 :param str: byte array of serialized message, ``str``
02710 :param numpy: numpy python module
02711 """
02712 try:
02713 if self.place_locations is None:
02714 self.place_locations = None
02715 if self.grasp is None:
02716 self.grasp = object_manipulation_msgs.msg.Grasp()
02717 if self.approach is None:
02718 self.approach = object_manipulation_msgs.msg.GripperTranslation()
02719 if self.path_constraints is None:
02720 self.path_constraints = arm_navigation_msgs.msg.Constraints()
02721 if self.additional_collision_operations is None:
02722 self.additional_collision_operations = arm_navigation_msgs.msg.OrderedCollisionOperations()
02723 if self.additional_link_padding is None:
02724 self.additional_link_padding = None
02725 end = 0
02726 start = end
02727 end += 4
02728 (length,) = _struct_I.unpack(str[start:end])
02729 start = end
02730 end += length
02731 if python3:
02732 self.arm_name = str[start:end].decode('utf-8')
02733 else:
02734 self.arm_name = str[start:end]
02735 start = end
02736 end += 4
02737 (length,) = _struct_I.unpack(str[start:end])
02738 self.place_locations = []
02739 for i in range(0, length):
02740 val1 = geometry_msgs.msg.PoseStamped()
02741 _v169 = val1.header
02742 start = end
02743 end += 4
02744 (_v169.seq,) = _struct_I.unpack(str[start:end])
02745 _v170 = _v169.stamp
02746 _x = _v170
02747 start = end
02748 end += 8
02749 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02750 start = end
02751 end += 4
02752 (length,) = _struct_I.unpack(str[start:end])
02753 start = end
02754 end += length
02755 if python3:
02756 _v169.frame_id = str[start:end].decode('utf-8')
02757 else:
02758 _v169.frame_id = str[start:end]
02759 _v171 = val1.pose
02760 _v172 = _v171.position
02761 _x = _v172
02762 start = end
02763 end += 24
02764 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02765 _v173 = _v171.orientation
02766 _x = _v173
02767 start = end
02768 end += 32
02769 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02770 self.place_locations.append(val1)
02771 _x = self
02772 start = end
02773 end += 12
02774 (_x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02775 start = end
02776 end += 4
02777 (length,) = _struct_I.unpack(str[start:end])
02778 start = end
02779 end += length
02780 if python3:
02781 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02782 else:
02783 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
02784 start = end
02785 end += 4
02786 (length,) = _struct_I.unpack(str[start:end])
02787 self.grasp.pre_grasp_posture.name = []
02788 for i in range(0, length):
02789 start = end
02790 end += 4
02791 (length,) = _struct_I.unpack(str[start:end])
02792 start = end
02793 end += length
02794 if python3:
02795 val1 = str[start:end].decode('utf-8')
02796 else:
02797 val1 = str[start:end]
02798 self.grasp.pre_grasp_posture.name.append(val1)
02799 start = end
02800 end += 4
02801 (length,) = _struct_I.unpack(str[start:end])
02802 pattern = '<%sd'%length
02803 start = end
02804 end += struct.calcsize(pattern)
02805 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02806 start = end
02807 end += 4
02808 (length,) = _struct_I.unpack(str[start:end])
02809 pattern = '<%sd'%length
02810 start = end
02811 end += struct.calcsize(pattern)
02812 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02813 start = end
02814 end += 4
02815 (length,) = _struct_I.unpack(str[start:end])
02816 pattern = '<%sd'%length
02817 start = end
02818 end += struct.calcsize(pattern)
02819 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02820 _x = self
02821 start = end
02822 end += 12
02823 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02824 start = end
02825 end += 4
02826 (length,) = _struct_I.unpack(str[start:end])
02827 start = end
02828 end += length
02829 if python3:
02830 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02831 else:
02832 self.grasp.grasp_posture.header.frame_id = str[start:end]
02833 start = end
02834 end += 4
02835 (length,) = _struct_I.unpack(str[start:end])
02836 self.grasp.grasp_posture.name = []
02837 for i in range(0, length):
02838 start = end
02839 end += 4
02840 (length,) = _struct_I.unpack(str[start:end])
02841 start = end
02842 end += length
02843 if python3:
02844 val1 = str[start:end].decode('utf-8')
02845 else:
02846 val1 = str[start:end]
02847 self.grasp.grasp_posture.name.append(val1)
02848 start = end
02849 end += 4
02850 (length,) = _struct_I.unpack(str[start:end])
02851 pattern = '<%sd'%length
02852 start = end
02853 end += struct.calcsize(pattern)
02854 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02855 start = end
02856 end += 4
02857 (length,) = _struct_I.unpack(str[start:end])
02858 pattern = '<%sd'%length
02859 start = end
02860 end += struct.calcsize(pattern)
02861 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02862 start = end
02863 end += 4
02864 (length,) = _struct_I.unpack(str[start:end])
02865 pattern = '<%sd'%length
02866 start = end
02867 end += struct.calcsize(pattern)
02868 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02869 _x = self
02870 start = end
02871 end += 73
02872 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end])
02873 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
02874 start = end
02875 end += 4
02876 (length,) = _struct_I.unpack(str[start:end])
02877 self.grasp.moved_obstacles = []
02878 for i in range(0, length):
02879 val1 = object_manipulation_msgs.msg.GraspableObject()
02880 start = end
02881 end += 4
02882 (length,) = _struct_I.unpack(str[start:end])
02883 start = end
02884 end += length
02885 if python3:
02886 val1.reference_frame_id = str[start:end].decode('utf-8')
02887 else:
02888 val1.reference_frame_id = str[start:end]
02889 start = end
02890 end += 4
02891 (length,) = _struct_I.unpack(str[start:end])
02892 val1.potential_models = []
02893 for i in range(0, length):
02894 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
02895 start = end
02896 end += 4
02897 (val2.model_id,) = _struct_i.unpack(str[start:end])
02898 _v174 = val2.pose
02899 _v175 = _v174.header
02900 start = end
02901 end += 4
02902 (_v175.seq,) = _struct_I.unpack(str[start:end])
02903 _v176 = _v175.stamp
02904 _x = _v176
02905 start = end
02906 end += 8
02907 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02908 start = end
02909 end += 4
02910 (length,) = _struct_I.unpack(str[start:end])
02911 start = end
02912 end += length
02913 if python3:
02914 _v175.frame_id = str[start:end].decode('utf-8')
02915 else:
02916 _v175.frame_id = str[start:end]
02917 _v177 = _v174.pose
02918 _v178 = _v177.position
02919 _x = _v178
02920 start = end
02921 end += 24
02922 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02923 _v179 = _v177.orientation
02924 _x = _v179
02925 start = end
02926 end += 32
02927 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02928 start = end
02929 end += 4
02930 (val2.confidence,) = _struct_f.unpack(str[start:end])
02931 start = end
02932 end += 4
02933 (length,) = _struct_I.unpack(str[start:end])
02934 start = end
02935 end += length
02936 if python3:
02937 val2.detector_name = str[start:end].decode('utf-8')
02938 else:
02939 val2.detector_name = str[start:end]
02940 val1.potential_models.append(val2)
02941 _v180 = val1.cluster
02942 _v181 = _v180.header
02943 start = end
02944 end += 4
02945 (_v181.seq,) = _struct_I.unpack(str[start:end])
02946 _v182 = _v181.stamp
02947 _x = _v182
02948 start = end
02949 end += 8
02950 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02951 start = end
02952 end += 4
02953 (length,) = _struct_I.unpack(str[start:end])
02954 start = end
02955 end += length
02956 if python3:
02957 _v181.frame_id = str[start:end].decode('utf-8')
02958 else:
02959 _v181.frame_id = str[start:end]
02960 start = end
02961 end += 4
02962 (length,) = _struct_I.unpack(str[start:end])
02963 _v180.points = []
02964 for i in range(0, length):
02965 val3 = geometry_msgs.msg.Point32()
02966 _x = val3
02967 start = end
02968 end += 12
02969 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02970 _v180.points.append(val3)
02971 start = end
02972 end += 4
02973 (length,) = _struct_I.unpack(str[start:end])
02974 _v180.channels = []
02975 for i in range(0, length):
02976 val3 = sensor_msgs.msg.ChannelFloat32()
02977 start = end
02978 end += 4
02979 (length,) = _struct_I.unpack(str[start:end])
02980 start = end
02981 end += length
02982 if python3:
02983 val3.name = str[start:end].decode('utf-8')
02984 else:
02985 val3.name = str[start:end]
02986 start = end
02987 end += 4
02988 (length,) = _struct_I.unpack(str[start:end])
02989 pattern = '<%sf'%length
02990 start = end
02991 end += struct.calcsize(pattern)
02992 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
02993 _v180.channels.append(val3)
02994 _v183 = val1.region
02995 _v184 = _v183.cloud
02996 _v185 = _v184.header
02997 start = end
02998 end += 4
02999 (_v185.seq,) = _struct_I.unpack(str[start:end])
03000 _v186 = _v185.stamp
03001 _x = _v186
03002 start = end
03003 end += 8
03004 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03005 start = end
03006 end += 4
03007 (length,) = _struct_I.unpack(str[start:end])
03008 start = end
03009 end += length
03010 if python3:
03011 _v185.frame_id = str[start:end].decode('utf-8')
03012 else:
03013 _v185.frame_id = str[start:end]
03014 _x = _v184
03015 start = end
03016 end += 8
03017 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03018 start = end
03019 end += 4
03020 (length,) = _struct_I.unpack(str[start:end])
03021 _v184.fields = []
03022 for i in range(0, length):
03023 val4 = sensor_msgs.msg.PointField()
03024 start = end
03025 end += 4
03026 (length,) = _struct_I.unpack(str[start:end])
03027 start = end
03028 end += length
03029 if python3:
03030 val4.name = str[start:end].decode('utf-8')
03031 else:
03032 val4.name = str[start:end]
03033 _x = val4
03034 start = end
03035 end += 9
03036 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
03037 _v184.fields.append(val4)
03038 _x = _v184
03039 start = end
03040 end += 9
03041 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
03042 _v184.is_bigendian = bool(_v184.is_bigendian)
03043 start = end
03044 end += 4
03045 (length,) = _struct_I.unpack(str[start:end])
03046 start = end
03047 end += length
03048 if python3:
03049 _v184.data = str[start:end].decode('utf-8')
03050 else:
03051 _v184.data = str[start:end]
03052 start = end
03053 end += 1
03054 (_v184.is_dense,) = _struct_B.unpack(str[start:end])
03055 _v184.is_dense = bool(_v184.is_dense)
03056 start = end
03057 end += 4
03058 (length,) = _struct_I.unpack(str[start:end])
03059 pattern = '<%si'%length
03060 start = end
03061 end += struct.calcsize(pattern)
03062 _v183.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03063 _v187 = _v183.image
03064 _v188 = _v187.header
03065 start = end
03066 end += 4
03067 (_v188.seq,) = _struct_I.unpack(str[start:end])
03068 _v189 = _v188.stamp
03069 _x = _v189
03070 start = end
03071 end += 8
03072 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03073 start = end
03074 end += 4
03075 (length,) = _struct_I.unpack(str[start:end])
03076 start = end
03077 end += length
03078 if python3:
03079 _v188.frame_id = str[start:end].decode('utf-8')
03080 else:
03081 _v188.frame_id = str[start:end]
03082 _x = _v187
03083 start = end
03084 end += 8
03085 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03086 start = end
03087 end += 4
03088 (length,) = _struct_I.unpack(str[start:end])
03089 start = end
03090 end += length
03091 if python3:
03092 _v187.encoding = str[start:end].decode('utf-8')
03093 else:
03094 _v187.encoding = str[start:end]
03095 _x = _v187
03096 start = end
03097 end += 5
03098 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03099 start = end
03100 end += 4
03101 (length,) = _struct_I.unpack(str[start:end])
03102 start = end
03103 end += length
03104 if python3:
03105 _v187.data = str[start:end].decode('utf-8')
03106 else:
03107 _v187.data = str[start:end]
03108 _v190 = _v183.disparity_image
03109 _v191 = _v190.header
03110 start = end
03111 end += 4
03112 (_v191.seq,) = _struct_I.unpack(str[start:end])
03113 _v192 = _v191.stamp
03114 _x = _v192
03115 start = end
03116 end += 8
03117 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03118 start = end
03119 end += 4
03120 (length,) = _struct_I.unpack(str[start:end])
03121 start = end
03122 end += length
03123 if python3:
03124 _v191.frame_id = str[start:end].decode('utf-8')
03125 else:
03126 _v191.frame_id = str[start:end]
03127 _x = _v190
03128 start = end
03129 end += 8
03130 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03131 start = end
03132 end += 4
03133 (length,) = _struct_I.unpack(str[start:end])
03134 start = end
03135 end += length
03136 if python3:
03137 _v190.encoding = str[start:end].decode('utf-8')
03138 else:
03139 _v190.encoding = str[start:end]
03140 _x = _v190
03141 start = end
03142 end += 5
03143 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03144 start = end
03145 end += 4
03146 (length,) = _struct_I.unpack(str[start:end])
03147 start = end
03148 end += length
03149 if python3:
03150 _v190.data = str[start:end].decode('utf-8')
03151 else:
03152 _v190.data = str[start:end]
03153 _v193 = _v183.cam_info
03154 _v194 = _v193.header
03155 start = end
03156 end += 4
03157 (_v194.seq,) = _struct_I.unpack(str[start:end])
03158 _v195 = _v194.stamp
03159 _x = _v195
03160 start = end
03161 end += 8
03162 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03163 start = end
03164 end += 4
03165 (length,) = _struct_I.unpack(str[start:end])
03166 start = end
03167 end += length
03168 if python3:
03169 _v194.frame_id = str[start:end].decode('utf-8')
03170 else:
03171 _v194.frame_id = str[start:end]
03172 _x = _v193
03173 start = end
03174 end += 8
03175 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03176 start = end
03177 end += 4
03178 (length,) = _struct_I.unpack(str[start:end])
03179 start = end
03180 end += length
03181 if python3:
03182 _v193.distortion_model = str[start:end].decode('utf-8')
03183 else:
03184 _v193.distortion_model = str[start:end]
03185 start = end
03186 end += 4
03187 (length,) = _struct_I.unpack(str[start:end])
03188 pattern = '<%sd'%length
03189 start = end
03190 end += struct.calcsize(pattern)
03191 _v193.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03192 start = end
03193 end += 72
03194 _v193.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
03195 start = end
03196 end += 72
03197 _v193.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
03198 start = end
03199 end += 96
03200 _v193.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
03201 _x = _v193
03202 start = end
03203 end += 8
03204 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
03205 _v196 = _v193.roi
03206 _x = _v196
03207 start = end
03208 end += 17
03209 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
03210 _v196.do_rectify = bool(_v196.do_rectify)
03211 _v197 = _v183.roi_box_pose
03212 _v198 = _v197.header
03213 start = end
03214 end += 4
03215 (_v198.seq,) = _struct_I.unpack(str[start:end])
03216 _v199 = _v198.stamp
03217 _x = _v199
03218 start = end
03219 end += 8
03220 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03221 start = end
03222 end += 4
03223 (length,) = _struct_I.unpack(str[start:end])
03224 start = end
03225 end += length
03226 if python3:
03227 _v198.frame_id = str[start:end].decode('utf-8')
03228 else:
03229 _v198.frame_id = str[start:end]
03230 _v200 = _v197.pose
03231 _v201 = _v200.position
03232 _x = _v201
03233 start = end
03234 end += 24
03235 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03236 _v202 = _v200.orientation
03237 _x = _v202
03238 start = end
03239 end += 32
03240 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03241 _v203 = _v183.roi_box_dims
03242 _x = _v203
03243 start = end
03244 end += 24
03245 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03246 start = end
03247 end += 4
03248 (length,) = _struct_I.unpack(str[start:end])
03249 start = end
03250 end += length
03251 if python3:
03252 val1.collision_name = str[start:end].decode('utf-8')
03253 else:
03254 val1.collision_name = str[start:end]
03255 self.grasp.moved_obstacles.append(val1)
03256 _x = self
03257 start = end
03258 end += 20
03259 (_x.desired_retreat_distance, _x.min_retreat_distance, _x.approach.direction.header.seq, _x.approach.direction.header.stamp.secs, _x.approach.direction.header.stamp.nsecs,) = _struct_2f3I.unpack(str[start:end])
03260 start = end
03261 end += 4
03262 (length,) = _struct_I.unpack(str[start:end])
03263 start = end
03264 end += length
03265 if python3:
03266 self.approach.direction.header.frame_id = str[start:end].decode('utf-8')
03267 else:
03268 self.approach.direction.header.frame_id = str[start:end]
03269 _x = self
03270 start = end
03271 end += 32
03272 (_x.approach.direction.vector.x, _x.approach.direction.vector.y, _x.approach.direction.vector.z, _x.approach.desired_distance, _x.approach.min_distance,) = _struct_3d2f.unpack(str[start:end])
03273 start = end
03274 end += 4
03275 (length,) = _struct_I.unpack(str[start:end])
03276 start = end
03277 end += length
03278 if python3:
03279 self.collision_object_name = str[start:end].decode('utf-8')
03280 else:
03281 self.collision_object_name = str[start:end]
03282 start = end
03283 end += 4
03284 (length,) = _struct_I.unpack(str[start:end])
03285 start = end
03286 end += length
03287 if python3:
03288 self.collision_support_surface_name = str[start:end].decode('utf-8')
03289 else:
03290 self.collision_support_surface_name = str[start:end]
03291 _x = self
03292 start = end
03293 end += 11
03294 (_x.allow_gripper_support_collision, _x.use_reactive_place, _x.place_padding, _x.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end])
03295 self.allow_gripper_support_collision = bool(self.allow_gripper_support_collision)
03296 self.use_reactive_place = bool(self.use_reactive_place)
03297 self.only_perform_feasibility_test = bool(self.only_perform_feasibility_test)
03298 start = end
03299 end += 4
03300 (length,) = _struct_I.unpack(str[start:end])
03301 self.path_constraints.joint_constraints = []
03302 for i in range(0, length):
03303 val1 = arm_navigation_msgs.msg.JointConstraint()
03304 start = end
03305 end += 4
03306 (length,) = _struct_I.unpack(str[start:end])
03307 start = end
03308 end += length
03309 if python3:
03310 val1.joint_name = str[start:end].decode('utf-8')
03311 else:
03312 val1.joint_name = str[start:end]
03313 _x = val1
03314 start = end
03315 end += 32
03316 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
03317 self.path_constraints.joint_constraints.append(val1)
03318 start = end
03319 end += 4
03320 (length,) = _struct_I.unpack(str[start:end])
03321 self.path_constraints.position_constraints = []
03322 for i in range(0, length):
03323 val1 = arm_navigation_msgs.msg.PositionConstraint()
03324 _v204 = val1.header
03325 start = end
03326 end += 4
03327 (_v204.seq,) = _struct_I.unpack(str[start:end])
03328 _v205 = _v204.stamp
03329 _x = _v205
03330 start = end
03331 end += 8
03332 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03333 start = end
03334 end += 4
03335 (length,) = _struct_I.unpack(str[start:end])
03336 start = end
03337 end += length
03338 if python3:
03339 _v204.frame_id = str[start:end].decode('utf-8')
03340 else:
03341 _v204.frame_id = str[start:end]
03342 start = end
03343 end += 4
03344 (length,) = _struct_I.unpack(str[start:end])
03345 start = end
03346 end += length
03347 if python3:
03348 val1.link_name = str[start:end].decode('utf-8')
03349 else:
03350 val1.link_name = str[start:end]
03351 _v206 = val1.target_point_offset
03352 _x = _v206
03353 start = end
03354 end += 24
03355 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03356 _v207 = val1.position
03357 _x = _v207
03358 start = end
03359 end += 24
03360 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03361 _v208 = val1.constraint_region_shape
03362 start = end
03363 end += 1
03364 (_v208.type,) = _struct_b.unpack(str[start:end])
03365 start = end
03366 end += 4
03367 (length,) = _struct_I.unpack(str[start:end])
03368 pattern = '<%sd'%length
03369 start = end
03370 end += struct.calcsize(pattern)
03371 _v208.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03372 start = end
03373 end += 4
03374 (length,) = _struct_I.unpack(str[start:end])
03375 pattern = '<%si'%length
03376 start = end
03377 end += struct.calcsize(pattern)
03378 _v208.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03379 start = end
03380 end += 4
03381 (length,) = _struct_I.unpack(str[start:end])
03382 _v208.vertices = []
03383 for i in range(0, length):
03384 val3 = geometry_msgs.msg.Point()
03385 _x = val3
03386 start = end
03387 end += 24
03388 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03389 _v208.vertices.append(val3)
03390 _v209 = val1.constraint_region_orientation
03391 _x = _v209
03392 start = end
03393 end += 32
03394 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03395 start = end
03396 end += 8
03397 (val1.weight,) = _struct_d.unpack(str[start:end])
03398 self.path_constraints.position_constraints.append(val1)
03399 start = end
03400 end += 4
03401 (length,) = _struct_I.unpack(str[start:end])
03402 self.path_constraints.orientation_constraints = []
03403 for i in range(0, length):
03404 val1 = arm_navigation_msgs.msg.OrientationConstraint()
03405 _v210 = val1.header
03406 start = end
03407 end += 4
03408 (_v210.seq,) = _struct_I.unpack(str[start:end])
03409 _v211 = _v210.stamp
03410 _x = _v211
03411 start = end
03412 end += 8
03413 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03414 start = end
03415 end += 4
03416 (length,) = _struct_I.unpack(str[start:end])
03417 start = end
03418 end += length
03419 if python3:
03420 _v210.frame_id = str[start:end].decode('utf-8')
03421 else:
03422 _v210.frame_id = str[start:end]
03423 start = end
03424 end += 4
03425 (length,) = _struct_I.unpack(str[start:end])
03426 start = end
03427 end += length
03428 if python3:
03429 val1.link_name = str[start:end].decode('utf-8')
03430 else:
03431 val1.link_name = str[start:end]
03432 start = end
03433 end += 4
03434 (val1.type,) = _struct_i.unpack(str[start:end])
03435 _v212 = val1.orientation
03436 _x = _v212
03437 start = end
03438 end += 32
03439 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03440 _x = val1
03441 start = end
03442 end += 32
03443 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
03444 self.path_constraints.orientation_constraints.append(val1)
03445 start = end
03446 end += 4
03447 (length,) = _struct_I.unpack(str[start:end])
03448 self.path_constraints.visibility_constraints = []
03449 for i in range(0, length):
03450 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
03451 _v213 = val1.header
03452 start = end
03453 end += 4
03454 (_v213.seq,) = _struct_I.unpack(str[start:end])
03455 _v214 = _v213.stamp
03456 _x = _v214
03457 start = end
03458 end += 8
03459 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03460 start = end
03461 end += 4
03462 (length,) = _struct_I.unpack(str[start:end])
03463 start = end
03464 end += length
03465 if python3:
03466 _v213.frame_id = str[start:end].decode('utf-8')
03467 else:
03468 _v213.frame_id = str[start:end]
03469 _v215 = val1.target
03470 _v216 = _v215.header
03471 start = end
03472 end += 4
03473 (_v216.seq,) = _struct_I.unpack(str[start:end])
03474 _v217 = _v216.stamp
03475 _x = _v217
03476 start = end
03477 end += 8
03478 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03479 start = end
03480 end += 4
03481 (length,) = _struct_I.unpack(str[start:end])
03482 start = end
03483 end += length
03484 if python3:
03485 _v216.frame_id = str[start:end].decode('utf-8')
03486 else:
03487 _v216.frame_id = str[start:end]
03488 _v218 = _v215.point
03489 _x = _v218
03490 start = end
03491 end += 24
03492 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03493 _v219 = val1.sensor_pose
03494 _v220 = _v219.header
03495 start = end
03496 end += 4
03497 (_v220.seq,) = _struct_I.unpack(str[start:end])
03498 _v221 = _v220.stamp
03499 _x = _v221
03500 start = end
03501 end += 8
03502 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03503 start = end
03504 end += 4
03505 (length,) = _struct_I.unpack(str[start:end])
03506 start = end
03507 end += length
03508 if python3:
03509 _v220.frame_id = str[start:end].decode('utf-8')
03510 else:
03511 _v220.frame_id = str[start:end]
03512 _v222 = _v219.pose
03513 _v223 = _v222.position
03514 _x = _v223
03515 start = end
03516 end += 24
03517 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03518 _v224 = _v222.orientation
03519 _x = _v224
03520 start = end
03521 end += 32
03522 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03523 start = end
03524 end += 8
03525 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
03526 self.path_constraints.visibility_constraints.append(val1)
03527 start = end
03528 end += 4
03529 (length,) = _struct_I.unpack(str[start:end])
03530 self.additional_collision_operations.collision_operations = []
03531 for i in range(0, length):
03532 val1 = arm_navigation_msgs.msg.CollisionOperation()
03533 start = end
03534 end += 4
03535 (length,) = _struct_I.unpack(str[start:end])
03536 start = end
03537 end += length
03538 if python3:
03539 val1.object1 = str[start:end].decode('utf-8')
03540 else:
03541 val1.object1 = str[start:end]
03542 start = end
03543 end += 4
03544 (length,) = _struct_I.unpack(str[start:end])
03545 start = end
03546 end += length
03547 if python3:
03548 val1.object2 = str[start:end].decode('utf-8')
03549 else:
03550 val1.object2 = str[start:end]
03551 _x = val1
03552 start = end
03553 end += 12
03554 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
03555 self.additional_collision_operations.collision_operations.append(val1)
03556 start = end
03557 end += 4
03558 (length,) = _struct_I.unpack(str[start:end])
03559 self.additional_link_padding = []
03560 for i in range(0, length):
03561 val1 = arm_navigation_msgs.msg.LinkPadding()
03562 start = end
03563 end += 4
03564 (length,) = _struct_I.unpack(str[start:end])
03565 start = end
03566 end += length
03567 if python3:
03568 val1.link_name = str[start:end].decode('utf-8')
03569 else:
03570 val1.link_name = str[start:end]
03571 start = end
03572 end += 8
03573 (val1.padding,) = _struct_d.unpack(str[start:end])
03574 self.additional_link_padding.append(val1)
03575 return self
03576 except struct.error as e:
03577 raise genpy.DeserializationError(e) #most likely buffer underfill
03578
03579 _struct_I = genpy.struct_I
03580 _struct_IBI = struct.Struct("<IBI")
03581 _struct_12d = struct.Struct("<12d")
03582 _struct_BI = struct.Struct("<BI")
03583 _struct_2BdB = struct.Struct("<2BdB")
03584 _struct_3I = struct.Struct("<3I")
03585 _struct_B2I = struct.Struct("<B2I")
03586 _struct_8dB2f = struct.Struct("<8dB2f")
03587 _struct_2f3I = struct.Struct("<2f3I")
03588 _struct_3f = struct.Struct("<3f")
03589 _struct_3d = struct.Struct("<3d")
03590 _struct_B = struct.Struct("<B")
03591 _struct_di = struct.Struct("<di")
03592 _struct_9d = struct.Struct("<9d")
03593 _struct_2I = struct.Struct("<2I")
03594 _struct_b = struct.Struct("<b")
03595 _struct_d = struct.Struct("<d")
03596 _struct_f = struct.Struct("<f")
03597 _struct_i = struct.Struct("<i")
03598 _struct_3d2f = struct.Struct("<3d2f")
03599 _struct_4d = struct.Struct("<4d")
03600 _struct_4IB = struct.Struct("<4IB")