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