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