00001 """autogenerated by genmsg_py from ReactiveGraspRequest.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import trajectory_msgs.msg
00006 import object_manipulation_msgs.msg
00007 import roslib.rostime
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 ReactiveGraspRequest(roslib.message.Message):
00014 _md5sum = "cfc8e5ca2e7695fd97bbd17c05dd9ce7"
00015 _type = "object_manipulation_msgs/ReactiveGraspRequest"
00016 _has_header = False
00017 _full_text = """
00018 ReactiveGraspGoal goal
00019
00020
00021 ================================================================================
00022 MSG: object_manipulation_msgs/ReactiveGraspGoal
00023 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00024 # an action for reactive grasping
00025 # a reactive grasp starts from the current pose of the gripper and ends
00026 # at a desired grasp pose, presumably using the touch sensors along the way
00027
00028 # the name of the arm being used
00029 string arm_name
00030
00031 # the object to be grasped
00032 GraspableObject target
00033
00034 # the desired grasp pose for the hand
00035 geometry_msgs/PoseStamped final_grasp_pose
00036
00037 # the joint trajectory to use for the approach (if available)
00038 # this trajectory is expected to start at the current pose of the gripper
00039 # and end at the desired grasp pose
00040 trajectory_msgs/JointTrajectory trajectory
00041
00042 # the name of the support surface in the collision environment, if any
00043 string collision_support_surface_name
00044
00045 # The internal posture of the hand for the pre-grasp
00046 # only positions are used
00047 sensor_msgs/JointState pre_grasp_posture
00048
00049 # The internal posture of the hand for the grasp
00050 # positions and efforts are used
00051 sensor_msgs/JointState grasp_posture
00052
00053 # The max contact force to use while grasping (<=0 to disable)
00054 float32 max_contact_force
00055
00056
00057 ================================================================================
00058 MSG: object_manipulation_msgs/GraspableObject
00059 # an object that the object_manipulator can work on
00060
00061 # a graspable object can be represented in multiple ways. This message
00062 # can contain all of them. Which one is actually used is up to the receiver
00063 # of this message. When adding new representations, one must be careful that
00064 # they have reasonable lightweight defaults indicating that that particular
00065 # representation is not available.
00066
00067 # the tf frame to be used as a reference frame when combining information from
00068 # the different representations below
00069 string reference_frame_id
00070
00071 # potential recognition results from a database of models
00072 # all poses are relative to the object reference pose
00073 household_objects_database_msgs/DatabaseModelPose[] potential_models
00074
00075 # the point cloud itself
00076 sensor_msgs/PointCloud cluster
00077
00078 # a region of a PointCloud2 of interest
00079 object_manipulation_msgs/SceneRegion region
00080
00081 # the name that this object has in the collision environment
00082 string collision_name
00083 ================================================================================
00084 MSG: household_objects_database_msgs/DatabaseModelPose
00085 # Informs that a specific model from the Model Database has been
00086 # identified at a certain location
00087
00088 # the database id of the model
00089 int32 model_id
00090
00091 # the pose that it can be found in
00092 geometry_msgs/PoseStamped pose
00093
00094 # a measure of the confidence level in this detection result
00095 float32 confidence
00096
00097 # the name of the object detector that generated this detection result
00098 string detector_name
00099
00100 ================================================================================
00101 MSG: geometry_msgs/PoseStamped
00102 # A Pose with reference coordinate frame and timestamp
00103 Header header
00104 Pose pose
00105
00106 ================================================================================
00107 MSG: std_msgs/Header
00108 # Standard metadata for higher-level stamped data types.
00109 # This is generally used to communicate timestamped data
00110 # in a particular coordinate frame.
00111 #
00112 # sequence ID: consecutively increasing ID
00113 uint32 seq
00114 #Two-integer timestamp that is expressed as:
00115 # * stamp.secs: seconds (stamp_secs) since epoch
00116 # * stamp.nsecs: nanoseconds since stamp_secs
00117 # time-handling sugar is provided by the client library
00118 time stamp
00119 #Frame this data is associated with
00120 # 0: no frame
00121 # 1: global frame
00122 string frame_id
00123
00124 ================================================================================
00125 MSG: geometry_msgs/Pose
00126 # A representation of pose in free space, composed of postion and orientation.
00127 Point position
00128 Quaternion orientation
00129
00130 ================================================================================
00131 MSG: geometry_msgs/Point
00132 # This contains the position of a point in free space
00133 float64 x
00134 float64 y
00135 float64 z
00136
00137 ================================================================================
00138 MSG: geometry_msgs/Quaternion
00139 # This represents an orientation in free space in quaternion form.
00140
00141 float64 x
00142 float64 y
00143 float64 z
00144 float64 w
00145
00146 ================================================================================
00147 MSG: sensor_msgs/PointCloud
00148 # This message holds a collection of 3d points, plus optional additional
00149 # information about each point.
00150
00151 # Time of sensor data acquisition, coordinate frame ID.
00152 Header header
00153
00154 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00155 # in the frame given in the header.
00156 geometry_msgs/Point32[] points
00157
00158 # Each channel should have the same number of elements as points array,
00159 # and the data in each channel should correspond 1:1 with each point.
00160 # Channel names in common practice are listed in ChannelFloat32.msg.
00161 ChannelFloat32[] channels
00162
00163 ================================================================================
00164 MSG: geometry_msgs/Point32
00165 # This contains the position of a point in free space(with 32 bits of precision).
00166 # It is recommeded to use Point wherever possible instead of Point32.
00167 #
00168 # This recommendation is to promote interoperability.
00169 #
00170 # This message is designed to take up less space when sending
00171 # lots of points at once, as in the case of a PointCloud.
00172
00173 float32 x
00174 float32 y
00175 float32 z
00176 ================================================================================
00177 MSG: sensor_msgs/ChannelFloat32
00178 # This message is used by the PointCloud message to hold optional data
00179 # associated with each point in the cloud. The length of the values
00180 # array should be the same as the length of the points array in the
00181 # PointCloud, and each value should be associated with the corresponding
00182 # point.
00183
00184 # Channel names in existing practice include:
00185 # "u", "v" - row and column (respectively) in the left stereo image.
00186 # This is opposite to usual conventions but remains for
00187 # historical reasons. The newer PointCloud2 message has no
00188 # such problem.
00189 # "rgb" - For point clouds produced by color stereo cameras. uint8
00190 # (R,G,B) values packed into the least significant 24 bits,
00191 # in order.
00192 # "intensity" - laser or pixel intensity.
00193 # "distance"
00194
00195 # The channel name should give semantics of the channel (e.g.
00196 # "intensity" instead of "value").
00197 string name
00198
00199 # The values array should be 1-1 with the elements of the associated
00200 # PointCloud.
00201 float32[] values
00202
00203 ================================================================================
00204 MSG: object_manipulation_msgs/SceneRegion
00205 # Point cloud
00206 sensor_msgs/PointCloud2 cloud
00207
00208 # Indices for the region of interest
00209 int32[] mask
00210
00211 # One of the corresponding 2D images, if applicable
00212 sensor_msgs/Image image
00213
00214 # The disparity image, if applicable
00215 sensor_msgs/Image disparity_image
00216
00217 # Camera info for the camera that took the image
00218 sensor_msgs/CameraInfo cam_info
00219
00220 # a 3D region of interest for grasp planning
00221 geometry_msgs/PoseStamped roi_box_pose
00222 geometry_msgs/Vector3 roi_box_dims
00223
00224 ================================================================================
00225 MSG: sensor_msgs/PointCloud2
00226 # This message holds a collection of N-dimensional points, which may
00227 # contain additional information such as normals, intensity, etc. The
00228 # point data is stored as a binary blob, its layout described by the
00229 # contents of the "fields" array.
00230
00231 # The point cloud data may be organized 2d (image-like) or 1d
00232 # (unordered). Point clouds organized as 2d images may be produced by
00233 # camera depth sensors such as stereo or time-of-flight.
00234
00235 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00236 # points).
00237 Header header
00238
00239 # 2D structure of the point cloud. If the cloud is unordered, height is
00240 # 1 and width is the length of the point cloud.
00241 uint32 height
00242 uint32 width
00243
00244 # Describes the channels and their layout in the binary data blob.
00245 PointField[] fields
00246
00247 bool is_bigendian # Is this data bigendian?
00248 uint32 point_step # Length of a point in bytes
00249 uint32 row_step # Length of a row in bytes
00250 uint8[] data # Actual point data, size is (row_step*height)
00251
00252 bool is_dense # True if there are no invalid points
00253
00254 ================================================================================
00255 MSG: sensor_msgs/PointField
00256 # This message holds the description of one point entry in the
00257 # PointCloud2 message format.
00258 uint8 INT8 = 1
00259 uint8 UINT8 = 2
00260 uint8 INT16 = 3
00261 uint8 UINT16 = 4
00262 uint8 INT32 = 5
00263 uint8 UINT32 = 6
00264 uint8 FLOAT32 = 7
00265 uint8 FLOAT64 = 8
00266
00267 string name # Name of field
00268 uint32 offset # Offset from start of point struct
00269 uint8 datatype # Datatype enumeration, see above
00270 uint32 count # How many elements in the field
00271
00272 ================================================================================
00273 MSG: sensor_msgs/Image
00274 # This message contains an uncompressed image
00275 # (0, 0) is at top-left corner of image
00276 #
00277
00278 Header header # Header timestamp should be acquisition time of image
00279 # Header frame_id should be optical frame of camera
00280 # origin of frame should be optical center of cameara
00281 # +x should point to the right in the image
00282 # +y should point down in the image
00283 # +z should point into to plane of the image
00284 # If the frame_id here and the frame_id of the CameraInfo
00285 # message associated with the image conflict
00286 # the behavior is undefined
00287
00288 uint32 height # image height, that is, number of rows
00289 uint32 width # image width, that is, number of columns
00290
00291 # The legal values for encoding are in file src/image_encodings.cpp
00292 # If you want to standardize a new string format, join
00293 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00294
00295 string encoding # Encoding of pixels -- channel meaning, ordering, size
00296 # taken from the list of strings in src/image_encodings.cpp
00297
00298 uint8 is_bigendian # is this data bigendian?
00299 uint32 step # Full row length in bytes
00300 uint8[] data # actual matrix data, size is (step * rows)
00301
00302 ================================================================================
00303 MSG: sensor_msgs/CameraInfo
00304 # This message defines meta information for a camera. It should be in a
00305 # camera namespace on topic "camera_info" and accompanied by up to five
00306 # image topics named:
00307 #
00308 # image_raw - raw data from the camera driver, possibly Bayer encoded
00309 # image - monochrome, distorted
00310 # image_color - color, distorted
00311 # image_rect - monochrome, rectified
00312 # image_rect_color - color, rectified
00313 #
00314 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00315 # for producing the four processed image topics from image_raw and
00316 # camera_info. The meaning of the camera parameters are described in
00317 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00318 #
00319 # The image_geometry package provides a user-friendly interface to
00320 # common operations using this meta information. If you want to, e.g.,
00321 # project a 3d point into image coordinates, we strongly recommend
00322 # using image_geometry.
00323 #
00324 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00325 # zeroed out. In particular, clients may assume that K[0] == 0.0
00326 # indicates an uncalibrated camera.
00327
00328 #######################################################################
00329 # Image acquisition info #
00330 #######################################################################
00331
00332 # Time of image acquisition, camera coordinate frame ID
00333 Header header # Header timestamp should be acquisition time of image
00334 # Header frame_id should be optical frame of camera
00335 # origin of frame should be optical center of camera
00336 # +x should point to the right in the image
00337 # +y should point down in the image
00338 # +z should point into the plane of the image
00339
00340
00341 #######################################################################
00342 # Calibration Parameters #
00343 #######################################################################
00344 # These are fixed during camera calibration. Their values will be the #
00345 # same in all messages until the camera is recalibrated. Note that #
00346 # self-calibrating systems may "recalibrate" frequently. #
00347 # #
00348 # The internal parameters can be used to warp a raw (distorted) image #
00349 # to: #
00350 # 1. An undistorted image (requires D and K) #
00351 # 2. A rectified image (requires D, K, R) #
00352 # The projection matrix P projects 3D points into the rectified image.#
00353 #######################################################################
00354
00355 # The image dimensions with which the camera was calibrated. Normally
00356 # this will be the full camera resolution in pixels.
00357 uint32 height
00358 uint32 width
00359
00360 # The distortion model used. Supported models are listed in
00361 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00362 # simple model of radial and tangential distortion - is sufficent.
00363 string distortion_model
00364
00365 # The distortion parameters, size depending on the distortion model.
00366 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00367 float64[] D
00368
00369 # Intrinsic camera matrix for the raw (distorted) images.
00370 # [fx 0 cx]
00371 # K = [ 0 fy cy]
00372 # [ 0 0 1]
00373 # Projects 3D points in the camera coordinate frame to 2D pixel
00374 # coordinates using the focal lengths (fx, fy) and principal point
00375 # (cx, cy).
00376 float64[9] K # 3x3 row-major matrix
00377
00378 # Rectification matrix (stereo cameras only)
00379 # A rotation matrix aligning the camera coordinate system to the ideal
00380 # stereo image plane so that epipolar lines in both stereo images are
00381 # parallel.
00382 float64[9] R # 3x3 row-major matrix
00383
00384 # Projection/camera matrix
00385 # [fx' 0 cx' Tx]
00386 # P = [ 0 fy' cy' Ty]
00387 # [ 0 0 1 0]
00388 # By convention, this matrix specifies the intrinsic (camera) matrix
00389 # of the processed (rectified) image. That is, the left 3x3 portion
00390 # is the normal camera intrinsic matrix for the rectified image.
00391 # It projects 3D points in the camera coordinate frame to 2D pixel
00392 # coordinates using the focal lengths (fx', fy') and principal point
00393 # (cx', cy') - these may differ from the values in K.
00394 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00395 # also have R = the identity and P[1:3,1:3] = K.
00396 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00397 # position of the optical center of the second camera in the first
00398 # camera's frame. We assume Tz = 0 so both cameras are in the same
00399 # stereo image plane. The first camera always has Tx = Ty = 0. For
00400 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00401 # Tx = -fx' * B, where B is the baseline between the cameras.
00402 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00403 # the rectified image is given by:
00404 # [u v w]' = P * [X Y Z 1]'
00405 # x = u / w
00406 # y = v / w
00407 # This holds for both images of a stereo pair.
00408 float64[12] P # 3x4 row-major matrix
00409
00410
00411 #######################################################################
00412 # Operational Parameters #
00413 #######################################################################
00414 # These define the image region actually captured by the camera #
00415 # driver. Although they affect the geometry of the output image, they #
00416 # may be changed freely without recalibrating the camera. #
00417 #######################################################################
00418
00419 # Binning refers here to any camera setting which combines rectangular
00420 # neighborhoods of pixels into larger "super-pixels." It reduces the
00421 # resolution of the output image to
00422 # (width / binning_x) x (height / binning_y).
00423 # The default values binning_x = binning_y = 0 is considered the same
00424 # as binning_x = binning_y = 1 (no subsampling).
00425 uint32 binning_x
00426 uint32 binning_y
00427
00428 # Region of interest (subwindow of full camera resolution), given in
00429 # full resolution (unbinned) image coordinates. A particular ROI
00430 # always denotes the same window of pixels on the camera sensor,
00431 # regardless of binning settings.
00432 # The default setting of roi (all values 0) is considered the same as
00433 # full resolution (roi.width = width, roi.height = height).
00434 RegionOfInterest roi
00435
00436 ================================================================================
00437 MSG: sensor_msgs/RegionOfInterest
00438 # This message is used to specify a region of interest within an image.
00439 #
00440 # When used to specify the ROI setting of the camera when the image was
00441 # taken, the height and width fields should either match the height and
00442 # width fields for the associated image; or height = width = 0
00443 # indicates that the full resolution image was captured.
00444
00445 uint32 x_offset # Leftmost pixel of the ROI
00446 # (0 if the ROI includes the left edge of the image)
00447 uint32 y_offset # Topmost pixel of the ROI
00448 # (0 if the ROI includes the top edge of the image)
00449 uint32 height # Height of ROI
00450 uint32 width # Width of ROI
00451
00452 # True if a distinct rectified ROI should be calculated from the "raw"
00453 # ROI in this message. Typically this should be False if the full image
00454 # is captured (ROI not used), and True if a subwindow is captured (ROI
00455 # used).
00456 bool do_rectify
00457
00458 ================================================================================
00459 MSG: geometry_msgs/Vector3
00460 # This represents a vector in free space.
00461
00462 float64 x
00463 float64 y
00464 float64 z
00465 ================================================================================
00466 MSG: trajectory_msgs/JointTrajectory
00467 Header header
00468 string[] joint_names
00469 JointTrajectoryPoint[] points
00470 ================================================================================
00471 MSG: trajectory_msgs/JointTrajectoryPoint
00472 float64[] positions
00473 float64[] velocities
00474 float64[] accelerations
00475 duration time_from_start
00476 ================================================================================
00477 MSG: sensor_msgs/JointState
00478 # This is a message that holds data to describe the state of a set of torque controlled joints.
00479 #
00480 # The state of each joint (revolute or prismatic) is defined by:
00481 # * the position of the joint (rad or m),
00482 # * the velocity of the joint (rad/s or m/s) and
00483 # * the effort that is applied in the joint (Nm or N).
00484 #
00485 # Each joint is uniquely identified by its name
00486 # The header specifies the time at which the joint states were recorded. All the joint states
00487 # in one message have to be recorded at the same time.
00488 #
00489 # This message consists of a multiple arrays, one for each part of the joint state.
00490 # The goal is to make each of the fields optional. When e.g. your joints have no
00491 # effort associated with them, you can leave the effort array empty.
00492 #
00493 # All arrays in this message should have the same size, or be empty.
00494 # This is the only way to uniquely associate the joint name with the correct
00495 # states.
00496
00497
00498 Header header
00499
00500 string[] name
00501 float64[] position
00502 float64[] velocity
00503 float64[] effort
00504
00505 """
00506 __slots__ = ['goal']
00507 _slot_types = ['object_manipulation_msgs/ReactiveGraspGoal']
00508
00509 def __init__(self, *args, **kwds):
00510 """
00511 Constructor. Any message fields that are implicitly/explicitly
00512 set to None will be assigned a default value. The recommend
00513 use is keyword arguments as this is more robust to future message
00514 changes. You cannot mix in-order arguments and keyword arguments.
00515
00516 The available fields are:
00517 goal
00518
00519 @param args: complete set of field values, in .msg order
00520 @param kwds: use keyword arguments corresponding to message field names
00521 to set specific fields.
00522 """
00523 if args or kwds:
00524 super(ReactiveGraspRequest, self).__init__(*args, **kwds)
00525 #message fields cannot be None, assign default values for those that are
00526 if self.goal is None:
00527 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
00528 else:
00529 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
00530
00531 def _get_types(self):
00532 """
00533 internal API method
00534 """
00535 return self._slot_types
00536
00537 def serialize(self, buff):
00538 """
00539 serialize message into buffer
00540 @param buff: buffer
00541 @type buff: StringIO
00542 """
00543 try:
00544 _x = self.goal.arm_name
00545 length = len(_x)
00546 buff.write(struct.pack('<I%ss'%length, length, _x))
00547 _x = self.goal.target.reference_frame_id
00548 length = len(_x)
00549 buff.write(struct.pack('<I%ss'%length, length, _x))
00550 length = len(self.goal.target.potential_models)
00551 buff.write(_struct_I.pack(length))
00552 for val1 in self.goal.target.potential_models:
00553 buff.write(_struct_i.pack(val1.model_id))
00554 _v1 = val1.pose
00555 _v2 = _v1.header
00556 buff.write(_struct_I.pack(_v2.seq))
00557 _v3 = _v2.stamp
00558 _x = _v3
00559 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00560 _x = _v2.frame_id
00561 length = len(_x)
00562 buff.write(struct.pack('<I%ss'%length, length, _x))
00563 _v4 = _v1.pose
00564 _v5 = _v4.position
00565 _x = _v5
00566 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00567 _v6 = _v4.orientation
00568 _x = _v6
00569 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00570 buff.write(_struct_f.pack(val1.confidence))
00571 _x = val1.detector_name
00572 length = len(_x)
00573 buff.write(struct.pack('<I%ss'%length, length, _x))
00574 _x = self
00575 buff.write(_struct_3I.pack(_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs))
00576 _x = self.goal.target.cluster.header.frame_id
00577 length = len(_x)
00578 buff.write(struct.pack('<I%ss'%length, length, _x))
00579 length = len(self.goal.target.cluster.points)
00580 buff.write(_struct_I.pack(length))
00581 for val1 in self.goal.target.cluster.points:
00582 _x = val1
00583 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00584 length = len(self.goal.target.cluster.channels)
00585 buff.write(_struct_I.pack(length))
00586 for val1 in self.goal.target.cluster.channels:
00587 _x = val1.name
00588 length = len(_x)
00589 buff.write(struct.pack('<I%ss'%length, length, _x))
00590 length = len(val1.values)
00591 buff.write(_struct_I.pack(length))
00592 pattern = '<%sf'%length
00593 buff.write(struct.pack(pattern, *val1.values))
00594 _x = self
00595 buff.write(_struct_3I.pack(_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs))
00596 _x = self.goal.target.region.cloud.header.frame_id
00597 length = len(_x)
00598 buff.write(struct.pack('<I%ss'%length, length, _x))
00599 _x = self
00600 buff.write(_struct_2I.pack(_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width))
00601 length = len(self.goal.target.region.cloud.fields)
00602 buff.write(_struct_I.pack(length))
00603 for val1 in self.goal.target.region.cloud.fields:
00604 _x = val1.name
00605 length = len(_x)
00606 buff.write(struct.pack('<I%ss'%length, length, _x))
00607 _x = val1
00608 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00609 _x = self
00610 buff.write(_struct_B2I.pack(_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step))
00611 _x = self.goal.target.region.cloud.data
00612 length = len(_x)
00613 # - if encoded as a list instead, serialize as bytes instead of string
00614 if type(_x) in [list, tuple]:
00615 buff.write(struct.pack('<I%sB'%length, length, *_x))
00616 else:
00617 buff.write(struct.pack('<I%ss'%length, length, _x))
00618 buff.write(_struct_B.pack(self.goal.target.region.cloud.is_dense))
00619 length = len(self.goal.target.region.mask)
00620 buff.write(_struct_I.pack(length))
00621 pattern = '<%si'%length
00622 buff.write(struct.pack(pattern, *self.goal.target.region.mask))
00623 _x = self
00624 buff.write(_struct_3I.pack(_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs))
00625 _x = self.goal.target.region.image.header.frame_id
00626 length = len(_x)
00627 buff.write(struct.pack('<I%ss'%length, length, _x))
00628 _x = self
00629 buff.write(_struct_2I.pack(_x.goal.target.region.image.height, _x.goal.target.region.image.width))
00630 _x = self.goal.target.region.image.encoding
00631 length = len(_x)
00632 buff.write(struct.pack('<I%ss'%length, length, _x))
00633 _x = self
00634 buff.write(_struct_BI.pack(_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step))
00635 _x = self.goal.target.region.image.data
00636 length = len(_x)
00637 # - if encoded as a list instead, serialize as bytes instead of string
00638 if type(_x) in [list, tuple]:
00639 buff.write(struct.pack('<I%sB'%length, length, *_x))
00640 else:
00641 buff.write(struct.pack('<I%ss'%length, length, _x))
00642 _x = self
00643 buff.write(_struct_3I.pack(_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs))
00644 _x = self.goal.target.region.disparity_image.header.frame_id
00645 length = len(_x)
00646 buff.write(struct.pack('<I%ss'%length, length, _x))
00647 _x = self
00648 buff.write(_struct_2I.pack(_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width))
00649 _x = self.goal.target.region.disparity_image.encoding
00650 length = len(_x)
00651 buff.write(struct.pack('<I%ss'%length, length, _x))
00652 _x = self
00653 buff.write(_struct_BI.pack(_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step))
00654 _x = self.goal.target.region.disparity_image.data
00655 length = len(_x)
00656 # - if encoded as a list instead, serialize as bytes instead of string
00657 if type(_x) in [list, tuple]:
00658 buff.write(struct.pack('<I%sB'%length, length, *_x))
00659 else:
00660 buff.write(struct.pack('<I%ss'%length, length, _x))
00661 _x = self
00662 buff.write(_struct_3I.pack(_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs))
00663 _x = self.goal.target.region.cam_info.header.frame_id
00664 length = len(_x)
00665 buff.write(struct.pack('<I%ss'%length, length, _x))
00666 _x = self
00667 buff.write(_struct_2I.pack(_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width))
00668 _x = self.goal.target.region.cam_info.distortion_model
00669 length = len(_x)
00670 buff.write(struct.pack('<I%ss'%length, length, _x))
00671 length = len(self.goal.target.region.cam_info.D)
00672 buff.write(_struct_I.pack(length))
00673 pattern = '<%sd'%length
00674 buff.write(struct.pack(pattern, *self.goal.target.region.cam_info.D))
00675 buff.write(_struct_9d.pack(*self.goal.target.region.cam_info.K))
00676 buff.write(_struct_9d.pack(*self.goal.target.region.cam_info.R))
00677 buff.write(_struct_12d.pack(*self.goal.target.region.cam_info.P))
00678 _x = self
00679 buff.write(_struct_6IB3I.pack(_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs))
00680 _x = self.goal.target.region.roi_box_pose.header.frame_id
00681 length = len(_x)
00682 buff.write(struct.pack('<I%ss'%length, length, _x))
00683 _x = self
00684 buff.write(_struct_10d.pack(_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z))
00685 _x = self.goal.target.collision_name
00686 length = len(_x)
00687 buff.write(struct.pack('<I%ss'%length, length, _x))
00688 _x = self
00689 buff.write(_struct_3I.pack(_x.goal.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs))
00690 _x = self.goal.final_grasp_pose.header.frame_id
00691 length = len(_x)
00692 buff.write(struct.pack('<I%ss'%length, length, _x))
00693 _x = self
00694 buff.write(_struct_7d3I.pack(_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs))
00695 _x = self.goal.trajectory.header.frame_id
00696 length = len(_x)
00697 buff.write(struct.pack('<I%ss'%length, length, _x))
00698 length = len(self.goal.trajectory.joint_names)
00699 buff.write(_struct_I.pack(length))
00700 for val1 in self.goal.trajectory.joint_names:
00701 length = len(val1)
00702 buff.write(struct.pack('<I%ss'%length, length, val1))
00703 length = len(self.goal.trajectory.points)
00704 buff.write(_struct_I.pack(length))
00705 for val1 in self.goal.trajectory.points:
00706 length = len(val1.positions)
00707 buff.write(_struct_I.pack(length))
00708 pattern = '<%sd'%length
00709 buff.write(struct.pack(pattern, *val1.positions))
00710 length = len(val1.velocities)
00711 buff.write(_struct_I.pack(length))
00712 pattern = '<%sd'%length
00713 buff.write(struct.pack(pattern, *val1.velocities))
00714 length = len(val1.accelerations)
00715 buff.write(_struct_I.pack(length))
00716 pattern = '<%sd'%length
00717 buff.write(struct.pack(pattern, *val1.accelerations))
00718 _v7 = val1.time_from_start
00719 _x = _v7
00720 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00721 _x = self.goal.collision_support_surface_name
00722 length = len(_x)
00723 buff.write(struct.pack('<I%ss'%length, length, _x))
00724 _x = self
00725 buff.write(_struct_3I.pack(_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs))
00726 _x = self.goal.pre_grasp_posture.header.frame_id
00727 length = len(_x)
00728 buff.write(struct.pack('<I%ss'%length, length, _x))
00729 length = len(self.goal.pre_grasp_posture.name)
00730 buff.write(_struct_I.pack(length))
00731 for val1 in self.goal.pre_grasp_posture.name:
00732 length = len(val1)
00733 buff.write(struct.pack('<I%ss'%length, length, val1))
00734 length = len(self.goal.pre_grasp_posture.position)
00735 buff.write(_struct_I.pack(length))
00736 pattern = '<%sd'%length
00737 buff.write(struct.pack(pattern, *self.goal.pre_grasp_posture.position))
00738 length = len(self.goal.pre_grasp_posture.velocity)
00739 buff.write(_struct_I.pack(length))
00740 pattern = '<%sd'%length
00741 buff.write(struct.pack(pattern, *self.goal.pre_grasp_posture.velocity))
00742 length = len(self.goal.pre_grasp_posture.effort)
00743 buff.write(_struct_I.pack(length))
00744 pattern = '<%sd'%length
00745 buff.write(struct.pack(pattern, *self.goal.pre_grasp_posture.effort))
00746 _x = self
00747 buff.write(_struct_3I.pack(_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs))
00748 _x = self.goal.grasp_posture.header.frame_id
00749 length = len(_x)
00750 buff.write(struct.pack('<I%ss'%length, length, _x))
00751 length = len(self.goal.grasp_posture.name)
00752 buff.write(_struct_I.pack(length))
00753 for val1 in self.goal.grasp_posture.name:
00754 length = len(val1)
00755 buff.write(struct.pack('<I%ss'%length, length, val1))
00756 length = len(self.goal.grasp_posture.position)
00757 buff.write(_struct_I.pack(length))
00758 pattern = '<%sd'%length
00759 buff.write(struct.pack(pattern, *self.goal.grasp_posture.position))
00760 length = len(self.goal.grasp_posture.velocity)
00761 buff.write(_struct_I.pack(length))
00762 pattern = '<%sd'%length
00763 buff.write(struct.pack(pattern, *self.goal.grasp_posture.velocity))
00764 length = len(self.goal.grasp_posture.effort)
00765 buff.write(_struct_I.pack(length))
00766 pattern = '<%sd'%length
00767 buff.write(struct.pack(pattern, *self.goal.grasp_posture.effort))
00768 buff.write(_struct_f.pack(self.goal.max_contact_force))
00769 except struct.error as se: self._check_types(se)
00770 except TypeError as te: self._check_types(te)
00771
00772 def deserialize(self, str):
00773 """
00774 unpack serialized message in str into this message instance
00775 @param str: byte array of serialized message
00776 @type str: str
00777 """
00778 try:
00779 if self.goal is None:
00780 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
00781 end = 0
00782 start = end
00783 end += 4
00784 (length,) = _struct_I.unpack(str[start:end])
00785 start = end
00786 end += length
00787 self.goal.arm_name = str[start:end]
00788 start = end
00789 end += 4
00790 (length,) = _struct_I.unpack(str[start:end])
00791 start = end
00792 end += length
00793 self.goal.target.reference_frame_id = str[start:end]
00794 start = end
00795 end += 4
00796 (length,) = _struct_I.unpack(str[start:end])
00797 self.goal.target.potential_models = []
00798 for i in range(0, length):
00799 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
00800 start = end
00801 end += 4
00802 (val1.model_id,) = _struct_i.unpack(str[start:end])
00803 _v8 = val1.pose
00804 _v9 = _v8.header
00805 start = end
00806 end += 4
00807 (_v9.seq,) = _struct_I.unpack(str[start:end])
00808 _v10 = _v9.stamp
00809 _x = _v10
00810 start = end
00811 end += 8
00812 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00813 start = end
00814 end += 4
00815 (length,) = _struct_I.unpack(str[start:end])
00816 start = end
00817 end += length
00818 _v9.frame_id = str[start:end]
00819 _v11 = _v8.pose
00820 _v12 = _v11.position
00821 _x = _v12
00822 start = end
00823 end += 24
00824 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00825 _v13 = _v11.orientation
00826 _x = _v13
00827 start = end
00828 end += 32
00829 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00830 start = end
00831 end += 4
00832 (val1.confidence,) = _struct_f.unpack(str[start:end])
00833 start = end
00834 end += 4
00835 (length,) = _struct_I.unpack(str[start:end])
00836 start = end
00837 end += length
00838 val1.detector_name = str[start:end]
00839 self.goal.target.potential_models.append(val1)
00840 _x = self
00841 start = end
00842 end += 12
00843 (_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00844 start = end
00845 end += 4
00846 (length,) = _struct_I.unpack(str[start:end])
00847 start = end
00848 end += length
00849 self.goal.target.cluster.header.frame_id = str[start:end]
00850 start = end
00851 end += 4
00852 (length,) = _struct_I.unpack(str[start:end])
00853 self.goal.target.cluster.points = []
00854 for i in range(0, length):
00855 val1 = geometry_msgs.msg.Point32()
00856 _x = val1
00857 start = end
00858 end += 12
00859 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00860 self.goal.target.cluster.points.append(val1)
00861 start = end
00862 end += 4
00863 (length,) = _struct_I.unpack(str[start:end])
00864 self.goal.target.cluster.channels = []
00865 for i in range(0, length):
00866 val1 = sensor_msgs.msg.ChannelFloat32()
00867 start = end
00868 end += 4
00869 (length,) = _struct_I.unpack(str[start:end])
00870 start = end
00871 end += length
00872 val1.name = str[start:end]
00873 start = end
00874 end += 4
00875 (length,) = _struct_I.unpack(str[start:end])
00876 pattern = '<%sf'%length
00877 start = end
00878 end += struct.calcsize(pattern)
00879 val1.values = struct.unpack(pattern, str[start:end])
00880 self.goal.target.cluster.channels.append(val1)
00881 _x = self
00882 start = end
00883 end += 12
00884 (_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00885 start = end
00886 end += 4
00887 (length,) = _struct_I.unpack(str[start:end])
00888 start = end
00889 end += length
00890 self.goal.target.region.cloud.header.frame_id = str[start:end]
00891 _x = self
00892 start = end
00893 end += 8
00894 (_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
00895 start = end
00896 end += 4
00897 (length,) = _struct_I.unpack(str[start:end])
00898 self.goal.target.region.cloud.fields = []
00899 for i in range(0, length):
00900 val1 = sensor_msgs.msg.PointField()
00901 start = end
00902 end += 4
00903 (length,) = _struct_I.unpack(str[start:end])
00904 start = end
00905 end += length
00906 val1.name = str[start:end]
00907 _x = val1
00908 start = end
00909 end += 9
00910 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
00911 self.goal.target.region.cloud.fields.append(val1)
00912 _x = self
00913 start = end
00914 end += 9
00915 (_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
00916 self.goal.target.region.cloud.is_bigendian = bool(self.goal.target.region.cloud.is_bigendian)
00917 start = end
00918 end += 4
00919 (length,) = _struct_I.unpack(str[start:end])
00920 start = end
00921 end += length
00922 self.goal.target.region.cloud.data = str[start:end]
00923 start = end
00924 end += 1
00925 (self.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
00926 self.goal.target.region.cloud.is_dense = bool(self.goal.target.region.cloud.is_dense)
00927 start = end
00928 end += 4
00929 (length,) = _struct_I.unpack(str[start:end])
00930 pattern = '<%si'%length
00931 start = end
00932 end += struct.calcsize(pattern)
00933 self.goal.target.region.mask = struct.unpack(pattern, str[start:end])
00934 _x = self
00935 start = end
00936 end += 12
00937 (_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00938 start = end
00939 end += 4
00940 (length,) = _struct_I.unpack(str[start:end])
00941 start = end
00942 end += length
00943 self.goal.target.region.image.header.frame_id = str[start:end]
00944 _x = self
00945 start = end
00946 end += 8
00947 (_x.goal.target.region.image.height, _x.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
00948 start = end
00949 end += 4
00950 (length,) = _struct_I.unpack(str[start:end])
00951 start = end
00952 end += length
00953 self.goal.target.region.image.encoding = str[start:end]
00954 _x = self
00955 start = end
00956 end += 5
00957 (_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
00958 start = end
00959 end += 4
00960 (length,) = _struct_I.unpack(str[start:end])
00961 start = end
00962 end += length
00963 self.goal.target.region.image.data = str[start:end]
00964 _x = self
00965 start = end
00966 end += 12
00967 (_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00968 start = end
00969 end += 4
00970 (length,) = _struct_I.unpack(str[start:end])
00971 start = end
00972 end += length
00973 self.goal.target.region.disparity_image.header.frame_id = str[start:end]
00974 _x = self
00975 start = end
00976 end += 8
00977 (_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
00978 start = end
00979 end += 4
00980 (length,) = _struct_I.unpack(str[start:end])
00981 start = end
00982 end += length
00983 self.goal.target.region.disparity_image.encoding = str[start:end]
00984 _x = self
00985 start = end
00986 end += 5
00987 (_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
00988 start = end
00989 end += 4
00990 (length,) = _struct_I.unpack(str[start:end])
00991 start = end
00992 end += length
00993 self.goal.target.region.disparity_image.data = str[start:end]
00994 _x = self
00995 start = end
00996 end += 12
00997 (_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00998 start = end
00999 end += 4
01000 (length,) = _struct_I.unpack(str[start:end])
01001 start = end
01002 end += length
01003 self.goal.target.region.cam_info.header.frame_id = str[start:end]
01004 _x = self
01005 start = end
01006 end += 8
01007 (_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01008 start = end
01009 end += 4
01010 (length,) = _struct_I.unpack(str[start:end])
01011 start = end
01012 end += length
01013 self.goal.target.region.cam_info.distortion_model = str[start:end]
01014 start = end
01015 end += 4
01016 (length,) = _struct_I.unpack(str[start:end])
01017 pattern = '<%sd'%length
01018 start = end
01019 end += struct.calcsize(pattern)
01020 self.goal.target.region.cam_info.D = struct.unpack(pattern, str[start:end])
01021 start = end
01022 end += 72
01023 self.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end])
01024 start = end
01025 end += 72
01026 self.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end])
01027 start = end
01028 end += 96
01029 self.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end])
01030 _x = self
01031 start = end
01032 end += 37
01033 (_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01034 self.goal.target.region.cam_info.roi.do_rectify = bool(self.goal.target.region.cam_info.roi.do_rectify)
01035 start = end
01036 end += 4
01037 (length,) = _struct_I.unpack(str[start:end])
01038 start = end
01039 end += length
01040 self.goal.target.region.roi_box_pose.header.frame_id = str[start:end]
01041 _x = self
01042 start = end
01043 end += 80
01044 (_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
01045 start = end
01046 end += 4
01047 (length,) = _struct_I.unpack(str[start:end])
01048 start = end
01049 end += length
01050 self.goal.target.collision_name = str[start:end]
01051 _x = self
01052 start = end
01053 end += 12
01054 (_x.goal.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01055 start = end
01056 end += 4
01057 (length,) = _struct_I.unpack(str[start:end])
01058 start = end
01059 end += length
01060 self.goal.final_grasp_pose.header.frame_id = str[start:end]
01061 _x = self
01062 start = end
01063 end += 68
01064 (_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01065 start = end
01066 end += 4
01067 (length,) = _struct_I.unpack(str[start:end])
01068 start = end
01069 end += length
01070 self.goal.trajectory.header.frame_id = str[start:end]
01071 start = end
01072 end += 4
01073 (length,) = _struct_I.unpack(str[start:end])
01074 self.goal.trajectory.joint_names = []
01075 for i in range(0, length):
01076 start = end
01077 end += 4
01078 (length,) = _struct_I.unpack(str[start:end])
01079 start = end
01080 end += length
01081 val1 = str[start:end]
01082 self.goal.trajectory.joint_names.append(val1)
01083 start = end
01084 end += 4
01085 (length,) = _struct_I.unpack(str[start:end])
01086 self.goal.trajectory.points = []
01087 for i in range(0, length):
01088 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01089 start = end
01090 end += 4
01091 (length,) = _struct_I.unpack(str[start:end])
01092 pattern = '<%sd'%length
01093 start = end
01094 end += struct.calcsize(pattern)
01095 val1.positions = struct.unpack(pattern, str[start:end])
01096 start = end
01097 end += 4
01098 (length,) = _struct_I.unpack(str[start:end])
01099 pattern = '<%sd'%length
01100 start = end
01101 end += struct.calcsize(pattern)
01102 val1.velocities = struct.unpack(pattern, str[start:end])
01103 start = end
01104 end += 4
01105 (length,) = _struct_I.unpack(str[start:end])
01106 pattern = '<%sd'%length
01107 start = end
01108 end += struct.calcsize(pattern)
01109 val1.accelerations = struct.unpack(pattern, str[start:end])
01110 _v14 = val1.time_from_start
01111 _x = _v14
01112 start = end
01113 end += 8
01114 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01115 self.goal.trajectory.points.append(val1)
01116 start = end
01117 end += 4
01118 (length,) = _struct_I.unpack(str[start:end])
01119 start = end
01120 end += length
01121 self.goal.collision_support_surface_name = str[start:end]
01122 _x = self
01123 start = end
01124 end += 12
01125 (_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01126 start = end
01127 end += 4
01128 (length,) = _struct_I.unpack(str[start:end])
01129 start = end
01130 end += length
01131 self.goal.pre_grasp_posture.header.frame_id = str[start:end]
01132 start = end
01133 end += 4
01134 (length,) = _struct_I.unpack(str[start:end])
01135 self.goal.pre_grasp_posture.name = []
01136 for i in range(0, length):
01137 start = end
01138 end += 4
01139 (length,) = _struct_I.unpack(str[start:end])
01140 start = end
01141 end += length
01142 val1 = str[start:end]
01143 self.goal.pre_grasp_posture.name.append(val1)
01144 start = end
01145 end += 4
01146 (length,) = _struct_I.unpack(str[start:end])
01147 pattern = '<%sd'%length
01148 start = end
01149 end += struct.calcsize(pattern)
01150 self.goal.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 pattern = '<%sd'%length
01155 start = end
01156 end += struct.calcsize(pattern)
01157 self.goal.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01158 start = end
01159 end += 4
01160 (length,) = _struct_I.unpack(str[start:end])
01161 pattern = '<%sd'%length
01162 start = end
01163 end += struct.calcsize(pattern)
01164 self.goal.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01165 _x = self
01166 start = end
01167 end += 12
01168 (_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01169 start = end
01170 end += 4
01171 (length,) = _struct_I.unpack(str[start:end])
01172 start = end
01173 end += length
01174 self.goal.grasp_posture.header.frame_id = str[start:end]
01175 start = end
01176 end += 4
01177 (length,) = _struct_I.unpack(str[start:end])
01178 self.goal.grasp_posture.name = []
01179 for i in range(0, length):
01180 start = end
01181 end += 4
01182 (length,) = _struct_I.unpack(str[start:end])
01183 start = end
01184 end += length
01185 val1 = str[start:end]
01186 self.goal.grasp_posture.name.append(val1)
01187 start = end
01188 end += 4
01189 (length,) = _struct_I.unpack(str[start:end])
01190 pattern = '<%sd'%length
01191 start = end
01192 end += struct.calcsize(pattern)
01193 self.goal.grasp_posture.position = struct.unpack(pattern, str[start:end])
01194 start = end
01195 end += 4
01196 (length,) = _struct_I.unpack(str[start:end])
01197 pattern = '<%sd'%length
01198 start = end
01199 end += struct.calcsize(pattern)
01200 self.goal.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01201 start = end
01202 end += 4
01203 (length,) = _struct_I.unpack(str[start:end])
01204 pattern = '<%sd'%length
01205 start = end
01206 end += struct.calcsize(pattern)
01207 self.goal.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01208 start = end
01209 end += 4
01210 (self.goal.max_contact_force,) = _struct_f.unpack(str[start:end])
01211 return self
01212 except struct.error as e:
01213 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01214
01215
01216 def serialize_numpy(self, buff, numpy):
01217 """
01218 serialize message with numpy array types into buffer
01219 @param buff: buffer
01220 @type buff: StringIO
01221 @param numpy: numpy python module
01222 @type numpy module
01223 """
01224 try:
01225 _x = self.goal.arm_name
01226 length = len(_x)
01227 buff.write(struct.pack('<I%ss'%length, length, _x))
01228 _x = self.goal.target.reference_frame_id
01229 length = len(_x)
01230 buff.write(struct.pack('<I%ss'%length, length, _x))
01231 length = len(self.goal.target.potential_models)
01232 buff.write(_struct_I.pack(length))
01233 for val1 in self.goal.target.potential_models:
01234 buff.write(_struct_i.pack(val1.model_id))
01235 _v15 = val1.pose
01236 _v16 = _v15.header
01237 buff.write(_struct_I.pack(_v16.seq))
01238 _v17 = _v16.stamp
01239 _x = _v17
01240 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01241 _x = _v16.frame_id
01242 length = len(_x)
01243 buff.write(struct.pack('<I%ss'%length, length, _x))
01244 _v18 = _v15.pose
01245 _v19 = _v18.position
01246 _x = _v19
01247 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01248 _v20 = _v18.orientation
01249 _x = _v20
01250 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01251 buff.write(_struct_f.pack(val1.confidence))
01252 _x = val1.detector_name
01253 length = len(_x)
01254 buff.write(struct.pack('<I%ss'%length, length, _x))
01255 _x = self
01256 buff.write(_struct_3I.pack(_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs))
01257 _x = self.goal.target.cluster.header.frame_id
01258 length = len(_x)
01259 buff.write(struct.pack('<I%ss'%length, length, _x))
01260 length = len(self.goal.target.cluster.points)
01261 buff.write(_struct_I.pack(length))
01262 for val1 in self.goal.target.cluster.points:
01263 _x = val1
01264 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01265 length = len(self.goal.target.cluster.channels)
01266 buff.write(_struct_I.pack(length))
01267 for val1 in self.goal.target.cluster.channels:
01268 _x = val1.name
01269 length = len(_x)
01270 buff.write(struct.pack('<I%ss'%length, length, _x))
01271 length = len(val1.values)
01272 buff.write(_struct_I.pack(length))
01273 pattern = '<%sf'%length
01274 buff.write(val1.values.tostring())
01275 _x = self
01276 buff.write(_struct_3I.pack(_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs))
01277 _x = self.goal.target.region.cloud.header.frame_id
01278 length = len(_x)
01279 buff.write(struct.pack('<I%ss'%length, length, _x))
01280 _x = self
01281 buff.write(_struct_2I.pack(_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width))
01282 length = len(self.goal.target.region.cloud.fields)
01283 buff.write(_struct_I.pack(length))
01284 for val1 in self.goal.target.region.cloud.fields:
01285 _x = val1.name
01286 length = len(_x)
01287 buff.write(struct.pack('<I%ss'%length, length, _x))
01288 _x = val1
01289 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01290 _x = self
01291 buff.write(_struct_B2I.pack(_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step))
01292 _x = self.goal.target.region.cloud.data
01293 length = len(_x)
01294 # - if encoded as a list instead, serialize as bytes instead of string
01295 if type(_x) in [list, tuple]:
01296 buff.write(struct.pack('<I%sB'%length, length, *_x))
01297 else:
01298 buff.write(struct.pack('<I%ss'%length, length, _x))
01299 buff.write(_struct_B.pack(self.goal.target.region.cloud.is_dense))
01300 length = len(self.goal.target.region.mask)
01301 buff.write(_struct_I.pack(length))
01302 pattern = '<%si'%length
01303 buff.write(self.goal.target.region.mask.tostring())
01304 _x = self
01305 buff.write(_struct_3I.pack(_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs))
01306 _x = self.goal.target.region.image.header.frame_id
01307 length = len(_x)
01308 buff.write(struct.pack('<I%ss'%length, length, _x))
01309 _x = self
01310 buff.write(_struct_2I.pack(_x.goal.target.region.image.height, _x.goal.target.region.image.width))
01311 _x = self.goal.target.region.image.encoding
01312 length = len(_x)
01313 buff.write(struct.pack('<I%ss'%length, length, _x))
01314 _x = self
01315 buff.write(_struct_BI.pack(_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step))
01316 _x = self.goal.target.region.image.data
01317 length = len(_x)
01318 # - if encoded as a list instead, serialize as bytes instead of string
01319 if type(_x) in [list, tuple]:
01320 buff.write(struct.pack('<I%sB'%length, length, *_x))
01321 else:
01322 buff.write(struct.pack('<I%ss'%length, length, _x))
01323 _x = self
01324 buff.write(_struct_3I.pack(_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs))
01325 _x = self.goal.target.region.disparity_image.header.frame_id
01326 length = len(_x)
01327 buff.write(struct.pack('<I%ss'%length, length, _x))
01328 _x = self
01329 buff.write(_struct_2I.pack(_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width))
01330 _x = self.goal.target.region.disparity_image.encoding
01331 length = len(_x)
01332 buff.write(struct.pack('<I%ss'%length, length, _x))
01333 _x = self
01334 buff.write(_struct_BI.pack(_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step))
01335 _x = self.goal.target.region.disparity_image.data
01336 length = len(_x)
01337 # - if encoded as a list instead, serialize as bytes instead of string
01338 if type(_x) in [list, tuple]:
01339 buff.write(struct.pack('<I%sB'%length, length, *_x))
01340 else:
01341 buff.write(struct.pack('<I%ss'%length, length, _x))
01342 _x = self
01343 buff.write(_struct_3I.pack(_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs))
01344 _x = self.goal.target.region.cam_info.header.frame_id
01345 length = len(_x)
01346 buff.write(struct.pack('<I%ss'%length, length, _x))
01347 _x = self
01348 buff.write(_struct_2I.pack(_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width))
01349 _x = self.goal.target.region.cam_info.distortion_model
01350 length = len(_x)
01351 buff.write(struct.pack('<I%ss'%length, length, _x))
01352 length = len(self.goal.target.region.cam_info.D)
01353 buff.write(_struct_I.pack(length))
01354 pattern = '<%sd'%length
01355 buff.write(self.goal.target.region.cam_info.D.tostring())
01356 buff.write(self.goal.target.region.cam_info.K.tostring())
01357 buff.write(self.goal.target.region.cam_info.R.tostring())
01358 buff.write(self.goal.target.region.cam_info.P.tostring())
01359 _x = self
01360 buff.write(_struct_6IB3I.pack(_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs))
01361 _x = self.goal.target.region.roi_box_pose.header.frame_id
01362 length = len(_x)
01363 buff.write(struct.pack('<I%ss'%length, length, _x))
01364 _x = self
01365 buff.write(_struct_10d.pack(_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z))
01366 _x = self.goal.target.collision_name
01367 length = len(_x)
01368 buff.write(struct.pack('<I%ss'%length, length, _x))
01369 _x = self
01370 buff.write(_struct_3I.pack(_x.goal.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs))
01371 _x = self.goal.final_grasp_pose.header.frame_id
01372 length = len(_x)
01373 buff.write(struct.pack('<I%ss'%length, length, _x))
01374 _x = self
01375 buff.write(_struct_7d3I.pack(_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs))
01376 _x = self.goal.trajectory.header.frame_id
01377 length = len(_x)
01378 buff.write(struct.pack('<I%ss'%length, length, _x))
01379 length = len(self.goal.trajectory.joint_names)
01380 buff.write(_struct_I.pack(length))
01381 for val1 in self.goal.trajectory.joint_names:
01382 length = len(val1)
01383 buff.write(struct.pack('<I%ss'%length, length, val1))
01384 length = len(self.goal.trajectory.points)
01385 buff.write(_struct_I.pack(length))
01386 for val1 in self.goal.trajectory.points:
01387 length = len(val1.positions)
01388 buff.write(_struct_I.pack(length))
01389 pattern = '<%sd'%length
01390 buff.write(val1.positions.tostring())
01391 length = len(val1.velocities)
01392 buff.write(_struct_I.pack(length))
01393 pattern = '<%sd'%length
01394 buff.write(val1.velocities.tostring())
01395 length = len(val1.accelerations)
01396 buff.write(_struct_I.pack(length))
01397 pattern = '<%sd'%length
01398 buff.write(val1.accelerations.tostring())
01399 _v21 = val1.time_from_start
01400 _x = _v21
01401 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01402 _x = self.goal.collision_support_surface_name
01403 length = len(_x)
01404 buff.write(struct.pack('<I%ss'%length, length, _x))
01405 _x = self
01406 buff.write(_struct_3I.pack(_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs))
01407 _x = self.goal.pre_grasp_posture.header.frame_id
01408 length = len(_x)
01409 buff.write(struct.pack('<I%ss'%length, length, _x))
01410 length = len(self.goal.pre_grasp_posture.name)
01411 buff.write(_struct_I.pack(length))
01412 for val1 in self.goal.pre_grasp_posture.name:
01413 length = len(val1)
01414 buff.write(struct.pack('<I%ss'%length, length, val1))
01415 length = len(self.goal.pre_grasp_posture.position)
01416 buff.write(_struct_I.pack(length))
01417 pattern = '<%sd'%length
01418 buff.write(self.goal.pre_grasp_posture.position.tostring())
01419 length = len(self.goal.pre_grasp_posture.velocity)
01420 buff.write(_struct_I.pack(length))
01421 pattern = '<%sd'%length
01422 buff.write(self.goal.pre_grasp_posture.velocity.tostring())
01423 length = len(self.goal.pre_grasp_posture.effort)
01424 buff.write(_struct_I.pack(length))
01425 pattern = '<%sd'%length
01426 buff.write(self.goal.pre_grasp_posture.effort.tostring())
01427 _x = self
01428 buff.write(_struct_3I.pack(_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs))
01429 _x = self.goal.grasp_posture.header.frame_id
01430 length = len(_x)
01431 buff.write(struct.pack('<I%ss'%length, length, _x))
01432 length = len(self.goal.grasp_posture.name)
01433 buff.write(_struct_I.pack(length))
01434 for val1 in self.goal.grasp_posture.name:
01435 length = len(val1)
01436 buff.write(struct.pack('<I%ss'%length, length, val1))
01437 length = len(self.goal.grasp_posture.position)
01438 buff.write(_struct_I.pack(length))
01439 pattern = '<%sd'%length
01440 buff.write(self.goal.grasp_posture.position.tostring())
01441 length = len(self.goal.grasp_posture.velocity)
01442 buff.write(_struct_I.pack(length))
01443 pattern = '<%sd'%length
01444 buff.write(self.goal.grasp_posture.velocity.tostring())
01445 length = len(self.goal.grasp_posture.effort)
01446 buff.write(_struct_I.pack(length))
01447 pattern = '<%sd'%length
01448 buff.write(self.goal.grasp_posture.effort.tostring())
01449 buff.write(_struct_f.pack(self.goal.max_contact_force))
01450 except struct.error as se: self._check_types(se)
01451 except TypeError as te: self._check_types(te)
01452
01453 def deserialize_numpy(self, str, numpy):
01454 """
01455 unpack serialized message in str into this message instance using numpy for array types
01456 @param str: byte array of serialized message
01457 @type str: str
01458 @param numpy: numpy python module
01459 @type numpy: module
01460 """
01461 try:
01462 if self.goal is None:
01463 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
01464 end = 0
01465 start = end
01466 end += 4
01467 (length,) = _struct_I.unpack(str[start:end])
01468 start = end
01469 end += length
01470 self.goal.arm_name = str[start:end]
01471 start = end
01472 end += 4
01473 (length,) = _struct_I.unpack(str[start:end])
01474 start = end
01475 end += length
01476 self.goal.target.reference_frame_id = str[start:end]
01477 start = end
01478 end += 4
01479 (length,) = _struct_I.unpack(str[start:end])
01480 self.goal.target.potential_models = []
01481 for i in range(0, length):
01482 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01483 start = end
01484 end += 4
01485 (val1.model_id,) = _struct_i.unpack(str[start:end])
01486 _v22 = val1.pose
01487 _v23 = _v22.header
01488 start = end
01489 end += 4
01490 (_v23.seq,) = _struct_I.unpack(str[start:end])
01491 _v24 = _v23.stamp
01492 _x = _v24
01493 start = end
01494 end += 8
01495 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01496 start = end
01497 end += 4
01498 (length,) = _struct_I.unpack(str[start:end])
01499 start = end
01500 end += length
01501 _v23.frame_id = str[start:end]
01502 _v25 = _v22.pose
01503 _v26 = _v25.position
01504 _x = _v26
01505 start = end
01506 end += 24
01507 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01508 _v27 = _v25.orientation
01509 _x = _v27
01510 start = end
01511 end += 32
01512 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01513 start = end
01514 end += 4
01515 (val1.confidence,) = _struct_f.unpack(str[start:end])
01516 start = end
01517 end += 4
01518 (length,) = _struct_I.unpack(str[start:end])
01519 start = end
01520 end += length
01521 val1.detector_name = str[start:end]
01522 self.goal.target.potential_models.append(val1)
01523 _x = self
01524 start = end
01525 end += 12
01526 (_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01527 start = end
01528 end += 4
01529 (length,) = _struct_I.unpack(str[start:end])
01530 start = end
01531 end += length
01532 self.goal.target.cluster.header.frame_id = str[start:end]
01533 start = end
01534 end += 4
01535 (length,) = _struct_I.unpack(str[start:end])
01536 self.goal.target.cluster.points = []
01537 for i in range(0, length):
01538 val1 = geometry_msgs.msg.Point32()
01539 _x = val1
01540 start = end
01541 end += 12
01542 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01543 self.goal.target.cluster.points.append(val1)
01544 start = end
01545 end += 4
01546 (length,) = _struct_I.unpack(str[start:end])
01547 self.goal.target.cluster.channels = []
01548 for i in range(0, length):
01549 val1 = sensor_msgs.msg.ChannelFloat32()
01550 start = end
01551 end += 4
01552 (length,) = _struct_I.unpack(str[start:end])
01553 start = end
01554 end += length
01555 val1.name = str[start:end]
01556 start = end
01557 end += 4
01558 (length,) = _struct_I.unpack(str[start:end])
01559 pattern = '<%sf'%length
01560 start = end
01561 end += struct.calcsize(pattern)
01562 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01563 self.goal.target.cluster.channels.append(val1)
01564 _x = self
01565 start = end
01566 end += 12
01567 (_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01568 start = end
01569 end += 4
01570 (length,) = _struct_I.unpack(str[start:end])
01571 start = end
01572 end += length
01573 self.goal.target.region.cloud.header.frame_id = str[start:end]
01574 _x = self
01575 start = end
01576 end += 8
01577 (_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01578 start = end
01579 end += 4
01580 (length,) = _struct_I.unpack(str[start:end])
01581 self.goal.target.region.cloud.fields = []
01582 for i in range(0, length):
01583 val1 = sensor_msgs.msg.PointField()
01584 start = end
01585 end += 4
01586 (length,) = _struct_I.unpack(str[start:end])
01587 start = end
01588 end += length
01589 val1.name = str[start:end]
01590 _x = val1
01591 start = end
01592 end += 9
01593 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01594 self.goal.target.region.cloud.fields.append(val1)
01595 _x = self
01596 start = end
01597 end += 9
01598 (_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01599 self.goal.target.region.cloud.is_bigendian = bool(self.goal.target.region.cloud.is_bigendian)
01600 start = end
01601 end += 4
01602 (length,) = _struct_I.unpack(str[start:end])
01603 start = end
01604 end += length
01605 self.goal.target.region.cloud.data = str[start:end]
01606 start = end
01607 end += 1
01608 (self.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01609 self.goal.target.region.cloud.is_dense = bool(self.goal.target.region.cloud.is_dense)
01610 start = end
01611 end += 4
01612 (length,) = _struct_I.unpack(str[start:end])
01613 pattern = '<%si'%length
01614 start = end
01615 end += struct.calcsize(pattern)
01616 self.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01617 _x = self
01618 start = end
01619 end += 12
01620 (_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01621 start = end
01622 end += 4
01623 (length,) = _struct_I.unpack(str[start:end])
01624 start = end
01625 end += length
01626 self.goal.target.region.image.header.frame_id = str[start:end]
01627 _x = self
01628 start = end
01629 end += 8
01630 (_x.goal.target.region.image.height, _x.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01631 start = end
01632 end += 4
01633 (length,) = _struct_I.unpack(str[start:end])
01634 start = end
01635 end += length
01636 self.goal.target.region.image.encoding = str[start:end]
01637 _x = self
01638 start = end
01639 end += 5
01640 (_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01641 start = end
01642 end += 4
01643 (length,) = _struct_I.unpack(str[start:end])
01644 start = end
01645 end += length
01646 self.goal.target.region.image.data = str[start:end]
01647 _x = self
01648 start = end
01649 end += 12
01650 (_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01651 start = end
01652 end += 4
01653 (length,) = _struct_I.unpack(str[start:end])
01654 start = end
01655 end += length
01656 self.goal.target.region.disparity_image.header.frame_id = str[start:end]
01657 _x = self
01658 start = end
01659 end += 8
01660 (_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01661 start = end
01662 end += 4
01663 (length,) = _struct_I.unpack(str[start:end])
01664 start = end
01665 end += length
01666 self.goal.target.region.disparity_image.encoding = str[start:end]
01667 _x = self
01668 start = end
01669 end += 5
01670 (_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01671 start = end
01672 end += 4
01673 (length,) = _struct_I.unpack(str[start:end])
01674 start = end
01675 end += length
01676 self.goal.target.region.disparity_image.data = str[start:end]
01677 _x = self
01678 start = end
01679 end += 12
01680 (_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01681 start = end
01682 end += 4
01683 (length,) = _struct_I.unpack(str[start:end])
01684 start = end
01685 end += length
01686 self.goal.target.region.cam_info.header.frame_id = str[start:end]
01687 _x = self
01688 start = end
01689 end += 8
01690 (_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01691 start = end
01692 end += 4
01693 (length,) = _struct_I.unpack(str[start:end])
01694 start = end
01695 end += length
01696 self.goal.target.region.cam_info.distortion_model = str[start:end]
01697 start = end
01698 end += 4
01699 (length,) = _struct_I.unpack(str[start:end])
01700 pattern = '<%sd'%length
01701 start = end
01702 end += struct.calcsize(pattern)
01703 self.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01704 start = end
01705 end += 72
01706 self.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01707 start = end
01708 end += 72
01709 self.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01710 start = end
01711 end += 96
01712 self.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
01713 _x = self
01714 start = end
01715 end += 37
01716 (_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01717 self.goal.target.region.cam_info.roi.do_rectify = bool(self.goal.target.region.cam_info.roi.do_rectify)
01718 start = end
01719 end += 4
01720 (length,) = _struct_I.unpack(str[start:end])
01721 start = end
01722 end += length
01723 self.goal.target.region.roi_box_pose.header.frame_id = str[start:end]
01724 _x = self
01725 start = end
01726 end += 80
01727 (_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
01728 start = end
01729 end += 4
01730 (length,) = _struct_I.unpack(str[start:end])
01731 start = end
01732 end += length
01733 self.goal.target.collision_name = str[start:end]
01734 _x = self
01735 start = end
01736 end += 12
01737 (_x.goal.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01738 start = end
01739 end += 4
01740 (length,) = _struct_I.unpack(str[start:end])
01741 start = end
01742 end += length
01743 self.goal.final_grasp_pose.header.frame_id = str[start:end]
01744 _x = self
01745 start = end
01746 end += 68
01747 (_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01748 start = end
01749 end += 4
01750 (length,) = _struct_I.unpack(str[start:end])
01751 start = end
01752 end += length
01753 self.goal.trajectory.header.frame_id = str[start:end]
01754 start = end
01755 end += 4
01756 (length,) = _struct_I.unpack(str[start:end])
01757 self.goal.trajectory.joint_names = []
01758 for i in range(0, length):
01759 start = end
01760 end += 4
01761 (length,) = _struct_I.unpack(str[start:end])
01762 start = end
01763 end += length
01764 val1 = str[start:end]
01765 self.goal.trajectory.joint_names.append(val1)
01766 start = end
01767 end += 4
01768 (length,) = _struct_I.unpack(str[start:end])
01769 self.goal.trajectory.points = []
01770 for i in range(0, length):
01771 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01772 start = end
01773 end += 4
01774 (length,) = _struct_I.unpack(str[start:end])
01775 pattern = '<%sd'%length
01776 start = end
01777 end += struct.calcsize(pattern)
01778 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01779 start = end
01780 end += 4
01781 (length,) = _struct_I.unpack(str[start:end])
01782 pattern = '<%sd'%length
01783 start = end
01784 end += struct.calcsize(pattern)
01785 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01786 start = end
01787 end += 4
01788 (length,) = _struct_I.unpack(str[start:end])
01789 pattern = '<%sd'%length
01790 start = end
01791 end += struct.calcsize(pattern)
01792 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01793 _v28 = val1.time_from_start
01794 _x = _v28
01795 start = end
01796 end += 8
01797 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01798 self.goal.trajectory.points.append(val1)
01799 start = end
01800 end += 4
01801 (length,) = _struct_I.unpack(str[start:end])
01802 start = end
01803 end += length
01804 self.goal.collision_support_surface_name = str[start:end]
01805 _x = self
01806 start = end
01807 end += 12
01808 (_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01809 start = end
01810 end += 4
01811 (length,) = _struct_I.unpack(str[start:end])
01812 start = end
01813 end += length
01814 self.goal.pre_grasp_posture.header.frame_id = str[start:end]
01815 start = end
01816 end += 4
01817 (length,) = _struct_I.unpack(str[start:end])
01818 self.goal.pre_grasp_posture.name = []
01819 for i in range(0, length):
01820 start = end
01821 end += 4
01822 (length,) = _struct_I.unpack(str[start:end])
01823 start = end
01824 end += length
01825 val1 = str[start:end]
01826 self.goal.pre_grasp_posture.name.append(val1)
01827 start = end
01828 end += 4
01829 (length,) = _struct_I.unpack(str[start:end])
01830 pattern = '<%sd'%length
01831 start = end
01832 end += struct.calcsize(pattern)
01833 self.goal.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01834 start = end
01835 end += 4
01836 (length,) = _struct_I.unpack(str[start:end])
01837 pattern = '<%sd'%length
01838 start = end
01839 end += struct.calcsize(pattern)
01840 self.goal.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01841 start = end
01842 end += 4
01843 (length,) = _struct_I.unpack(str[start:end])
01844 pattern = '<%sd'%length
01845 start = end
01846 end += struct.calcsize(pattern)
01847 self.goal.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01848 _x = self
01849 start = end
01850 end += 12
01851 (_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01852 start = end
01853 end += 4
01854 (length,) = _struct_I.unpack(str[start:end])
01855 start = end
01856 end += length
01857 self.goal.grasp_posture.header.frame_id = str[start:end]
01858 start = end
01859 end += 4
01860 (length,) = _struct_I.unpack(str[start:end])
01861 self.goal.grasp_posture.name = []
01862 for i in range(0, length):
01863 start = end
01864 end += 4
01865 (length,) = _struct_I.unpack(str[start:end])
01866 start = end
01867 end += length
01868 val1 = str[start:end]
01869 self.goal.grasp_posture.name.append(val1)
01870 start = end
01871 end += 4
01872 (length,) = _struct_I.unpack(str[start:end])
01873 pattern = '<%sd'%length
01874 start = end
01875 end += struct.calcsize(pattern)
01876 self.goal.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01877 start = end
01878 end += 4
01879 (length,) = _struct_I.unpack(str[start:end])
01880 pattern = '<%sd'%length
01881 start = end
01882 end += struct.calcsize(pattern)
01883 self.goal.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01884 start = end
01885 end += 4
01886 (length,) = _struct_I.unpack(str[start:end])
01887 pattern = '<%sd'%length
01888 start = end
01889 end += struct.calcsize(pattern)
01890 self.goal.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01891 start = end
01892 end += 4
01893 (self.goal.max_contact_force,) = _struct_f.unpack(str[start:end])
01894 return self
01895 except struct.error as e:
01896 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01897
01898 _struct_I = roslib.message.struct_I
01899 _struct_7d3I = struct.Struct("<7d3I")
01900 _struct_IBI = struct.Struct("<IBI")
01901 _struct_6IB3I = struct.Struct("<6IB3I")
01902 _struct_B = struct.Struct("<B")
01903 _struct_12d = struct.Struct("<12d")
01904 _struct_f = struct.Struct("<f")
01905 _struct_i = struct.Struct("<i")
01906 _struct_BI = struct.Struct("<BI")
01907 _struct_10d = struct.Struct("<10d")
01908 _struct_3f = struct.Struct("<3f")
01909 _struct_2i = struct.Struct("<2i")
01910 _struct_3I = struct.Struct("<3I")
01911 _struct_9d = struct.Struct("<9d")
01912 _struct_B2I = struct.Struct("<B2I")
01913 _struct_4d = struct.Struct("<4d")
01914 _struct_2I = struct.Struct("<2I")
01915 _struct_3d = struct.Struct("<3d")
01916 """autogenerated by genmsg_py from ReactiveGraspResponse.msg. Do not edit."""
01917 import roslib.message
01918 import struct
01919
01920 import object_manipulation_msgs.msg
01921
01922 class ReactiveGraspResponse(roslib.message.Message):
01923 _md5sum = "eb66ddf1bad009330070ddb3ebea8f76"
01924 _type = "object_manipulation_msgs/ReactiveGraspResponse"
01925 _has_header = False #flag to mark the presence of a Header object
01926 _full_text = """
01927
01928 ReactiveGraspResult result
01929
01930 ================================================================================
01931 MSG: object_manipulation_msgs/ReactiveGraspResult
01932 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
01933
01934 # the result of the reactive grasp attempt
01935
01936 ManipulationResult manipulation_result
01937
01938
01939 ================================================================================
01940 MSG: object_manipulation_msgs/ManipulationResult
01941 # Result codes for manipulation tasks
01942
01943 # task completed as expected
01944 # generally means you can proceed as planned
01945 int32 SUCCESS = 1
01946
01947 # task not possible (e.g. out of reach or obstacles in the way)
01948 # generally means that the world was not disturbed, so you can try another task
01949 int32 UNFEASIBLE = -1
01950
01951 # task was thought possible, but failed due to unexpected events during execution
01952 # it is likely that the world was disturbed, so you are encouraged to refresh
01953 # your sensed world model before proceeding to another task
01954 int32 FAILED = -2
01955
01956 # a lower level error prevented task completion (e.g. joint controller not responding)
01957 # generally requires human attention
01958 int32 ERROR = -3
01959
01960 # means that at some point during execution we ended up in a state that the collision-aware
01961 # arm navigation module will not move out of. The world was likely not disturbed, but you
01962 # probably need a new collision map to move the arm out of the stuck position
01963 int32 ARM_MOVEMENT_PREVENTED = -4
01964
01965 # specific to grasp actions
01966 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
01967 # it is likely that the collision environment will see collisions between the hand/object and the support surface
01968 int32 LIFT_FAILED = -5
01969
01970 # specific to place actions
01971 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
01972 # it is likely that the collision environment will see collisions between the hand and the object
01973 int32 RETREAT_FAILED = -6
01974
01975 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
01976 int32 CANCELLED = -7
01977
01978 # the actual value of this error code
01979 int32 value
01980
01981 """
01982 __slots__ = ['result']
01983 _slot_types = ['object_manipulation_msgs/ReactiveGraspResult']
01984
01985 def __init__(self, *args, **kwds):
01986 """
01987 Constructor. Any message fields that are implicitly/explicitly
01988 set to None will be assigned a default value. The recommend
01989 use is keyword arguments as this is more robust to future message
01990 changes. You cannot mix in-order arguments and keyword arguments.
01991
01992 The available fields are:
01993 result
01994
01995 @param args: complete set of field values, in .msg order
01996 @param kwds: use keyword arguments corresponding to message field names
01997 to set specific fields.
01998 """
01999 if args or kwds:
02000 super(ReactiveGraspResponse, self).__init__(*args, **kwds)
02001 #message fields cannot be None, assign default values for those that are
02002 if self.result is None:
02003 self.result = object_manipulation_msgs.msg.ReactiveGraspResult()
02004 else:
02005 self.result = object_manipulation_msgs.msg.ReactiveGraspResult()
02006
02007 def _get_types(self):
02008 """
02009 internal API method
02010 """
02011 return self._slot_types
02012
02013 def serialize(self, buff):
02014 """
02015 serialize message into buffer
02016 @param buff: buffer
02017 @type buff: StringIO
02018 """
02019 try:
02020 buff.write(_struct_i.pack(self.result.manipulation_result.value))
02021 except struct.error as se: self._check_types(se)
02022 except TypeError as te: self._check_types(te)
02023
02024 def deserialize(self, str):
02025 """
02026 unpack serialized message in str into this message instance
02027 @param str: byte array of serialized message
02028 @type str: str
02029 """
02030 try:
02031 if self.result is None:
02032 self.result = object_manipulation_msgs.msg.ReactiveGraspResult()
02033 end = 0
02034 start = end
02035 end += 4
02036 (self.result.manipulation_result.value,) = _struct_i.unpack(str[start:end])
02037 return self
02038 except struct.error as e:
02039 raise roslib.message.DeserializationError(e) #most likely buffer underfill
02040
02041
02042 def serialize_numpy(self, buff, numpy):
02043 """
02044 serialize message with numpy array types into buffer
02045 @param buff: buffer
02046 @type buff: StringIO
02047 @param numpy: numpy python module
02048 @type numpy module
02049 """
02050 try:
02051 buff.write(_struct_i.pack(self.result.manipulation_result.value))
02052 except struct.error as se: self._check_types(se)
02053 except TypeError as te: self._check_types(te)
02054
02055 def deserialize_numpy(self, str, numpy):
02056 """
02057 unpack serialized message in str into this message instance using numpy for array types
02058 @param str: byte array of serialized message
02059 @type str: str
02060 @param numpy: numpy python module
02061 @type numpy: module
02062 """
02063 try:
02064 if self.result is None:
02065 self.result = object_manipulation_msgs.msg.ReactiveGraspResult()
02066 end = 0
02067 start = end
02068 end += 4
02069 (self.result.manipulation_result.value,) = _struct_i.unpack(str[start:end])
02070 return self
02071 except struct.error as e:
02072 raise roslib.message.DeserializationError(e) #most likely buffer underfill
02073
02074 _struct_I = roslib.message.struct_I
02075 _struct_i = struct.Struct("<i")
02076 class ReactiveGrasp(roslib.message.ServiceDefinition):
02077 _type = 'object_manipulation_msgs/ReactiveGrasp'
02078 _md5sum = '4d5c2e842d2f6dbd7fc4253a66c2bb5f'
02079 _request_class = ReactiveGraspRequest
02080 _response_class = ReactiveGraspResponse