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