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