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