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