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