$search
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 #flag to mark the presence of a Header object 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")