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