00001 """autogenerated by genmsg_py from ReactiveGraspActionGoal.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import trajectory_msgs.msg
00006 import object_manipulation_msgs.msg
00007 import roslib.rostime
00008 import actionlib_msgs.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import std_msgs.msg
00012 import household_objects_database_msgs.msg
00013
00014 class ReactiveGraspActionGoal(roslib.message.Message):
00015 _md5sum = "aa320d11322919c57c31cc4e7c61a872"
00016 _type = "object_manipulation_msgs/ReactiveGraspActionGoal"
00017 _has_header = True
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 Header header
00021 actionlib_msgs/GoalID goal_id
00022 ReactiveGraspGoal goal
00023
00024 ================================================================================
00025 MSG: std_msgs/Header
00026 # Standard metadata for higher-level stamped data types.
00027 # This is generally used to communicate timestamped data
00028 # in a particular coordinate frame.
00029 #
00030 # sequence ID: consecutively increasing ID
00031 uint32 seq
00032 #Two-integer timestamp that is expressed as:
00033 # * stamp.secs: seconds (stamp_secs) since epoch
00034 # * stamp.nsecs: nanoseconds since stamp_secs
00035 # time-handling sugar is provided by the client library
00036 time stamp
00037 #Frame this data is associated with
00038 # 0: no frame
00039 # 1: global frame
00040 string frame_id
00041
00042 ================================================================================
00043 MSG: actionlib_msgs/GoalID
00044 # The stamp should store the time at which this goal was requested.
00045 # It is used by an action server when it tries to preempt all
00046 # goals that were requested before a certain time
00047 time stamp
00048
00049 # The id provides a way to associate feedback and
00050 # result message with specific goal requests. The id
00051 # specified must be unique.
00052 string id
00053
00054
00055 ================================================================================
00056 MSG: object_manipulation_msgs/ReactiveGraspGoal
00057 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00058 # an action for reactive grasping
00059 # a reactive grasp starts from the current pose of the gripper and ends
00060 # at a desired grasp pose, presumably using the touch sensors along the way
00061
00062 # the name of the arm being used
00063 string arm_name
00064
00065 # the object to be grasped
00066 GraspableObject target
00067
00068 # the desired grasp pose for the hand
00069 geometry_msgs/PoseStamped final_grasp_pose
00070
00071 # the joint trajectory to use for the approach (if available)
00072 # this trajectory is expected to start at the current pose of the gripper
00073 # and end at the desired grasp pose
00074 trajectory_msgs/JointTrajectory trajectory
00075
00076 # the name of the support surface in the collision environment, if any
00077 string collision_support_surface_name
00078
00079 # The internal posture of the hand for the pre-grasp
00080 # only positions are used
00081 sensor_msgs/JointState pre_grasp_posture
00082
00083 # The internal posture of the hand for the grasp
00084 # positions and efforts are used
00085 sensor_msgs/JointState grasp_posture
00086
00087
00088 ================================================================================
00089 MSG: object_manipulation_msgs/GraspableObject
00090 # an object that the object_manipulator can work on
00091
00092 # a graspable object can be represented in multiple ways. This message
00093 # can contain all of them. Which one is actually used is up to the receiver
00094 # of this message. When adding new representations, one must be careful that
00095 # they have reasonable lightweight defaults indicating that that particular
00096 # representation is not available.
00097
00098 # the tf frame to be used as a reference frame when combining information from
00099 # the different representations below
00100 string reference_frame_id
00101
00102 # potential recognition results from a database of models
00103 # all poses are relative to the object reference pose
00104 household_objects_database_msgs/DatabaseModelPose[] potential_models
00105
00106 # the point cloud itself
00107 sensor_msgs/PointCloud cluster
00108
00109 # a region of a PointCloud2 of interest
00110 object_manipulation_msgs/SceneRegion region
00111
00112
00113 ================================================================================
00114 MSG: household_objects_database_msgs/DatabaseModelPose
00115 # Informs that a specific model from the Model Database has been
00116 # identified at a certain location
00117
00118 # the database id of the model
00119 int32 model_id
00120
00121 # the pose that it can be found in
00122 geometry_msgs/PoseStamped pose
00123
00124 # a measure of the confidence level in this detection result
00125 float32 confidence
00126 ================================================================================
00127 MSG: geometry_msgs/PoseStamped
00128 # A Pose with reference coordinate frame and timestamp
00129 Header header
00130 Pose pose
00131
00132 ================================================================================
00133 MSG: geometry_msgs/Pose
00134 # A representation of pose in free space, composed of postion and orientation.
00135 Point position
00136 Quaternion orientation
00137
00138 ================================================================================
00139 MSG: geometry_msgs/Point
00140 # This contains the position of a point in free space
00141 float64 x
00142 float64 y
00143 float64 z
00144
00145 ================================================================================
00146 MSG: geometry_msgs/Quaternion
00147 # This represents an orientation in free space in quaternion form.
00148
00149 float64 x
00150 float64 y
00151 float64 z
00152 float64 w
00153
00154 ================================================================================
00155 MSG: sensor_msgs/PointCloud
00156 # This message holds a collection of 3d points, plus optional additional
00157 # information about each point.
00158
00159 # Time of sensor data acquisition, coordinate frame ID.
00160 Header header
00161
00162 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00163 # in the frame given in the header.
00164 geometry_msgs/Point32[] points
00165
00166 # Each channel should have the same number of elements as points array,
00167 # and the data in each channel should correspond 1:1 with each point.
00168 # Channel names in common practice are listed in ChannelFloat32.msg.
00169 ChannelFloat32[] channels
00170
00171 ================================================================================
00172 MSG: geometry_msgs/Point32
00173 # This contains the position of a point in free space(with 32 bits of precision).
00174 # It is recommeded to use Point wherever possible instead of Point32.
00175 #
00176 # This recommendation is to promote interoperability.
00177 #
00178 # This message is designed to take up less space when sending
00179 # lots of points at once, as in the case of a PointCloud.
00180
00181 float32 x
00182 float32 y
00183 float32 z
00184 ================================================================================
00185 MSG: sensor_msgs/ChannelFloat32
00186 # This message is used by the PointCloud message to hold optional data
00187 # associated with each point in the cloud. The length of the values
00188 # array should be the same as the length of the points array in the
00189 # PointCloud, and each value should be associated with the corresponding
00190 # point.
00191
00192 # Channel names in existing practice include:
00193 # "u", "v" - row and column (respectively) in the left stereo image.
00194 # This is opposite to usual conventions but remains for
00195 # historical reasons. The newer PointCloud2 message has no
00196 # such problem.
00197 # "rgb" - For point clouds produced by color stereo cameras. uint8
00198 # (R,G,B) values packed into the least significant 24 bits,
00199 # in order.
00200 # "intensity" - laser or pixel intensity.
00201 # "distance"
00202
00203 # The channel name should give semantics of the channel (e.g.
00204 # "intensity" instead of "value").
00205 string name
00206
00207 # The values array should be 1-1 with the elements of the associated
00208 # PointCloud.
00209 float32[] values
00210
00211 ================================================================================
00212 MSG: object_manipulation_msgs/SceneRegion
00213 # Point cloud
00214 sensor_msgs/PointCloud2 cloud
00215
00216 # Indices for the region of interest
00217 int32[] mask
00218
00219 # One of the corresponding 2D images, if applicable
00220 sensor_msgs/Image image
00221
00222 # The disparity image, if applicable
00223 sensor_msgs/Image disparity_image
00224
00225 # Camera info for the camera that took the image
00226 sensor_msgs/CameraInfo cam_info
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: trajectory_msgs/JointTrajectory
00464 Header header
00465 string[] joint_names
00466 JointTrajectoryPoint[] points
00467 ================================================================================
00468 MSG: trajectory_msgs/JointTrajectoryPoint
00469 float64[] positions
00470 float64[] velocities
00471 float64[] accelerations
00472 duration time_from_start
00473 ================================================================================
00474 MSG: sensor_msgs/JointState
00475 # This is a message that holds data to describe the state of a set of torque controlled joints.
00476 #
00477 # The state of each joint (revolute or prismatic) is defined by:
00478 # * the position of the joint (rad or m),
00479 # * the velocity of the joint (rad/s or m/s) and
00480 # * the effort that is applied in the joint (Nm or N).
00481 #
00482 # Each joint is uniquely identified by its name
00483 # The header specifies the time at which the joint states were recorded. All the joint states
00484 # in one message have to be recorded at the same time.
00485 #
00486 # This message consists of a multiple arrays, one for each part of the joint state.
00487 # The goal is to make each of the fields optional. When e.g. your joints have no
00488 # effort associated with them, you can leave the effort array empty.
00489 #
00490 # All arrays in this message should have the same size, or be empty.
00491 # This is the only way to uniquely associate the joint name with the correct
00492 # states.
00493
00494
00495 Header header
00496
00497 string[] name
00498 float64[] position
00499 float64[] velocity
00500 float64[] effort
00501
00502 """
00503 __slots__ = ['header','goal_id','goal']
00504 _slot_types = ['Header','actionlib_msgs/GoalID','object_manipulation_msgs/ReactiveGraspGoal']
00505
00506 def __init__(self, *args, **kwds):
00507 """
00508 Constructor. Any message fields that are implicitly/explicitly
00509 set to None will be assigned a default value. The recommend
00510 use is keyword arguments as this is more robust to future message
00511 changes. You cannot mix in-order arguments and keyword arguments.
00512
00513 The available fields are:
00514 header,goal_id,goal
00515
00516 @param args: complete set of field values, in .msg order
00517 @param kwds: use keyword arguments corresponding to message field names
00518 to set specific fields.
00519 """
00520 if args or kwds:
00521 super(ReactiveGraspActionGoal, self).__init__(*args, **kwds)
00522 #message fields cannot be None, assign default values for those that are
00523 if self.header is None:
00524 self.header = std_msgs.msg._Header.Header()
00525 if self.goal_id is None:
00526 self.goal_id = actionlib_msgs.msg.GoalID()
00527 if self.goal is None:
00528 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
00529 else:
00530 self.header = std_msgs.msg._Header.Header()
00531 self.goal_id = actionlib_msgs.msg.GoalID()
00532 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
00533
00534 def _get_types(self):
00535 """
00536 internal API method
00537 """
00538 return self._slot_types
00539
00540 def serialize(self, buff):
00541 """
00542 serialize message into buffer
00543 @param buff: buffer
00544 @type buff: StringIO
00545 """
00546 try:
00547 _x = self
00548 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00549 _x = self.header.frame_id
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 buff.write(struct.pack('<I%ss'%length, length, _x))
00557 _x = self.goal.arm_name
00558 length = len(_x)
00559 buff.write(struct.pack('<I%ss'%length, length, _x))
00560 _x = self.goal.target.reference_frame_id
00561 length = len(_x)
00562 buff.write(struct.pack('<I%ss'%length, length, _x))
00563 length = len(self.goal.target.potential_models)
00564 buff.write(_struct_I.pack(length))
00565 for val1 in self.goal.target.potential_models:
00566 buff.write(_struct_i.pack(val1.model_id))
00567 _v1 = val1.pose
00568 _v2 = _v1.header
00569 buff.write(_struct_I.pack(_v2.seq))
00570 _v3 = _v2.stamp
00571 _x = _v3
00572 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00573 _x = _v2.frame_id
00574 length = len(_x)
00575 buff.write(struct.pack('<I%ss'%length, length, _x))
00576 _v4 = _v1.pose
00577 _v5 = _v4.position
00578 _x = _v5
00579 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00580 _v6 = _v4.orientation
00581 _x = _v6
00582 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00583 buff.write(_struct_f.pack(val1.confidence))
00584 _x = self
00585 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))
00586 _x = self.goal.target.cluster.header.frame_id
00587 length = len(_x)
00588 buff.write(struct.pack('<I%ss'%length, length, _x))
00589 length = len(self.goal.target.cluster.points)
00590 buff.write(_struct_I.pack(length))
00591 for val1 in self.goal.target.cluster.points:
00592 _x = val1
00593 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00594 length = len(self.goal.target.cluster.channels)
00595 buff.write(_struct_I.pack(length))
00596 for val1 in self.goal.target.cluster.channels:
00597 _x = val1.name
00598 length = len(_x)
00599 buff.write(struct.pack('<I%ss'%length, length, _x))
00600 length = len(val1.values)
00601 buff.write(_struct_I.pack(length))
00602 pattern = '<%sf'%length
00603 buff.write(struct.pack(pattern, *val1.values))
00604 _x = self
00605 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))
00606 _x = self.goal.target.region.cloud.header.frame_id
00607 length = len(_x)
00608 buff.write(struct.pack('<I%ss'%length, length, _x))
00609 _x = self
00610 buff.write(_struct_2I.pack(_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width))
00611 length = len(self.goal.target.region.cloud.fields)
00612 buff.write(_struct_I.pack(length))
00613 for val1 in self.goal.target.region.cloud.fields:
00614 _x = val1.name
00615 length = len(_x)
00616 buff.write(struct.pack('<I%ss'%length, length, _x))
00617 _x = val1
00618 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00619 _x = self
00620 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))
00621 _x = self.goal.target.region.cloud.data
00622 length = len(_x)
00623 # - if encoded as a list instead, serialize as bytes instead of string
00624 if type(_x) in [list, tuple]:
00625 buff.write(struct.pack('<I%sB'%length, length, *_x))
00626 else:
00627 buff.write(struct.pack('<I%ss'%length, length, _x))
00628 buff.write(_struct_B.pack(self.goal.target.region.cloud.is_dense))
00629 length = len(self.goal.target.region.mask)
00630 buff.write(_struct_I.pack(length))
00631 pattern = '<%si'%length
00632 buff.write(struct.pack(pattern, *self.goal.target.region.mask))
00633 _x = self
00634 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))
00635 _x = self.goal.target.region.image.header.frame_id
00636 length = len(_x)
00637 buff.write(struct.pack('<I%ss'%length, length, _x))
00638 _x = self
00639 buff.write(_struct_2I.pack(_x.goal.target.region.image.height, _x.goal.target.region.image.width))
00640 _x = self.goal.target.region.image.encoding
00641 length = len(_x)
00642 buff.write(struct.pack('<I%ss'%length, length, _x))
00643 _x = self
00644 buff.write(_struct_BI.pack(_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step))
00645 _x = self.goal.target.region.image.data
00646 length = len(_x)
00647 # - if encoded as a list instead, serialize as bytes instead of string
00648 if type(_x) in [list, tuple]:
00649 buff.write(struct.pack('<I%sB'%length, length, *_x))
00650 else:
00651 buff.write(struct.pack('<I%ss'%length, length, _x))
00652 _x = self
00653 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))
00654 _x = self.goal.target.region.disparity_image.header.frame_id
00655 length = len(_x)
00656 buff.write(struct.pack('<I%ss'%length, length, _x))
00657 _x = self
00658 buff.write(_struct_2I.pack(_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width))
00659 _x = self.goal.target.region.disparity_image.encoding
00660 length = len(_x)
00661 buff.write(struct.pack('<I%ss'%length, length, _x))
00662 _x = self
00663 buff.write(_struct_BI.pack(_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step))
00664 _x = self.goal.target.region.disparity_image.data
00665 length = len(_x)
00666 # - if encoded as a list instead, serialize as bytes instead of string
00667 if type(_x) in [list, tuple]:
00668 buff.write(struct.pack('<I%sB'%length, length, *_x))
00669 else:
00670 buff.write(struct.pack('<I%ss'%length, length, _x))
00671 _x = self
00672 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))
00673 _x = self.goal.target.region.cam_info.header.frame_id
00674 length = len(_x)
00675 buff.write(struct.pack('<I%ss'%length, length, _x))
00676 _x = self
00677 buff.write(_struct_2I.pack(_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width))
00678 _x = self.goal.target.region.cam_info.distortion_model
00679 length = len(_x)
00680 buff.write(struct.pack('<I%ss'%length, length, _x))
00681 length = len(self.goal.target.region.cam_info.D)
00682 buff.write(_struct_I.pack(length))
00683 pattern = '<%sd'%length
00684 buff.write(struct.pack(pattern, *self.goal.target.region.cam_info.D))
00685 buff.write(_struct_9d.pack(*self.goal.target.region.cam_info.K))
00686 buff.write(_struct_9d.pack(*self.goal.target.region.cam_info.R))
00687 buff.write(_struct_12d.pack(*self.goal.target.region.cam_info.P))
00688 _x = self
00689 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.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs))
00690 _x = self.goal.final_grasp_pose.header.frame_id
00691 length = len(_x)
00692 buff.write(struct.pack('<I%ss'%length, length, _x))
00693 _x = self
00694 buff.write(_struct_7d3I.pack(_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs))
00695 _x = self.goal.trajectory.header.frame_id
00696 length = len(_x)
00697 buff.write(struct.pack('<I%ss'%length, length, _x))
00698 length = len(self.goal.trajectory.joint_names)
00699 buff.write(_struct_I.pack(length))
00700 for val1 in self.goal.trajectory.joint_names:
00701 length = len(val1)
00702 buff.write(struct.pack('<I%ss'%length, length, val1))
00703 length = len(self.goal.trajectory.points)
00704 buff.write(_struct_I.pack(length))
00705 for val1 in self.goal.trajectory.points:
00706 length = len(val1.positions)
00707 buff.write(_struct_I.pack(length))
00708 pattern = '<%sd'%length
00709 buff.write(struct.pack(pattern, *val1.positions))
00710 length = len(val1.velocities)
00711 buff.write(_struct_I.pack(length))
00712 pattern = '<%sd'%length
00713 buff.write(struct.pack(pattern, *val1.velocities))
00714 length = len(val1.accelerations)
00715 buff.write(_struct_I.pack(length))
00716 pattern = '<%sd'%length
00717 buff.write(struct.pack(pattern, *val1.accelerations))
00718 _v7 = val1.time_from_start
00719 _x = _v7
00720 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00721 _x = self.goal.collision_support_surface_name
00722 length = len(_x)
00723 buff.write(struct.pack('<I%ss'%length, length, _x))
00724 _x = self
00725 buff.write(_struct_3I.pack(_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs))
00726 _x = self.goal.pre_grasp_posture.header.frame_id
00727 length = len(_x)
00728 buff.write(struct.pack('<I%ss'%length, length, _x))
00729 length = len(self.goal.pre_grasp_posture.name)
00730 buff.write(_struct_I.pack(length))
00731 for val1 in self.goal.pre_grasp_posture.name:
00732 length = len(val1)
00733 buff.write(struct.pack('<I%ss'%length, length, val1))
00734 length = len(self.goal.pre_grasp_posture.position)
00735 buff.write(_struct_I.pack(length))
00736 pattern = '<%sd'%length
00737 buff.write(struct.pack(pattern, *self.goal.pre_grasp_posture.position))
00738 length = len(self.goal.pre_grasp_posture.velocity)
00739 buff.write(_struct_I.pack(length))
00740 pattern = '<%sd'%length
00741 buff.write(struct.pack(pattern, *self.goal.pre_grasp_posture.velocity))
00742 length = len(self.goal.pre_grasp_posture.effort)
00743 buff.write(_struct_I.pack(length))
00744 pattern = '<%sd'%length
00745 buff.write(struct.pack(pattern, *self.goal.pre_grasp_posture.effort))
00746 _x = self
00747 buff.write(_struct_3I.pack(_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs))
00748 _x = self.goal.grasp_posture.header.frame_id
00749 length = len(_x)
00750 buff.write(struct.pack('<I%ss'%length, length, _x))
00751 length = len(self.goal.grasp_posture.name)
00752 buff.write(_struct_I.pack(length))
00753 for val1 in self.goal.grasp_posture.name:
00754 length = len(val1)
00755 buff.write(struct.pack('<I%ss'%length, length, val1))
00756 length = len(self.goal.grasp_posture.position)
00757 buff.write(_struct_I.pack(length))
00758 pattern = '<%sd'%length
00759 buff.write(struct.pack(pattern, *self.goal.grasp_posture.position))
00760 length = len(self.goal.grasp_posture.velocity)
00761 buff.write(_struct_I.pack(length))
00762 pattern = '<%sd'%length
00763 buff.write(struct.pack(pattern, *self.goal.grasp_posture.velocity))
00764 length = len(self.goal.grasp_posture.effort)
00765 buff.write(_struct_I.pack(length))
00766 pattern = '<%sd'%length
00767 buff.write(struct.pack(pattern, *self.goal.grasp_posture.effort))
00768 except struct.error, se: self._check_types(se)
00769 except TypeError, te: self._check_types(te)
00770
00771 def deserialize(self, str):
00772 """
00773 unpack serialized message in str into this message instance
00774 @param str: byte array of serialized message
00775 @type str: str
00776 """
00777 try:
00778 if self.header is None:
00779 self.header = std_msgs.msg._Header.Header()
00780 if self.goal_id is None:
00781 self.goal_id = actionlib_msgs.msg.GoalID()
00782 if self.goal is None:
00783 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
00784 end = 0
00785 _x = self
00786 start = end
00787 end += 12
00788 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00789 start = end
00790 end += 4
00791 (length,) = _struct_I.unpack(str[start:end])
00792 start = end
00793 end += length
00794 self.header.frame_id = str[start:end]
00795 _x = self
00796 start = end
00797 end += 8
00798 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00799 start = end
00800 end += 4
00801 (length,) = _struct_I.unpack(str[start:end])
00802 start = end
00803 end += length
00804 self.goal_id.id = str[start:end]
00805 start = end
00806 end += 4
00807 (length,) = _struct_I.unpack(str[start:end])
00808 start = end
00809 end += length
00810 self.goal.arm_name = str[start:end]
00811 start = end
00812 end += 4
00813 (length,) = _struct_I.unpack(str[start:end])
00814 start = end
00815 end += length
00816 self.goal.target.reference_frame_id = str[start:end]
00817 start = end
00818 end += 4
00819 (length,) = _struct_I.unpack(str[start:end])
00820 self.goal.target.potential_models = []
00821 for i in xrange(0, length):
00822 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
00823 start = end
00824 end += 4
00825 (val1.model_id,) = _struct_i.unpack(str[start:end])
00826 _v8 = val1.pose
00827 _v9 = _v8.header
00828 start = end
00829 end += 4
00830 (_v9.seq,) = _struct_I.unpack(str[start:end])
00831 _v10 = _v9.stamp
00832 _x = _v10
00833 start = end
00834 end += 8
00835 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00836 start = end
00837 end += 4
00838 (length,) = _struct_I.unpack(str[start:end])
00839 start = end
00840 end += length
00841 _v9.frame_id = str[start:end]
00842 _v11 = _v8.pose
00843 _v12 = _v11.position
00844 _x = _v12
00845 start = end
00846 end += 24
00847 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00848 _v13 = _v11.orientation
00849 _x = _v13
00850 start = end
00851 end += 32
00852 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00853 start = end
00854 end += 4
00855 (val1.confidence,) = _struct_f.unpack(str[start:end])
00856 self.goal.target.potential_models.append(val1)
00857 _x = self
00858 start = end
00859 end += 12
00860 (_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])
00861 start = end
00862 end += 4
00863 (length,) = _struct_I.unpack(str[start:end])
00864 start = end
00865 end += length
00866 self.goal.target.cluster.header.frame_id = str[start:end]
00867 start = end
00868 end += 4
00869 (length,) = _struct_I.unpack(str[start:end])
00870 self.goal.target.cluster.points = []
00871 for i in xrange(0, length):
00872 val1 = geometry_msgs.msg.Point32()
00873 _x = val1
00874 start = end
00875 end += 12
00876 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00877 self.goal.target.cluster.points.append(val1)
00878 start = end
00879 end += 4
00880 (length,) = _struct_I.unpack(str[start:end])
00881 self.goal.target.cluster.channels = []
00882 for i in xrange(0, length):
00883 val1 = sensor_msgs.msg.ChannelFloat32()
00884 start = end
00885 end += 4
00886 (length,) = _struct_I.unpack(str[start:end])
00887 start = end
00888 end += length
00889 val1.name = str[start:end]
00890 start = end
00891 end += 4
00892 (length,) = _struct_I.unpack(str[start:end])
00893 pattern = '<%sf'%length
00894 start = end
00895 end += struct.calcsize(pattern)
00896 val1.values = struct.unpack(pattern, str[start:end])
00897 self.goal.target.cluster.channels.append(val1)
00898 _x = self
00899 start = end
00900 end += 12
00901 (_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])
00902 start = end
00903 end += 4
00904 (length,) = _struct_I.unpack(str[start:end])
00905 start = end
00906 end += length
00907 self.goal.target.region.cloud.header.frame_id = str[start:end]
00908 _x = self
00909 start = end
00910 end += 8
00911 (_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
00912 start = end
00913 end += 4
00914 (length,) = _struct_I.unpack(str[start:end])
00915 self.goal.target.region.cloud.fields = []
00916 for i in xrange(0, length):
00917 val1 = sensor_msgs.msg.PointField()
00918 start = end
00919 end += 4
00920 (length,) = _struct_I.unpack(str[start:end])
00921 start = end
00922 end += length
00923 val1.name = str[start:end]
00924 _x = val1
00925 start = end
00926 end += 9
00927 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
00928 self.goal.target.region.cloud.fields.append(val1)
00929 _x = self
00930 start = end
00931 end += 9
00932 (_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])
00933 self.goal.target.region.cloud.is_bigendian = bool(self.goal.target.region.cloud.is_bigendian)
00934 start = end
00935 end += 4
00936 (length,) = _struct_I.unpack(str[start:end])
00937 start = end
00938 end += length
00939 self.goal.target.region.cloud.data = str[start:end]
00940 start = end
00941 end += 1
00942 (self.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
00943 self.goal.target.region.cloud.is_dense = bool(self.goal.target.region.cloud.is_dense)
00944 start = end
00945 end += 4
00946 (length,) = _struct_I.unpack(str[start:end])
00947 pattern = '<%si'%length
00948 start = end
00949 end += struct.calcsize(pattern)
00950 self.goal.target.region.mask = struct.unpack(pattern, str[start:end])
00951 _x = self
00952 start = end
00953 end += 12
00954 (_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])
00955 start = end
00956 end += 4
00957 (length,) = _struct_I.unpack(str[start:end])
00958 start = end
00959 end += length
00960 self.goal.target.region.image.header.frame_id = str[start:end]
00961 _x = self
00962 start = end
00963 end += 8
00964 (_x.goal.target.region.image.height, _x.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
00965 start = end
00966 end += 4
00967 (length,) = _struct_I.unpack(str[start:end])
00968 start = end
00969 end += length
00970 self.goal.target.region.image.encoding = str[start:end]
00971 _x = self
00972 start = end
00973 end += 5
00974 (_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
00975 start = end
00976 end += 4
00977 (length,) = _struct_I.unpack(str[start:end])
00978 start = end
00979 end += length
00980 self.goal.target.region.image.data = str[start:end]
00981 _x = self
00982 start = end
00983 end += 12
00984 (_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])
00985 start = end
00986 end += 4
00987 (length,) = _struct_I.unpack(str[start:end])
00988 start = end
00989 end += length
00990 self.goal.target.region.disparity_image.header.frame_id = str[start:end]
00991 _x = self
00992 start = end
00993 end += 8
00994 (_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
00995 start = end
00996 end += 4
00997 (length,) = _struct_I.unpack(str[start:end])
00998 start = end
00999 end += length
01000 self.goal.target.region.disparity_image.encoding = str[start:end]
01001 _x = self
01002 start = end
01003 end += 5
01004 (_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01005 start = end
01006 end += 4
01007 (length,) = _struct_I.unpack(str[start:end])
01008 start = end
01009 end += length
01010 self.goal.target.region.disparity_image.data = str[start:end]
01011 _x = self
01012 start = end
01013 end += 12
01014 (_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])
01015 start = end
01016 end += 4
01017 (length,) = _struct_I.unpack(str[start:end])
01018 start = end
01019 end += length
01020 self.goal.target.region.cam_info.header.frame_id = str[start:end]
01021 _x = self
01022 start = end
01023 end += 8
01024 (_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width,) = _struct_2I.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 self.goal.target.region.cam_info.distortion_model = str[start:end]
01031 start = end
01032 end += 4
01033 (length,) = _struct_I.unpack(str[start:end])
01034 pattern = '<%sd'%length
01035 start = end
01036 end += struct.calcsize(pattern)
01037 self.goal.target.region.cam_info.D = struct.unpack(pattern, str[start:end])
01038 start = end
01039 end += 72
01040 self.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end])
01041 start = end
01042 end += 72
01043 self.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end])
01044 start = end
01045 end += 96
01046 self.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end])
01047 _x = self
01048 start = end
01049 end += 37
01050 (_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.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01051 self.goal.target.region.cam_info.roi.do_rectify = bool(self.goal.target.region.cam_info.roi.do_rectify)
01052 start = end
01053 end += 4
01054 (length,) = _struct_I.unpack(str[start:end])
01055 start = end
01056 end += length
01057 self.goal.final_grasp_pose.header.frame_id = str[start:end]
01058 _x = self
01059 start = end
01060 end += 68
01061 (_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01062 start = end
01063 end += 4
01064 (length,) = _struct_I.unpack(str[start:end])
01065 start = end
01066 end += length
01067 self.goal.trajectory.header.frame_id = str[start:end]
01068 start = end
01069 end += 4
01070 (length,) = _struct_I.unpack(str[start:end])
01071 self.goal.trajectory.joint_names = []
01072 for i in xrange(0, length):
01073 start = end
01074 end += 4
01075 (length,) = _struct_I.unpack(str[start:end])
01076 start = end
01077 end += length
01078 val1 = str[start:end]
01079 self.goal.trajectory.joint_names.append(val1)
01080 start = end
01081 end += 4
01082 (length,) = _struct_I.unpack(str[start:end])
01083 self.goal.trajectory.points = []
01084 for i in xrange(0, length):
01085 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01086 start = end
01087 end += 4
01088 (length,) = _struct_I.unpack(str[start:end])
01089 pattern = '<%sd'%length
01090 start = end
01091 end += struct.calcsize(pattern)
01092 val1.positions = struct.unpack(pattern, str[start:end])
01093 start = end
01094 end += 4
01095 (length,) = _struct_I.unpack(str[start:end])
01096 pattern = '<%sd'%length
01097 start = end
01098 end += struct.calcsize(pattern)
01099 val1.velocities = struct.unpack(pattern, str[start:end])
01100 start = end
01101 end += 4
01102 (length,) = _struct_I.unpack(str[start:end])
01103 pattern = '<%sd'%length
01104 start = end
01105 end += struct.calcsize(pattern)
01106 val1.accelerations = struct.unpack(pattern, str[start:end])
01107 _v14 = val1.time_from_start
01108 _x = _v14
01109 start = end
01110 end += 8
01111 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01112 self.goal.trajectory.points.append(val1)
01113 start = end
01114 end += 4
01115 (length,) = _struct_I.unpack(str[start:end])
01116 start = end
01117 end += length
01118 self.goal.collision_support_surface_name = str[start:end]
01119 _x = self
01120 start = end
01121 end += 12
01122 (_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01123 start = end
01124 end += 4
01125 (length,) = _struct_I.unpack(str[start:end])
01126 start = end
01127 end += length
01128 self.goal.pre_grasp_posture.header.frame_id = str[start:end]
01129 start = end
01130 end += 4
01131 (length,) = _struct_I.unpack(str[start:end])
01132 self.goal.pre_grasp_posture.name = []
01133 for i in xrange(0, length):
01134 start = end
01135 end += 4
01136 (length,) = _struct_I.unpack(str[start:end])
01137 start = end
01138 end += length
01139 val1 = str[start:end]
01140 self.goal.pre_grasp_posture.name.append(val1)
01141 start = end
01142 end += 4
01143 (length,) = _struct_I.unpack(str[start:end])
01144 pattern = '<%sd'%length
01145 start = end
01146 end += struct.calcsize(pattern)
01147 self.goal.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01148 start = end
01149 end += 4
01150 (length,) = _struct_I.unpack(str[start:end])
01151 pattern = '<%sd'%length
01152 start = end
01153 end += struct.calcsize(pattern)
01154 self.goal.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01155 start = end
01156 end += 4
01157 (length,) = _struct_I.unpack(str[start:end])
01158 pattern = '<%sd'%length
01159 start = end
01160 end += struct.calcsize(pattern)
01161 self.goal.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01162 _x = self
01163 start = end
01164 end += 12
01165 (_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01166 start = end
01167 end += 4
01168 (length,) = _struct_I.unpack(str[start:end])
01169 start = end
01170 end += length
01171 self.goal.grasp_posture.header.frame_id = str[start:end]
01172 start = end
01173 end += 4
01174 (length,) = _struct_I.unpack(str[start:end])
01175 self.goal.grasp_posture.name = []
01176 for i in xrange(0, length):
01177 start = end
01178 end += 4
01179 (length,) = _struct_I.unpack(str[start:end])
01180 start = end
01181 end += length
01182 val1 = str[start:end]
01183 self.goal.grasp_posture.name.append(val1)
01184 start = end
01185 end += 4
01186 (length,) = _struct_I.unpack(str[start:end])
01187 pattern = '<%sd'%length
01188 start = end
01189 end += struct.calcsize(pattern)
01190 self.goal.grasp_posture.position = struct.unpack(pattern, str[start:end])
01191 start = end
01192 end += 4
01193 (length,) = _struct_I.unpack(str[start:end])
01194 pattern = '<%sd'%length
01195 start = end
01196 end += struct.calcsize(pattern)
01197 self.goal.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01198 start = end
01199 end += 4
01200 (length,) = _struct_I.unpack(str[start:end])
01201 pattern = '<%sd'%length
01202 start = end
01203 end += struct.calcsize(pattern)
01204 self.goal.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01205 return self
01206 except struct.error, e:
01207 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01208
01209
01210 def serialize_numpy(self, buff, numpy):
01211 """
01212 serialize message with numpy array types into buffer
01213 @param buff: buffer
01214 @type buff: StringIO
01215 @param numpy: numpy python module
01216 @type numpy module
01217 """
01218 try:
01219 _x = self
01220 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
01221 _x = self.header.frame_id
01222 length = len(_x)
01223 buff.write(struct.pack('<I%ss'%length, length, _x))
01224 _x = self
01225 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs))
01226 _x = self.goal_id.id
01227 length = len(_x)
01228 buff.write(struct.pack('<I%ss'%length, length, _x))
01229 _x = self.goal.arm_name
01230 length = len(_x)
01231 buff.write(struct.pack('<I%ss'%length, length, _x))
01232 _x = self.goal.target.reference_frame_id
01233 length = len(_x)
01234 buff.write(struct.pack('<I%ss'%length, length, _x))
01235 length = len(self.goal.target.potential_models)
01236 buff.write(_struct_I.pack(length))
01237 for val1 in self.goal.target.potential_models:
01238 buff.write(_struct_i.pack(val1.model_id))
01239 _v15 = val1.pose
01240 _v16 = _v15.header
01241 buff.write(_struct_I.pack(_v16.seq))
01242 _v17 = _v16.stamp
01243 _x = _v17
01244 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01245 _x = _v16.frame_id
01246 length = len(_x)
01247 buff.write(struct.pack('<I%ss'%length, length, _x))
01248 _v18 = _v15.pose
01249 _v19 = _v18.position
01250 _x = _v19
01251 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01252 _v20 = _v18.orientation
01253 _x = _v20
01254 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01255 buff.write(_struct_f.pack(val1.confidence))
01256 _x = self
01257 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))
01258 _x = self.goal.target.cluster.header.frame_id
01259 length = len(_x)
01260 buff.write(struct.pack('<I%ss'%length, length, _x))
01261 length = len(self.goal.target.cluster.points)
01262 buff.write(_struct_I.pack(length))
01263 for val1 in self.goal.target.cluster.points:
01264 _x = val1
01265 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01266 length = len(self.goal.target.cluster.channels)
01267 buff.write(_struct_I.pack(length))
01268 for val1 in self.goal.target.cluster.channels:
01269 _x = val1.name
01270 length = len(_x)
01271 buff.write(struct.pack('<I%ss'%length, length, _x))
01272 length = len(val1.values)
01273 buff.write(_struct_I.pack(length))
01274 pattern = '<%sf'%length
01275 buff.write(val1.values.tostring())
01276 _x = self
01277 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))
01278 _x = self.goal.target.region.cloud.header.frame_id
01279 length = len(_x)
01280 buff.write(struct.pack('<I%ss'%length, length, _x))
01281 _x = self
01282 buff.write(_struct_2I.pack(_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width))
01283 length = len(self.goal.target.region.cloud.fields)
01284 buff.write(_struct_I.pack(length))
01285 for val1 in self.goal.target.region.cloud.fields:
01286 _x = val1.name
01287 length = len(_x)
01288 buff.write(struct.pack('<I%ss'%length, length, _x))
01289 _x = val1
01290 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01291 _x = self
01292 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))
01293 _x = self.goal.target.region.cloud.data
01294 length = len(_x)
01295 # - if encoded as a list instead, serialize as bytes instead of string
01296 if type(_x) in [list, tuple]:
01297 buff.write(struct.pack('<I%sB'%length, length, *_x))
01298 else:
01299 buff.write(struct.pack('<I%ss'%length, length, _x))
01300 buff.write(_struct_B.pack(self.goal.target.region.cloud.is_dense))
01301 length = len(self.goal.target.region.mask)
01302 buff.write(_struct_I.pack(length))
01303 pattern = '<%si'%length
01304 buff.write(self.goal.target.region.mask.tostring())
01305 _x = self
01306 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))
01307 _x = self.goal.target.region.image.header.frame_id
01308 length = len(_x)
01309 buff.write(struct.pack('<I%ss'%length, length, _x))
01310 _x = self
01311 buff.write(_struct_2I.pack(_x.goal.target.region.image.height, _x.goal.target.region.image.width))
01312 _x = self.goal.target.region.image.encoding
01313 length = len(_x)
01314 buff.write(struct.pack('<I%ss'%length, length, _x))
01315 _x = self
01316 buff.write(_struct_BI.pack(_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step))
01317 _x = self.goal.target.region.image.data
01318 length = len(_x)
01319 # - if encoded as a list instead, serialize as bytes instead of string
01320 if type(_x) in [list, tuple]:
01321 buff.write(struct.pack('<I%sB'%length, length, *_x))
01322 else:
01323 buff.write(struct.pack('<I%ss'%length, length, _x))
01324 _x = self
01325 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))
01326 _x = self.goal.target.region.disparity_image.header.frame_id
01327 length = len(_x)
01328 buff.write(struct.pack('<I%ss'%length, length, _x))
01329 _x = self
01330 buff.write(_struct_2I.pack(_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width))
01331 _x = self.goal.target.region.disparity_image.encoding
01332 length = len(_x)
01333 buff.write(struct.pack('<I%ss'%length, length, _x))
01334 _x = self
01335 buff.write(_struct_BI.pack(_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step))
01336 _x = self.goal.target.region.disparity_image.data
01337 length = len(_x)
01338 # - if encoded as a list instead, serialize as bytes instead of string
01339 if type(_x) in [list, tuple]:
01340 buff.write(struct.pack('<I%sB'%length, length, *_x))
01341 else:
01342 buff.write(struct.pack('<I%ss'%length, length, _x))
01343 _x = self
01344 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))
01345 _x = self.goal.target.region.cam_info.header.frame_id
01346 length = len(_x)
01347 buff.write(struct.pack('<I%ss'%length, length, _x))
01348 _x = self
01349 buff.write(_struct_2I.pack(_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width))
01350 _x = self.goal.target.region.cam_info.distortion_model
01351 length = len(_x)
01352 buff.write(struct.pack('<I%ss'%length, length, _x))
01353 length = len(self.goal.target.region.cam_info.D)
01354 buff.write(_struct_I.pack(length))
01355 pattern = '<%sd'%length
01356 buff.write(self.goal.target.region.cam_info.D.tostring())
01357 buff.write(self.goal.target.region.cam_info.K.tostring())
01358 buff.write(self.goal.target.region.cam_info.R.tostring())
01359 buff.write(self.goal.target.region.cam_info.P.tostring())
01360 _x = self
01361 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.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs))
01362 _x = self.goal.final_grasp_pose.header.frame_id
01363 length = len(_x)
01364 buff.write(struct.pack('<I%ss'%length, length, _x))
01365 _x = self
01366 buff.write(_struct_7d3I.pack(_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs))
01367 _x = self.goal.trajectory.header.frame_id
01368 length = len(_x)
01369 buff.write(struct.pack('<I%ss'%length, length, _x))
01370 length = len(self.goal.trajectory.joint_names)
01371 buff.write(_struct_I.pack(length))
01372 for val1 in self.goal.trajectory.joint_names:
01373 length = len(val1)
01374 buff.write(struct.pack('<I%ss'%length, length, val1))
01375 length = len(self.goal.trajectory.points)
01376 buff.write(_struct_I.pack(length))
01377 for val1 in self.goal.trajectory.points:
01378 length = len(val1.positions)
01379 buff.write(_struct_I.pack(length))
01380 pattern = '<%sd'%length
01381 buff.write(val1.positions.tostring())
01382 length = len(val1.velocities)
01383 buff.write(_struct_I.pack(length))
01384 pattern = '<%sd'%length
01385 buff.write(val1.velocities.tostring())
01386 length = len(val1.accelerations)
01387 buff.write(_struct_I.pack(length))
01388 pattern = '<%sd'%length
01389 buff.write(val1.accelerations.tostring())
01390 _v21 = val1.time_from_start
01391 _x = _v21
01392 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01393 _x = self.goal.collision_support_surface_name
01394 length = len(_x)
01395 buff.write(struct.pack('<I%ss'%length, length, _x))
01396 _x = self
01397 buff.write(_struct_3I.pack(_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs))
01398 _x = self.goal.pre_grasp_posture.header.frame_id
01399 length = len(_x)
01400 buff.write(struct.pack('<I%ss'%length, length, _x))
01401 length = len(self.goal.pre_grasp_posture.name)
01402 buff.write(_struct_I.pack(length))
01403 for val1 in self.goal.pre_grasp_posture.name:
01404 length = len(val1)
01405 buff.write(struct.pack('<I%ss'%length, length, val1))
01406 length = len(self.goal.pre_grasp_posture.position)
01407 buff.write(_struct_I.pack(length))
01408 pattern = '<%sd'%length
01409 buff.write(self.goal.pre_grasp_posture.position.tostring())
01410 length = len(self.goal.pre_grasp_posture.velocity)
01411 buff.write(_struct_I.pack(length))
01412 pattern = '<%sd'%length
01413 buff.write(self.goal.pre_grasp_posture.velocity.tostring())
01414 length = len(self.goal.pre_grasp_posture.effort)
01415 buff.write(_struct_I.pack(length))
01416 pattern = '<%sd'%length
01417 buff.write(self.goal.pre_grasp_posture.effort.tostring())
01418 _x = self
01419 buff.write(_struct_3I.pack(_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs))
01420 _x = self.goal.grasp_posture.header.frame_id
01421 length = len(_x)
01422 buff.write(struct.pack('<I%ss'%length, length, _x))
01423 length = len(self.goal.grasp_posture.name)
01424 buff.write(_struct_I.pack(length))
01425 for val1 in self.goal.grasp_posture.name:
01426 length = len(val1)
01427 buff.write(struct.pack('<I%ss'%length, length, val1))
01428 length = len(self.goal.grasp_posture.position)
01429 buff.write(_struct_I.pack(length))
01430 pattern = '<%sd'%length
01431 buff.write(self.goal.grasp_posture.position.tostring())
01432 length = len(self.goal.grasp_posture.velocity)
01433 buff.write(_struct_I.pack(length))
01434 pattern = '<%sd'%length
01435 buff.write(self.goal.grasp_posture.velocity.tostring())
01436 length = len(self.goal.grasp_posture.effort)
01437 buff.write(_struct_I.pack(length))
01438 pattern = '<%sd'%length
01439 buff.write(self.goal.grasp_posture.effort.tostring())
01440 except struct.error, se: self._check_types(se)
01441 except TypeError, te: self._check_types(te)
01442
01443 def deserialize_numpy(self, str, numpy):
01444 """
01445 unpack serialized message in str into this message instance using numpy for array types
01446 @param str: byte array of serialized message
01447 @type str: str
01448 @param numpy: numpy python module
01449 @type numpy: module
01450 """
01451 try:
01452 if self.header is None:
01453 self.header = std_msgs.msg._Header.Header()
01454 if self.goal_id is None:
01455 self.goal_id = actionlib_msgs.msg.GoalID()
01456 if self.goal is None:
01457 self.goal = object_manipulation_msgs.msg.ReactiveGraspGoal()
01458 end = 0
01459 _x = self
01460 start = end
01461 end += 12
01462 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01463 start = end
01464 end += 4
01465 (length,) = _struct_I.unpack(str[start:end])
01466 start = end
01467 end += length
01468 self.header.frame_id = str[start:end]
01469 _x = self
01470 start = end
01471 end += 8
01472 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01473 start = end
01474 end += 4
01475 (length,) = _struct_I.unpack(str[start:end])
01476 start = end
01477 end += length
01478 self.goal_id.id = str[start:end]
01479 start = end
01480 end += 4
01481 (length,) = _struct_I.unpack(str[start:end])
01482 start = end
01483 end += length
01484 self.goal.arm_name = str[start:end]
01485 start = end
01486 end += 4
01487 (length,) = _struct_I.unpack(str[start:end])
01488 start = end
01489 end += length
01490 self.goal.target.reference_frame_id = str[start:end]
01491 start = end
01492 end += 4
01493 (length,) = _struct_I.unpack(str[start:end])
01494 self.goal.target.potential_models = []
01495 for i in xrange(0, length):
01496 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01497 start = end
01498 end += 4
01499 (val1.model_id,) = _struct_i.unpack(str[start:end])
01500 _v22 = val1.pose
01501 _v23 = _v22.header
01502 start = end
01503 end += 4
01504 (_v23.seq,) = _struct_I.unpack(str[start:end])
01505 _v24 = _v23.stamp
01506 _x = _v24
01507 start = end
01508 end += 8
01509 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01510 start = end
01511 end += 4
01512 (length,) = _struct_I.unpack(str[start:end])
01513 start = end
01514 end += length
01515 _v23.frame_id = str[start:end]
01516 _v25 = _v22.pose
01517 _v26 = _v25.position
01518 _x = _v26
01519 start = end
01520 end += 24
01521 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01522 _v27 = _v25.orientation
01523 _x = _v27
01524 start = end
01525 end += 32
01526 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01527 start = end
01528 end += 4
01529 (val1.confidence,) = _struct_f.unpack(str[start:end])
01530 self.goal.target.potential_models.append(val1)
01531 _x = self
01532 start = end
01533 end += 12
01534 (_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])
01535 start = end
01536 end += 4
01537 (length,) = _struct_I.unpack(str[start:end])
01538 start = end
01539 end += length
01540 self.goal.target.cluster.header.frame_id = str[start:end]
01541 start = end
01542 end += 4
01543 (length,) = _struct_I.unpack(str[start:end])
01544 self.goal.target.cluster.points = []
01545 for i in xrange(0, length):
01546 val1 = geometry_msgs.msg.Point32()
01547 _x = val1
01548 start = end
01549 end += 12
01550 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01551 self.goal.target.cluster.points.append(val1)
01552 start = end
01553 end += 4
01554 (length,) = _struct_I.unpack(str[start:end])
01555 self.goal.target.cluster.channels = []
01556 for i in xrange(0, length):
01557 val1 = sensor_msgs.msg.ChannelFloat32()
01558 start = end
01559 end += 4
01560 (length,) = _struct_I.unpack(str[start:end])
01561 start = end
01562 end += length
01563 val1.name = str[start:end]
01564 start = end
01565 end += 4
01566 (length,) = _struct_I.unpack(str[start:end])
01567 pattern = '<%sf'%length
01568 start = end
01569 end += struct.calcsize(pattern)
01570 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01571 self.goal.target.cluster.channels.append(val1)
01572 _x = self
01573 start = end
01574 end += 12
01575 (_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])
01576 start = end
01577 end += 4
01578 (length,) = _struct_I.unpack(str[start:end])
01579 start = end
01580 end += length
01581 self.goal.target.region.cloud.header.frame_id = str[start:end]
01582 _x = self
01583 start = end
01584 end += 8
01585 (_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01586 start = end
01587 end += 4
01588 (length,) = _struct_I.unpack(str[start:end])
01589 self.goal.target.region.cloud.fields = []
01590 for i in xrange(0, length):
01591 val1 = sensor_msgs.msg.PointField()
01592 start = end
01593 end += 4
01594 (length,) = _struct_I.unpack(str[start:end])
01595 start = end
01596 end += length
01597 val1.name = str[start:end]
01598 _x = val1
01599 start = end
01600 end += 9
01601 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01602 self.goal.target.region.cloud.fields.append(val1)
01603 _x = self
01604 start = end
01605 end += 9
01606 (_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])
01607 self.goal.target.region.cloud.is_bigendian = bool(self.goal.target.region.cloud.is_bigendian)
01608 start = end
01609 end += 4
01610 (length,) = _struct_I.unpack(str[start:end])
01611 start = end
01612 end += length
01613 self.goal.target.region.cloud.data = str[start:end]
01614 start = end
01615 end += 1
01616 (self.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01617 self.goal.target.region.cloud.is_dense = bool(self.goal.target.region.cloud.is_dense)
01618 start = end
01619 end += 4
01620 (length,) = _struct_I.unpack(str[start:end])
01621 pattern = '<%si'%length
01622 start = end
01623 end += struct.calcsize(pattern)
01624 self.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01625 _x = self
01626 start = end
01627 end += 12
01628 (_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])
01629 start = end
01630 end += 4
01631 (length,) = _struct_I.unpack(str[start:end])
01632 start = end
01633 end += length
01634 self.goal.target.region.image.header.frame_id = str[start:end]
01635 _x = self
01636 start = end
01637 end += 8
01638 (_x.goal.target.region.image.height, _x.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01639 start = end
01640 end += 4
01641 (length,) = _struct_I.unpack(str[start:end])
01642 start = end
01643 end += length
01644 self.goal.target.region.image.encoding = str[start:end]
01645 _x = self
01646 start = end
01647 end += 5
01648 (_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01649 start = end
01650 end += 4
01651 (length,) = _struct_I.unpack(str[start:end])
01652 start = end
01653 end += length
01654 self.goal.target.region.image.data = str[start:end]
01655 _x = self
01656 start = end
01657 end += 12
01658 (_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])
01659 start = end
01660 end += 4
01661 (length,) = _struct_I.unpack(str[start:end])
01662 start = end
01663 end += length
01664 self.goal.target.region.disparity_image.header.frame_id = str[start:end]
01665 _x = self
01666 start = end
01667 end += 8
01668 (_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01669 start = end
01670 end += 4
01671 (length,) = _struct_I.unpack(str[start:end])
01672 start = end
01673 end += length
01674 self.goal.target.region.disparity_image.encoding = str[start:end]
01675 _x = self
01676 start = end
01677 end += 5
01678 (_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01679 start = end
01680 end += 4
01681 (length,) = _struct_I.unpack(str[start:end])
01682 start = end
01683 end += length
01684 self.goal.target.region.disparity_image.data = str[start:end]
01685 _x = self
01686 start = end
01687 end += 12
01688 (_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])
01689 start = end
01690 end += 4
01691 (length,) = _struct_I.unpack(str[start:end])
01692 start = end
01693 end += length
01694 self.goal.target.region.cam_info.header.frame_id = str[start:end]
01695 _x = self
01696 start = end
01697 end += 8
01698 (_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01699 start = end
01700 end += 4
01701 (length,) = _struct_I.unpack(str[start:end])
01702 start = end
01703 end += length
01704 self.goal.target.region.cam_info.distortion_model = str[start:end]
01705 start = end
01706 end += 4
01707 (length,) = _struct_I.unpack(str[start:end])
01708 pattern = '<%sd'%length
01709 start = end
01710 end += struct.calcsize(pattern)
01711 self.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01712 start = end
01713 end += 72
01714 self.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01715 start = end
01716 end += 72
01717 self.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01718 start = end
01719 end += 96
01720 self.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
01721 _x = self
01722 start = end
01723 end += 37
01724 (_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.final_grasp_pose.header.seq, _x.goal.final_grasp_pose.header.stamp.secs, _x.goal.final_grasp_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01725 self.goal.target.region.cam_info.roi.do_rectify = bool(self.goal.target.region.cam_info.roi.do_rectify)
01726 start = end
01727 end += 4
01728 (length,) = _struct_I.unpack(str[start:end])
01729 start = end
01730 end += length
01731 self.goal.final_grasp_pose.header.frame_id = str[start:end]
01732 _x = self
01733 start = end
01734 end += 68
01735 (_x.goal.final_grasp_pose.pose.position.x, _x.goal.final_grasp_pose.pose.position.y, _x.goal.final_grasp_pose.pose.position.z, _x.goal.final_grasp_pose.pose.orientation.x, _x.goal.final_grasp_pose.pose.orientation.y, _x.goal.final_grasp_pose.pose.orientation.z, _x.goal.final_grasp_pose.pose.orientation.w, _x.goal.trajectory.header.seq, _x.goal.trajectory.header.stamp.secs, _x.goal.trajectory.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end])
01736 start = end
01737 end += 4
01738 (length,) = _struct_I.unpack(str[start:end])
01739 start = end
01740 end += length
01741 self.goal.trajectory.header.frame_id = str[start:end]
01742 start = end
01743 end += 4
01744 (length,) = _struct_I.unpack(str[start:end])
01745 self.goal.trajectory.joint_names = []
01746 for i in xrange(0, length):
01747 start = end
01748 end += 4
01749 (length,) = _struct_I.unpack(str[start:end])
01750 start = end
01751 end += length
01752 val1 = str[start:end]
01753 self.goal.trajectory.joint_names.append(val1)
01754 start = end
01755 end += 4
01756 (length,) = _struct_I.unpack(str[start:end])
01757 self.goal.trajectory.points = []
01758 for i in xrange(0, length):
01759 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01760 start = end
01761 end += 4
01762 (length,) = _struct_I.unpack(str[start:end])
01763 pattern = '<%sd'%length
01764 start = end
01765 end += struct.calcsize(pattern)
01766 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01767 start = end
01768 end += 4
01769 (length,) = _struct_I.unpack(str[start:end])
01770 pattern = '<%sd'%length
01771 start = end
01772 end += struct.calcsize(pattern)
01773 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01774 start = end
01775 end += 4
01776 (length,) = _struct_I.unpack(str[start:end])
01777 pattern = '<%sd'%length
01778 start = end
01779 end += struct.calcsize(pattern)
01780 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01781 _v28 = val1.time_from_start
01782 _x = _v28
01783 start = end
01784 end += 8
01785 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01786 self.goal.trajectory.points.append(val1)
01787 start = end
01788 end += 4
01789 (length,) = _struct_I.unpack(str[start:end])
01790 start = end
01791 end += length
01792 self.goal.collision_support_surface_name = str[start:end]
01793 _x = self
01794 start = end
01795 end += 12
01796 (_x.goal.pre_grasp_posture.header.seq, _x.goal.pre_grasp_posture.header.stamp.secs, _x.goal.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.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 self.goal.pre_grasp_posture.header.frame_id = str[start:end]
01803 start = end
01804 end += 4
01805 (length,) = _struct_I.unpack(str[start:end])
01806 self.goal.pre_grasp_posture.name = []
01807 for i in xrange(0, length):
01808 start = end
01809 end += 4
01810 (length,) = _struct_I.unpack(str[start:end])
01811 start = end
01812 end += length
01813 val1 = str[start:end]
01814 self.goal.pre_grasp_posture.name.append(val1)
01815 start = end
01816 end += 4
01817 (length,) = _struct_I.unpack(str[start:end])
01818 pattern = '<%sd'%length
01819 start = end
01820 end += struct.calcsize(pattern)
01821 self.goal.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01822 start = end
01823 end += 4
01824 (length,) = _struct_I.unpack(str[start:end])
01825 pattern = '<%sd'%length
01826 start = end
01827 end += struct.calcsize(pattern)
01828 self.goal.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01829 start = end
01830 end += 4
01831 (length,) = _struct_I.unpack(str[start:end])
01832 pattern = '<%sd'%length
01833 start = end
01834 end += struct.calcsize(pattern)
01835 self.goal.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01836 _x = self
01837 start = end
01838 end += 12
01839 (_x.goal.grasp_posture.header.seq, _x.goal.grasp_posture.header.stamp.secs, _x.goal.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01840 start = end
01841 end += 4
01842 (length,) = _struct_I.unpack(str[start:end])
01843 start = end
01844 end += length
01845 self.goal.grasp_posture.header.frame_id = str[start:end]
01846 start = end
01847 end += 4
01848 (length,) = _struct_I.unpack(str[start:end])
01849 self.goal.grasp_posture.name = []
01850 for i in xrange(0, length):
01851 start = end
01852 end += 4
01853 (length,) = _struct_I.unpack(str[start:end])
01854 start = end
01855 end += length
01856 val1 = str[start:end]
01857 self.goal.grasp_posture.name.append(val1)
01858 start = end
01859 end += 4
01860 (length,) = _struct_I.unpack(str[start:end])
01861 pattern = '<%sd'%length
01862 start = end
01863 end += struct.calcsize(pattern)
01864 self.goal.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01865 start = end
01866 end += 4
01867 (length,) = _struct_I.unpack(str[start:end])
01868 pattern = '<%sd'%length
01869 start = end
01870 end += struct.calcsize(pattern)
01871 self.goal.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01872 start = end
01873 end += 4
01874 (length,) = _struct_I.unpack(str[start:end])
01875 pattern = '<%sd'%length
01876 start = end
01877 end += struct.calcsize(pattern)
01878 self.goal.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01879 return self
01880 except struct.error, e:
01881 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01882
01883 _struct_I = roslib.message.struct_I
01884 _struct_7d3I = struct.Struct("<7d3I")
01885 _struct_IBI = struct.Struct("<IBI")
01886 _struct_6IB3I = struct.Struct("<6IB3I")
01887 _struct_B = struct.Struct("<B")
01888 _struct_12d = struct.Struct("<12d")
01889 _struct_f = struct.Struct("<f")
01890 _struct_i = struct.Struct("<i")
01891 _struct_BI = struct.Struct("<BI")
01892 _struct_3f = struct.Struct("<3f")
01893 _struct_2i = struct.Struct("<2i")
01894 _struct_3I = struct.Struct("<3I")
01895 _struct_9d = struct.Struct("<9d")
01896 _struct_B2I = struct.Struct("<B2I")
01897 _struct_4d = struct.Struct("<4d")
01898 _struct_2I = struct.Struct("<2I")
01899 _struct_3d = struct.Struct("<3d")