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