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