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