$search
00001 """autogenerated by genmsg_py from RobotMeasurement.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import geometry_msgs.msg 00006 import sensor_msgs.msg 00007 import cob_calibration_msgs.msg 00008 import std_msgs.msg 00009 00010 class RobotMeasurement(roslib.message.Message): 00011 _md5sum = "48d4a626e19c4ac4e886191032058bb2" 00012 _type = "cob_calibration_msgs/RobotMeasurement" 00013 _has_header = False #flag to mark the presence of a Header object 00014 _full_text = """string sample_id # Tag to figure out which yaml file this was generated from 00015 00016 string target_id # Defines the target that we were sensing. 00017 string chain_id # Defines where this target was attached 00018 00019 CameraMeasurement[] M_cam 00020 ChainMeasurement[] M_chain 00021 00022 ================================================================================ 00023 MSG: cob_calibration_msgs/CameraMeasurement 00024 Header header 00025 string camera_id 00026 ImagePoint[] image_points 00027 sensor_msgs/CameraInfo cam_info 00028 00029 # True -> The extra debugging fields are populated 00030 bool verbose 00031 00032 # Extra, partially processed data. Only needed for debugging 00033 sensor_msgs/Image image 00034 sensor_msgs/Image image_rect 00035 cob_calibration_msgs/CalibrationPattern features 00036 00037 ================================================================================ 00038 MSG: std_msgs/Header 00039 # Standard metadata for higher-level stamped data types. 00040 # This is generally used to communicate timestamped data 00041 # in a particular coordinate frame. 00042 # 00043 # sequence ID: consecutively increasing ID 00044 uint32 seq 00045 #Two-integer timestamp that is expressed as: 00046 # * stamp.secs: seconds (stamp_secs) since epoch 00047 # * stamp.nsecs: nanoseconds since stamp_secs 00048 # time-handling sugar is provided by the client library 00049 time stamp 00050 #Frame this data is associated with 00051 # 0: no frame 00052 # 1: global frame 00053 string frame_id 00054 00055 ================================================================================ 00056 MSG: cob_calibration_msgs/ImagePoint 00057 float32 x 00058 float32 y 00059 00060 ================================================================================ 00061 MSG: sensor_msgs/CameraInfo 00062 # This message defines meta information for a camera. It should be in a 00063 # camera namespace on topic "camera_info" and accompanied by up to five 00064 # image topics named: 00065 # 00066 # image_raw - raw data from the camera driver, possibly Bayer encoded 00067 # image - monochrome, distorted 00068 # image_color - color, distorted 00069 # image_rect - monochrome, rectified 00070 # image_rect_color - color, rectified 00071 # 00072 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00073 # for producing the four processed image topics from image_raw and 00074 # camera_info. The meaning of the camera parameters are described in 00075 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00076 # 00077 # The image_geometry package provides a user-friendly interface to 00078 # common operations using this meta information. If you want to, e.g., 00079 # project a 3d point into image coordinates, we strongly recommend 00080 # using image_geometry. 00081 # 00082 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00083 # zeroed out. In particular, clients may assume that K[0] == 0.0 00084 # indicates an uncalibrated camera. 00085 00086 ####################################################################### 00087 # Image acquisition info # 00088 ####################################################################### 00089 00090 # Time of image acquisition, camera coordinate frame ID 00091 Header header # Header timestamp should be acquisition time of image 00092 # Header frame_id should be optical frame of camera 00093 # origin of frame should be optical center of camera 00094 # +x should point to the right in the image 00095 # +y should point down in the image 00096 # +z should point into the plane of the image 00097 00098 00099 ####################################################################### 00100 # Calibration Parameters # 00101 ####################################################################### 00102 # These are fixed during camera calibration. Their values will be the # 00103 # same in all messages until the camera is recalibrated. Note that # 00104 # self-calibrating systems may "recalibrate" frequently. # 00105 # # 00106 # The internal parameters can be used to warp a raw (distorted) image # 00107 # to: # 00108 # 1. An undistorted image (requires D and K) # 00109 # 2. A rectified image (requires D, K, R) # 00110 # The projection matrix P projects 3D points into the rectified image.# 00111 ####################################################################### 00112 00113 # The image dimensions with which the camera was calibrated. Normally 00114 # this will be the full camera resolution in pixels. 00115 uint32 height 00116 uint32 width 00117 00118 # The distortion model used. Supported models are listed in 00119 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00120 # simple model of radial and tangential distortion - is sufficent. 00121 string distortion_model 00122 00123 # The distortion parameters, size depending on the distortion model. 00124 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00125 float64[] D 00126 00127 # Intrinsic camera matrix for the raw (distorted) images. 00128 # [fx 0 cx] 00129 # K = [ 0 fy cy] 00130 # [ 0 0 1] 00131 # Projects 3D points in the camera coordinate frame to 2D pixel 00132 # coordinates using the focal lengths (fx, fy) and principal point 00133 # (cx, cy). 00134 float64[9] K # 3x3 row-major matrix 00135 00136 # Rectification matrix (stereo cameras only) 00137 # A rotation matrix aligning the camera coordinate system to the ideal 00138 # stereo image plane so that epipolar lines in both stereo images are 00139 # parallel. 00140 float64[9] R # 3x3 row-major matrix 00141 00142 # Projection/camera matrix 00143 # [fx' 0 cx' Tx] 00144 # P = [ 0 fy' cy' Ty] 00145 # [ 0 0 1 0] 00146 # By convention, this matrix specifies the intrinsic (camera) matrix 00147 # of the processed (rectified) image. That is, the left 3x3 portion 00148 # is the normal camera intrinsic matrix for the rectified image. 00149 # It projects 3D points in the camera coordinate frame to 2D pixel 00150 # coordinates using the focal lengths (fx', fy') and principal point 00151 # (cx', cy') - these may differ from the values in K. 00152 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00153 # also have R = the identity and P[1:3,1:3] = K. 00154 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00155 # position of the optical center of the second camera in the first 00156 # camera's frame. We assume Tz = 0 so both cameras are in the same 00157 # stereo image plane. The first camera always has Tx = Ty = 0. For 00158 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00159 # Tx = -fx' * B, where B is the baseline between the cameras. 00160 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00161 # the rectified image is given by: 00162 # [u v w]' = P * [X Y Z 1]' 00163 # x = u / w 00164 # y = v / w 00165 # This holds for both images of a stereo pair. 00166 float64[12] P # 3x4 row-major matrix 00167 00168 00169 ####################################################################### 00170 # Operational Parameters # 00171 ####################################################################### 00172 # These define the image region actually captured by the camera # 00173 # driver. Although they affect the geometry of the output image, they # 00174 # may be changed freely without recalibrating the camera. # 00175 ####################################################################### 00176 00177 # Binning refers here to any camera setting which combines rectangular 00178 # neighborhoods of pixels into larger "super-pixels." It reduces the 00179 # resolution of the output image to 00180 # (width / binning_x) x (height / binning_y). 00181 # The default values binning_x = binning_y = 0 is considered the same 00182 # as binning_x = binning_y = 1 (no subsampling). 00183 uint32 binning_x 00184 uint32 binning_y 00185 00186 # Region of interest (subwindow of full camera resolution), given in 00187 # full resolution (unbinned) image coordinates. A particular ROI 00188 # always denotes the same window of pixels on the camera sensor, 00189 # regardless of binning settings. 00190 # The default setting of roi (all values 0) is considered the same as 00191 # full resolution (roi.width = width, roi.height = height). 00192 RegionOfInterest roi 00193 00194 ================================================================================ 00195 MSG: sensor_msgs/RegionOfInterest 00196 # This message is used to specify a region of interest within an image. 00197 # 00198 # When used to specify the ROI setting of the camera when the image was 00199 # taken, the height and width fields should either match the height and 00200 # width fields for the associated image; or height = width = 0 00201 # indicates that the full resolution image was captured. 00202 00203 uint32 x_offset # Leftmost pixel of the ROI 00204 # (0 if the ROI includes the left edge of the image) 00205 uint32 y_offset # Topmost pixel of the ROI 00206 # (0 if the ROI includes the top edge of the image) 00207 uint32 height # Height of ROI 00208 uint32 width # Width of ROI 00209 00210 # True if a distinct rectified ROI should be calculated from the "raw" 00211 # ROI in this message. Typically this should be False if the full image 00212 # is captured (ROI not used), and True if a subwindow is captured (ROI 00213 # used). 00214 bool do_rectify 00215 00216 ================================================================================ 00217 MSG: sensor_msgs/Image 00218 # This message contains an uncompressed image 00219 # (0, 0) is at top-left corner of image 00220 # 00221 00222 Header header # Header timestamp should be acquisition time of image 00223 # Header frame_id should be optical frame of camera 00224 # origin of frame should be optical center of cameara 00225 # +x should point to the right in the image 00226 # +y should point down in the image 00227 # +z should point into to plane of the image 00228 # If the frame_id here and the frame_id of the CameraInfo 00229 # message associated with the image conflict 00230 # the behavior is undefined 00231 00232 uint32 height # image height, that is, number of rows 00233 uint32 width # image width, that is, number of columns 00234 00235 # The legal values for encoding are in file src/image_encodings.cpp 00236 # If you want to standardize a new string format, join 00237 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00238 00239 string encoding # Encoding of pixels -- channel meaning, ordering, size 00240 # taken from the list of strings in src/image_encodings.cpp 00241 00242 uint8 is_bigendian # is this data bigendian? 00243 uint32 step # Full row length in bytes 00244 uint8[] data # actual matrix data, size is (step * rows) 00245 00246 ================================================================================ 00247 MSG: cob_calibration_msgs/CalibrationPattern 00248 Header header 00249 geometry_msgs/Point32[] object_points 00250 ImagePoint[] image_points 00251 uint8 success 00252 00253 ================================================================================ 00254 MSG: geometry_msgs/Point32 00255 # This contains the position of a point in free space(with 32 bits of precision). 00256 # It is recommeded to use Point wherever possible instead of Point32. 00257 # 00258 # This recommendation is to promote interoperability. 00259 # 00260 # This message is designed to take up less space when sending 00261 # lots of points at once, as in the case of a PointCloud. 00262 00263 float32 x 00264 float32 y 00265 float32 z 00266 ================================================================================ 00267 MSG: cob_calibration_msgs/ChainMeasurement 00268 Header header 00269 string chain_id 00270 sensor_msgs/JointState chain_state 00271 00272 ================================================================================ 00273 MSG: sensor_msgs/JointState 00274 # This is a message that holds data to describe the state of a set of torque controlled joints. 00275 # 00276 # The state of each joint (revolute or prismatic) is defined by: 00277 # * the position of the joint (rad or m), 00278 # * the velocity of the joint (rad/s or m/s) and 00279 # * the effort that is applied in the joint (Nm or N). 00280 # 00281 # Each joint is uniquely identified by its name 00282 # The header specifies the time at which the joint states were recorded. All the joint states 00283 # in one message have to be recorded at the same time. 00284 # 00285 # This message consists of a multiple arrays, one for each part of the joint state. 00286 # The goal is to make each of the fields optional. When e.g. your joints have no 00287 # effort associated with them, you can leave the effort array empty. 00288 # 00289 # All arrays in this message should have the same size, or be empty. 00290 # This is the only way to uniquely associate the joint name with the correct 00291 # states. 00292 00293 00294 Header header 00295 00296 string[] name 00297 float64[] position 00298 float64[] velocity 00299 float64[] effort 00300 00301 """ 00302 __slots__ = ['sample_id','target_id','chain_id','M_cam','M_chain'] 00303 _slot_types = ['string','string','string','cob_calibration_msgs/CameraMeasurement[]','cob_calibration_msgs/ChainMeasurement[]'] 00304 00305 def __init__(self, *args, **kwds): 00306 """ 00307 Constructor. Any message fields that are implicitly/explicitly 00308 set to None will be assigned a default value. The recommend 00309 use is keyword arguments as this is more robust to future message 00310 changes. You cannot mix in-order arguments and keyword arguments. 00311 00312 The available fields are: 00313 sample_id,target_id,chain_id,M_cam,M_chain 00314 00315 @param args: complete set of field values, in .msg order 00316 @param kwds: use keyword arguments corresponding to message field names 00317 to set specific fields. 00318 """ 00319 if args or kwds: 00320 super(RobotMeasurement, self).__init__(*args, **kwds) 00321 #message fields cannot be None, assign default values for those that are 00322 if self.sample_id is None: 00323 self.sample_id = '' 00324 if self.target_id is None: 00325 self.target_id = '' 00326 if self.chain_id is None: 00327 self.chain_id = '' 00328 if self.M_cam is None: 00329 self.M_cam = [] 00330 if self.M_chain is None: 00331 self.M_chain = [] 00332 else: 00333 self.sample_id = '' 00334 self.target_id = '' 00335 self.chain_id = '' 00336 self.M_cam = [] 00337 self.M_chain = [] 00338 00339 def _get_types(self): 00340 """ 00341 internal API method 00342 """ 00343 return self._slot_types 00344 00345 def serialize(self, buff): 00346 """ 00347 serialize message into buffer 00348 @param buff: buffer 00349 @type buff: StringIO 00350 """ 00351 try: 00352 _x = self.sample_id 00353 length = len(_x) 00354 buff.write(struct.pack('<I%ss'%length, length, _x)) 00355 _x = self.target_id 00356 length = len(_x) 00357 buff.write(struct.pack('<I%ss'%length, length, _x)) 00358 _x = self.chain_id 00359 length = len(_x) 00360 buff.write(struct.pack('<I%ss'%length, length, _x)) 00361 length = len(self.M_cam) 00362 buff.write(_struct_I.pack(length)) 00363 for val1 in self.M_cam: 00364 _v1 = val1.header 00365 buff.write(_struct_I.pack(_v1.seq)) 00366 _v2 = _v1.stamp 00367 _x = _v2 00368 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00369 _x = _v1.frame_id 00370 length = len(_x) 00371 buff.write(struct.pack('<I%ss'%length, length, _x)) 00372 _x = val1.camera_id 00373 length = len(_x) 00374 buff.write(struct.pack('<I%ss'%length, length, _x)) 00375 length = len(val1.image_points) 00376 buff.write(_struct_I.pack(length)) 00377 for val2 in val1.image_points: 00378 _x = val2 00379 buff.write(_struct_2f.pack(_x.x, _x.y)) 00380 _v3 = val1.cam_info 00381 _v4 = _v3.header 00382 buff.write(_struct_I.pack(_v4.seq)) 00383 _v5 = _v4.stamp 00384 _x = _v5 00385 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00386 _x = _v4.frame_id 00387 length = len(_x) 00388 buff.write(struct.pack('<I%ss'%length, length, _x)) 00389 _x = _v3 00390 buff.write(_struct_2I.pack(_x.height, _x.width)) 00391 _x = _v3.distortion_model 00392 length = len(_x) 00393 buff.write(struct.pack('<I%ss'%length, length, _x)) 00394 length = len(_v3.D) 00395 buff.write(_struct_I.pack(length)) 00396 pattern = '<%sd'%length 00397 buff.write(struct.pack(pattern, *_v3.D)) 00398 buff.write(_struct_9d.pack(*_v3.K)) 00399 buff.write(_struct_9d.pack(*_v3.R)) 00400 buff.write(_struct_12d.pack(*_v3.P)) 00401 _x = _v3 00402 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00403 _v6 = _v3.roi 00404 _x = _v6 00405 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00406 buff.write(_struct_B.pack(val1.verbose)) 00407 _v7 = val1.image 00408 _v8 = _v7.header 00409 buff.write(_struct_I.pack(_v8.seq)) 00410 _v9 = _v8.stamp 00411 _x = _v9 00412 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00413 _x = _v8.frame_id 00414 length = len(_x) 00415 buff.write(struct.pack('<I%ss'%length, length, _x)) 00416 _x = _v7 00417 buff.write(_struct_2I.pack(_x.height, _x.width)) 00418 _x = _v7.encoding 00419 length = len(_x) 00420 buff.write(struct.pack('<I%ss'%length, length, _x)) 00421 _x = _v7 00422 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00423 _x = _v7.data 00424 length = len(_x) 00425 # - if encoded as a list instead, serialize as bytes instead of string 00426 if type(_x) in [list, tuple]: 00427 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00428 else: 00429 buff.write(struct.pack('<I%ss'%length, length, _x)) 00430 _v10 = val1.image_rect 00431 _v11 = _v10.header 00432 buff.write(_struct_I.pack(_v11.seq)) 00433 _v12 = _v11.stamp 00434 _x = _v12 00435 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00436 _x = _v11.frame_id 00437 length = len(_x) 00438 buff.write(struct.pack('<I%ss'%length, length, _x)) 00439 _x = _v10 00440 buff.write(_struct_2I.pack(_x.height, _x.width)) 00441 _x = _v10.encoding 00442 length = len(_x) 00443 buff.write(struct.pack('<I%ss'%length, length, _x)) 00444 _x = _v10 00445 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00446 _x = _v10.data 00447 length = len(_x) 00448 # - if encoded as a list instead, serialize as bytes instead of string 00449 if type(_x) in [list, tuple]: 00450 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00451 else: 00452 buff.write(struct.pack('<I%ss'%length, length, _x)) 00453 _v13 = val1.features 00454 _v14 = _v13.header 00455 buff.write(_struct_I.pack(_v14.seq)) 00456 _v15 = _v14.stamp 00457 _x = _v15 00458 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00459 _x = _v14.frame_id 00460 length = len(_x) 00461 buff.write(struct.pack('<I%ss'%length, length, _x)) 00462 length = len(_v13.object_points) 00463 buff.write(_struct_I.pack(length)) 00464 for val3 in _v13.object_points: 00465 _x = val3 00466 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00467 length = len(_v13.image_points) 00468 buff.write(_struct_I.pack(length)) 00469 for val3 in _v13.image_points: 00470 _x = val3 00471 buff.write(_struct_2f.pack(_x.x, _x.y)) 00472 buff.write(_struct_B.pack(_v13.success)) 00473 length = len(self.M_chain) 00474 buff.write(_struct_I.pack(length)) 00475 for val1 in self.M_chain: 00476 _v16 = val1.header 00477 buff.write(_struct_I.pack(_v16.seq)) 00478 _v17 = _v16.stamp 00479 _x = _v17 00480 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00481 _x = _v16.frame_id 00482 length = len(_x) 00483 buff.write(struct.pack('<I%ss'%length, length, _x)) 00484 _x = val1.chain_id 00485 length = len(_x) 00486 buff.write(struct.pack('<I%ss'%length, length, _x)) 00487 _v18 = val1.chain_state 00488 _v19 = _v18.header 00489 buff.write(_struct_I.pack(_v19.seq)) 00490 _v20 = _v19.stamp 00491 _x = _v20 00492 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00493 _x = _v19.frame_id 00494 length = len(_x) 00495 buff.write(struct.pack('<I%ss'%length, length, _x)) 00496 length = len(_v18.name) 00497 buff.write(_struct_I.pack(length)) 00498 for val3 in _v18.name: 00499 length = len(val3) 00500 buff.write(struct.pack('<I%ss'%length, length, val3)) 00501 length = len(_v18.position) 00502 buff.write(_struct_I.pack(length)) 00503 pattern = '<%sd'%length 00504 buff.write(struct.pack(pattern, *_v18.position)) 00505 length = len(_v18.velocity) 00506 buff.write(_struct_I.pack(length)) 00507 pattern = '<%sd'%length 00508 buff.write(struct.pack(pattern, *_v18.velocity)) 00509 length = len(_v18.effort) 00510 buff.write(_struct_I.pack(length)) 00511 pattern = '<%sd'%length 00512 buff.write(struct.pack(pattern, *_v18.effort)) 00513 except struct.error as se: self._check_types(se) 00514 except TypeError as te: self._check_types(te) 00515 00516 def deserialize(self, str): 00517 """ 00518 unpack serialized message in str into this message instance 00519 @param str: byte array of serialized message 00520 @type str: str 00521 """ 00522 try: 00523 end = 0 00524 start = end 00525 end += 4 00526 (length,) = _struct_I.unpack(str[start:end]) 00527 start = end 00528 end += length 00529 self.sample_id = str[start:end] 00530 start = end 00531 end += 4 00532 (length,) = _struct_I.unpack(str[start:end]) 00533 start = end 00534 end += length 00535 self.target_id = str[start:end] 00536 start = end 00537 end += 4 00538 (length,) = _struct_I.unpack(str[start:end]) 00539 start = end 00540 end += length 00541 self.chain_id = str[start:end] 00542 start = end 00543 end += 4 00544 (length,) = _struct_I.unpack(str[start:end]) 00545 self.M_cam = [] 00546 for i in range(0, length): 00547 val1 = cob_calibration_msgs.msg.CameraMeasurement() 00548 _v21 = val1.header 00549 start = end 00550 end += 4 00551 (_v21.seq,) = _struct_I.unpack(str[start:end]) 00552 _v22 = _v21.stamp 00553 _x = _v22 00554 start = end 00555 end += 8 00556 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00557 start = end 00558 end += 4 00559 (length,) = _struct_I.unpack(str[start:end]) 00560 start = end 00561 end += length 00562 _v21.frame_id = str[start:end] 00563 start = end 00564 end += 4 00565 (length,) = _struct_I.unpack(str[start:end]) 00566 start = end 00567 end += length 00568 val1.camera_id = str[start:end] 00569 start = end 00570 end += 4 00571 (length,) = _struct_I.unpack(str[start:end]) 00572 val1.image_points = [] 00573 for i in range(0, length): 00574 val2 = cob_calibration_msgs.msg.ImagePoint() 00575 _x = val2 00576 start = end 00577 end += 8 00578 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 00579 val1.image_points.append(val2) 00580 _v23 = val1.cam_info 00581 _v24 = _v23.header 00582 start = end 00583 end += 4 00584 (_v24.seq,) = _struct_I.unpack(str[start:end]) 00585 _v25 = _v24.stamp 00586 _x = _v25 00587 start = end 00588 end += 8 00589 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00590 start = end 00591 end += 4 00592 (length,) = _struct_I.unpack(str[start:end]) 00593 start = end 00594 end += length 00595 _v24.frame_id = str[start:end] 00596 _x = _v23 00597 start = end 00598 end += 8 00599 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 00600 start = end 00601 end += 4 00602 (length,) = _struct_I.unpack(str[start:end]) 00603 start = end 00604 end += length 00605 _v23.distortion_model = str[start:end] 00606 start = end 00607 end += 4 00608 (length,) = _struct_I.unpack(str[start:end]) 00609 pattern = '<%sd'%length 00610 start = end 00611 end += struct.calcsize(pattern) 00612 _v23.D = struct.unpack(pattern, str[start:end]) 00613 start = end 00614 end += 72 00615 _v23.K = _struct_9d.unpack(str[start:end]) 00616 start = end 00617 end += 72 00618 _v23.R = _struct_9d.unpack(str[start:end]) 00619 start = end 00620 end += 96 00621 _v23.P = _struct_12d.unpack(str[start:end]) 00622 _x = _v23 00623 start = end 00624 end += 8 00625 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 00626 _v26 = _v23.roi 00627 _x = _v26 00628 start = end 00629 end += 17 00630 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 00631 _v26.do_rectify = bool(_v26.do_rectify) 00632 start = end 00633 end += 1 00634 (val1.verbose,) = _struct_B.unpack(str[start:end]) 00635 val1.verbose = bool(val1.verbose) 00636 _v27 = val1.image 00637 _v28 = _v27.header 00638 start = end 00639 end += 4 00640 (_v28.seq,) = _struct_I.unpack(str[start:end]) 00641 _v29 = _v28.stamp 00642 _x = _v29 00643 start = end 00644 end += 8 00645 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00646 start = end 00647 end += 4 00648 (length,) = _struct_I.unpack(str[start:end]) 00649 start = end 00650 end += length 00651 _v28.frame_id = str[start:end] 00652 _x = _v27 00653 start = end 00654 end += 8 00655 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 00656 start = end 00657 end += 4 00658 (length,) = _struct_I.unpack(str[start:end]) 00659 start = end 00660 end += length 00661 _v27.encoding = str[start:end] 00662 _x = _v27 00663 start = end 00664 end += 5 00665 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 00666 start = end 00667 end += 4 00668 (length,) = _struct_I.unpack(str[start:end]) 00669 start = end 00670 end += length 00671 _v27.data = str[start:end] 00672 _v30 = val1.image_rect 00673 _v31 = _v30.header 00674 start = end 00675 end += 4 00676 (_v31.seq,) = _struct_I.unpack(str[start:end]) 00677 _v32 = _v31.stamp 00678 _x = _v32 00679 start = end 00680 end += 8 00681 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00682 start = end 00683 end += 4 00684 (length,) = _struct_I.unpack(str[start:end]) 00685 start = end 00686 end += length 00687 _v31.frame_id = str[start:end] 00688 _x = _v30 00689 start = end 00690 end += 8 00691 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 00692 start = end 00693 end += 4 00694 (length,) = _struct_I.unpack(str[start:end]) 00695 start = end 00696 end += length 00697 _v30.encoding = str[start:end] 00698 _x = _v30 00699 start = end 00700 end += 5 00701 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 00702 start = end 00703 end += 4 00704 (length,) = _struct_I.unpack(str[start:end]) 00705 start = end 00706 end += length 00707 _v30.data = str[start:end] 00708 _v33 = val1.features 00709 _v34 = _v33.header 00710 start = end 00711 end += 4 00712 (_v34.seq,) = _struct_I.unpack(str[start:end]) 00713 _v35 = _v34.stamp 00714 _x = _v35 00715 start = end 00716 end += 8 00717 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00718 start = end 00719 end += 4 00720 (length,) = _struct_I.unpack(str[start:end]) 00721 start = end 00722 end += length 00723 _v34.frame_id = str[start:end] 00724 start = end 00725 end += 4 00726 (length,) = _struct_I.unpack(str[start:end]) 00727 _v33.object_points = [] 00728 for i in range(0, length): 00729 val3 = geometry_msgs.msg.Point32() 00730 _x = val3 00731 start = end 00732 end += 12 00733 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00734 _v33.object_points.append(val3) 00735 start = end 00736 end += 4 00737 (length,) = _struct_I.unpack(str[start:end]) 00738 _v33.image_points = [] 00739 for i in range(0, length): 00740 val3 = cob_calibration_msgs.msg.ImagePoint() 00741 _x = val3 00742 start = end 00743 end += 8 00744 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 00745 _v33.image_points.append(val3) 00746 start = end 00747 end += 1 00748 (_v33.success,) = _struct_B.unpack(str[start:end]) 00749 self.M_cam.append(val1) 00750 start = end 00751 end += 4 00752 (length,) = _struct_I.unpack(str[start:end]) 00753 self.M_chain = [] 00754 for i in range(0, length): 00755 val1 = cob_calibration_msgs.msg.ChainMeasurement() 00756 _v36 = val1.header 00757 start = end 00758 end += 4 00759 (_v36.seq,) = _struct_I.unpack(str[start:end]) 00760 _v37 = _v36.stamp 00761 _x = _v37 00762 start = end 00763 end += 8 00764 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00765 start = end 00766 end += 4 00767 (length,) = _struct_I.unpack(str[start:end]) 00768 start = end 00769 end += length 00770 _v36.frame_id = str[start:end] 00771 start = end 00772 end += 4 00773 (length,) = _struct_I.unpack(str[start:end]) 00774 start = end 00775 end += length 00776 val1.chain_id = str[start:end] 00777 _v38 = val1.chain_state 00778 _v39 = _v38.header 00779 start = end 00780 end += 4 00781 (_v39.seq,) = _struct_I.unpack(str[start:end]) 00782 _v40 = _v39.stamp 00783 _x = _v40 00784 start = end 00785 end += 8 00786 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00787 start = end 00788 end += 4 00789 (length,) = _struct_I.unpack(str[start:end]) 00790 start = end 00791 end += length 00792 _v39.frame_id = str[start:end] 00793 start = end 00794 end += 4 00795 (length,) = _struct_I.unpack(str[start:end]) 00796 _v38.name = [] 00797 for i in range(0, length): 00798 start = end 00799 end += 4 00800 (length,) = _struct_I.unpack(str[start:end]) 00801 start = end 00802 end += length 00803 val3 = str[start:end] 00804 _v38.name.append(val3) 00805 start = end 00806 end += 4 00807 (length,) = _struct_I.unpack(str[start:end]) 00808 pattern = '<%sd'%length 00809 start = end 00810 end += struct.calcsize(pattern) 00811 _v38.position = struct.unpack(pattern, str[start:end]) 00812 start = end 00813 end += 4 00814 (length,) = _struct_I.unpack(str[start:end]) 00815 pattern = '<%sd'%length 00816 start = end 00817 end += struct.calcsize(pattern) 00818 _v38.velocity = struct.unpack(pattern, str[start:end]) 00819 start = end 00820 end += 4 00821 (length,) = _struct_I.unpack(str[start:end]) 00822 pattern = '<%sd'%length 00823 start = end 00824 end += struct.calcsize(pattern) 00825 _v38.effort = struct.unpack(pattern, str[start:end]) 00826 self.M_chain.append(val1) 00827 return self 00828 except struct.error as e: 00829 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00830 00831 00832 def serialize_numpy(self, buff, numpy): 00833 """ 00834 serialize message with numpy array types into buffer 00835 @param buff: buffer 00836 @type buff: StringIO 00837 @param numpy: numpy python module 00838 @type numpy module 00839 """ 00840 try: 00841 _x = self.sample_id 00842 length = len(_x) 00843 buff.write(struct.pack('<I%ss'%length, length, _x)) 00844 _x = self.target_id 00845 length = len(_x) 00846 buff.write(struct.pack('<I%ss'%length, length, _x)) 00847 _x = self.chain_id 00848 length = len(_x) 00849 buff.write(struct.pack('<I%ss'%length, length, _x)) 00850 length = len(self.M_cam) 00851 buff.write(_struct_I.pack(length)) 00852 for val1 in self.M_cam: 00853 _v41 = val1.header 00854 buff.write(_struct_I.pack(_v41.seq)) 00855 _v42 = _v41.stamp 00856 _x = _v42 00857 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00858 _x = _v41.frame_id 00859 length = len(_x) 00860 buff.write(struct.pack('<I%ss'%length, length, _x)) 00861 _x = val1.camera_id 00862 length = len(_x) 00863 buff.write(struct.pack('<I%ss'%length, length, _x)) 00864 length = len(val1.image_points) 00865 buff.write(_struct_I.pack(length)) 00866 for val2 in val1.image_points: 00867 _x = val2 00868 buff.write(_struct_2f.pack(_x.x, _x.y)) 00869 _v43 = val1.cam_info 00870 _v44 = _v43.header 00871 buff.write(_struct_I.pack(_v44.seq)) 00872 _v45 = _v44.stamp 00873 _x = _v45 00874 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00875 _x = _v44.frame_id 00876 length = len(_x) 00877 buff.write(struct.pack('<I%ss'%length, length, _x)) 00878 _x = _v43 00879 buff.write(_struct_2I.pack(_x.height, _x.width)) 00880 _x = _v43.distortion_model 00881 length = len(_x) 00882 buff.write(struct.pack('<I%ss'%length, length, _x)) 00883 length = len(_v43.D) 00884 buff.write(_struct_I.pack(length)) 00885 pattern = '<%sd'%length 00886 buff.write(_v43.D.tostring()) 00887 buff.write(_v43.K.tostring()) 00888 buff.write(_v43.R.tostring()) 00889 buff.write(_v43.P.tostring()) 00890 _x = _v43 00891 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 00892 _v46 = _v43.roi 00893 _x = _v46 00894 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 00895 buff.write(_struct_B.pack(val1.verbose)) 00896 _v47 = val1.image 00897 _v48 = _v47.header 00898 buff.write(_struct_I.pack(_v48.seq)) 00899 _v49 = _v48.stamp 00900 _x = _v49 00901 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00902 _x = _v48.frame_id 00903 length = len(_x) 00904 buff.write(struct.pack('<I%ss'%length, length, _x)) 00905 _x = _v47 00906 buff.write(_struct_2I.pack(_x.height, _x.width)) 00907 _x = _v47.encoding 00908 length = len(_x) 00909 buff.write(struct.pack('<I%ss'%length, length, _x)) 00910 _x = _v47 00911 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00912 _x = _v47.data 00913 length = len(_x) 00914 # - if encoded as a list instead, serialize as bytes instead of string 00915 if type(_x) in [list, tuple]: 00916 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00917 else: 00918 buff.write(struct.pack('<I%ss'%length, length, _x)) 00919 _v50 = val1.image_rect 00920 _v51 = _v50.header 00921 buff.write(_struct_I.pack(_v51.seq)) 00922 _v52 = _v51.stamp 00923 _x = _v52 00924 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00925 _x = _v51.frame_id 00926 length = len(_x) 00927 buff.write(struct.pack('<I%ss'%length, length, _x)) 00928 _x = _v50 00929 buff.write(_struct_2I.pack(_x.height, _x.width)) 00930 _x = _v50.encoding 00931 length = len(_x) 00932 buff.write(struct.pack('<I%ss'%length, length, _x)) 00933 _x = _v50 00934 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 00935 _x = _v50.data 00936 length = len(_x) 00937 # - if encoded as a list instead, serialize as bytes instead of string 00938 if type(_x) in [list, tuple]: 00939 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00940 else: 00941 buff.write(struct.pack('<I%ss'%length, length, _x)) 00942 _v53 = val1.features 00943 _v54 = _v53.header 00944 buff.write(_struct_I.pack(_v54.seq)) 00945 _v55 = _v54.stamp 00946 _x = _v55 00947 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00948 _x = _v54.frame_id 00949 length = len(_x) 00950 buff.write(struct.pack('<I%ss'%length, length, _x)) 00951 length = len(_v53.object_points) 00952 buff.write(_struct_I.pack(length)) 00953 for val3 in _v53.object_points: 00954 _x = val3 00955 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00956 length = len(_v53.image_points) 00957 buff.write(_struct_I.pack(length)) 00958 for val3 in _v53.image_points: 00959 _x = val3 00960 buff.write(_struct_2f.pack(_x.x, _x.y)) 00961 buff.write(_struct_B.pack(_v53.success)) 00962 length = len(self.M_chain) 00963 buff.write(_struct_I.pack(length)) 00964 for val1 in self.M_chain: 00965 _v56 = val1.header 00966 buff.write(_struct_I.pack(_v56.seq)) 00967 _v57 = _v56.stamp 00968 _x = _v57 00969 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00970 _x = _v56.frame_id 00971 length = len(_x) 00972 buff.write(struct.pack('<I%ss'%length, length, _x)) 00973 _x = val1.chain_id 00974 length = len(_x) 00975 buff.write(struct.pack('<I%ss'%length, length, _x)) 00976 _v58 = val1.chain_state 00977 _v59 = _v58.header 00978 buff.write(_struct_I.pack(_v59.seq)) 00979 _v60 = _v59.stamp 00980 _x = _v60 00981 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00982 _x = _v59.frame_id 00983 length = len(_x) 00984 buff.write(struct.pack('<I%ss'%length, length, _x)) 00985 length = len(_v58.name) 00986 buff.write(_struct_I.pack(length)) 00987 for val3 in _v58.name: 00988 length = len(val3) 00989 buff.write(struct.pack('<I%ss'%length, length, val3)) 00990 length = len(_v58.position) 00991 buff.write(_struct_I.pack(length)) 00992 pattern = '<%sd'%length 00993 buff.write(_v58.position.tostring()) 00994 length = len(_v58.velocity) 00995 buff.write(_struct_I.pack(length)) 00996 pattern = '<%sd'%length 00997 buff.write(_v58.velocity.tostring()) 00998 length = len(_v58.effort) 00999 buff.write(_struct_I.pack(length)) 01000 pattern = '<%sd'%length 01001 buff.write(_v58.effort.tostring()) 01002 except struct.error as se: self._check_types(se) 01003 except TypeError as te: self._check_types(te) 01004 01005 def deserialize_numpy(self, str, numpy): 01006 """ 01007 unpack serialized message in str into this message instance using numpy for array types 01008 @param str: byte array of serialized message 01009 @type str: str 01010 @param numpy: numpy python module 01011 @type numpy: module 01012 """ 01013 try: 01014 end = 0 01015 start = end 01016 end += 4 01017 (length,) = _struct_I.unpack(str[start:end]) 01018 start = end 01019 end += length 01020 self.sample_id = str[start:end] 01021 start = end 01022 end += 4 01023 (length,) = _struct_I.unpack(str[start:end]) 01024 start = end 01025 end += length 01026 self.target_id = str[start:end] 01027 start = end 01028 end += 4 01029 (length,) = _struct_I.unpack(str[start:end]) 01030 start = end 01031 end += length 01032 self.chain_id = str[start:end] 01033 start = end 01034 end += 4 01035 (length,) = _struct_I.unpack(str[start:end]) 01036 self.M_cam = [] 01037 for i in range(0, length): 01038 val1 = cob_calibration_msgs.msg.CameraMeasurement() 01039 _v61 = val1.header 01040 start = end 01041 end += 4 01042 (_v61.seq,) = _struct_I.unpack(str[start:end]) 01043 _v62 = _v61.stamp 01044 _x = _v62 01045 start = end 01046 end += 8 01047 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01048 start = end 01049 end += 4 01050 (length,) = _struct_I.unpack(str[start:end]) 01051 start = end 01052 end += length 01053 _v61.frame_id = str[start:end] 01054 start = end 01055 end += 4 01056 (length,) = _struct_I.unpack(str[start:end]) 01057 start = end 01058 end += length 01059 val1.camera_id = str[start:end] 01060 start = end 01061 end += 4 01062 (length,) = _struct_I.unpack(str[start:end]) 01063 val1.image_points = [] 01064 for i in range(0, length): 01065 val2 = cob_calibration_msgs.msg.ImagePoint() 01066 _x = val2 01067 start = end 01068 end += 8 01069 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 01070 val1.image_points.append(val2) 01071 _v63 = val1.cam_info 01072 _v64 = _v63.header 01073 start = end 01074 end += 4 01075 (_v64.seq,) = _struct_I.unpack(str[start:end]) 01076 _v65 = _v64.stamp 01077 _x = _v65 01078 start = end 01079 end += 8 01080 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01081 start = end 01082 end += 4 01083 (length,) = _struct_I.unpack(str[start:end]) 01084 start = end 01085 end += length 01086 _v64.frame_id = str[start:end] 01087 _x = _v63 01088 start = end 01089 end += 8 01090 (_x.height, _x.width,) = _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 _v63.distortion_model = str[start:end] 01097 start = end 01098 end += 4 01099 (length,) = _struct_I.unpack(str[start:end]) 01100 pattern = '<%sd'%length 01101 start = end 01102 end += struct.calcsize(pattern) 01103 _v63.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01104 start = end 01105 end += 72 01106 _v63.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 01107 start = end 01108 end += 72 01109 _v63.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 01110 start = end 01111 end += 96 01112 _v63.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 01113 _x = _v63 01114 start = end 01115 end += 8 01116 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01117 _v66 = _v63.roi 01118 _x = _v66 01119 start = end 01120 end += 17 01121 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01122 _v66.do_rectify = bool(_v66.do_rectify) 01123 start = end 01124 end += 1 01125 (val1.verbose,) = _struct_B.unpack(str[start:end]) 01126 val1.verbose = bool(val1.verbose) 01127 _v67 = val1.image 01128 _v68 = _v67.header 01129 start = end 01130 end += 4 01131 (_v68.seq,) = _struct_I.unpack(str[start:end]) 01132 _v69 = _v68.stamp 01133 _x = _v69 01134 start = end 01135 end += 8 01136 (_x.secs, _x.nsecs,) = _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 _v68.frame_id = str[start:end] 01143 _x = _v67 01144 start = end 01145 end += 8 01146 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01147 start = end 01148 end += 4 01149 (length,) = _struct_I.unpack(str[start:end]) 01150 start = end 01151 end += length 01152 _v67.encoding = str[start:end] 01153 _x = _v67 01154 start = end 01155 end += 5 01156 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01157 start = end 01158 end += 4 01159 (length,) = _struct_I.unpack(str[start:end]) 01160 start = end 01161 end += length 01162 _v67.data = str[start:end] 01163 _v70 = val1.image_rect 01164 _v71 = _v70.header 01165 start = end 01166 end += 4 01167 (_v71.seq,) = _struct_I.unpack(str[start:end]) 01168 _v72 = _v71.stamp 01169 _x = _v72 01170 start = end 01171 end += 8 01172 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01173 start = end 01174 end += 4 01175 (length,) = _struct_I.unpack(str[start:end]) 01176 start = end 01177 end += length 01178 _v71.frame_id = str[start:end] 01179 _x = _v70 01180 start = end 01181 end += 8 01182 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01183 start = end 01184 end += 4 01185 (length,) = _struct_I.unpack(str[start:end]) 01186 start = end 01187 end += length 01188 _v70.encoding = str[start:end] 01189 _x = _v70 01190 start = end 01191 end += 5 01192 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01193 start = end 01194 end += 4 01195 (length,) = _struct_I.unpack(str[start:end]) 01196 start = end 01197 end += length 01198 _v70.data = str[start:end] 01199 _v73 = val1.features 01200 _v74 = _v73.header 01201 start = end 01202 end += 4 01203 (_v74.seq,) = _struct_I.unpack(str[start:end]) 01204 _v75 = _v74.stamp 01205 _x = _v75 01206 start = end 01207 end += 8 01208 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01209 start = end 01210 end += 4 01211 (length,) = _struct_I.unpack(str[start:end]) 01212 start = end 01213 end += length 01214 _v74.frame_id = str[start:end] 01215 start = end 01216 end += 4 01217 (length,) = _struct_I.unpack(str[start:end]) 01218 _v73.object_points = [] 01219 for i in range(0, length): 01220 val3 = geometry_msgs.msg.Point32() 01221 _x = val3 01222 start = end 01223 end += 12 01224 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01225 _v73.object_points.append(val3) 01226 start = end 01227 end += 4 01228 (length,) = _struct_I.unpack(str[start:end]) 01229 _v73.image_points = [] 01230 for i in range(0, length): 01231 val3 = cob_calibration_msgs.msg.ImagePoint() 01232 _x = val3 01233 start = end 01234 end += 8 01235 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 01236 _v73.image_points.append(val3) 01237 start = end 01238 end += 1 01239 (_v73.success,) = _struct_B.unpack(str[start:end]) 01240 self.M_cam.append(val1) 01241 start = end 01242 end += 4 01243 (length,) = _struct_I.unpack(str[start:end]) 01244 self.M_chain = [] 01245 for i in range(0, length): 01246 val1 = cob_calibration_msgs.msg.ChainMeasurement() 01247 _v76 = val1.header 01248 start = end 01249 end += 4 01250 (_v76.seq,) = _struct_I.unpack(str[start:end]) 01251 _v77 = _v76.stamp 01252 _x = _v77 01253 start = end 01254 end += 8 01255 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01256 start = end 01257 end += 4 01258 (length,) = _struct_I.unpack(str[start:end]) 01259 start = end 01260 end += length 01261 _v76.frame_id = str[start:end] 01262 start = end 01263 end += 4 01264 (length,) = _struct_I.unpack(str[start:end]) 01265 start = end 01266 end += length 01267 val1.chain_id = str[start:end] 01268 _v78 = val1.chain_state 01269 _v79 = _v78.header 01270 start = end 01271 end += 4 01272 (_v79.seq,) = _struct_I.unpack(str[start:end]) 01273 _v80 = _v79.stamp 01274 _x = _v80 01275 start = end 01276 end += 8 01277 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01278 start = end 01279 end += 4 01280 (length,) = _struct_I.unpack(str[start:end]) 01281 start = end 01282 end += length 01283 _v79.frame_id = str[start:end] 01284 start = end 01285 end += 4 01286 (length,) = _struct_I.unpack(str[start:end]) 01287 _v78.name = [] 01288 for i in range(0, length): 01289 start = end 01290 end += 4 01291 (length,) = _struct_I.unpack(str[start:end]) 01292 start = end 01293 end += length 01294 val3 = str[start:end] 01295 _v78.name.append(val3) 01296 start = end 01297 end += 4 01298 (length,) = _struct_I.unpack(str[start:end]) 01299 pattern = '<%sd'%length 01300 start = end 01301 end += struct.calcsize(pattern) 01302 _v78.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01303 start = end 01304 end += 4 01305 (length,) = _struct_I.unpack(str[start:end]) 01306 pattern = '<%sd'%length 01307 start = end 01308 end += struct.calcsize(pattern) 01309 _v78.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01310 start = end 01311 end += 4 01312 (length,) = _struct_I.unpack(str[start:end]) 01313 pattern = '<%sd'%length 01314 start = end 01315 end += struct.calcsize(pattern) 01316 _v78.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01317 self.M_chain.append(val1) 01318 return self 01319 except struct.error as e: 01320 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01321 01322 _struct_I = roslib.message.struct_I 01323 _struct_B = struct.Struct("<B") 01324 _struct_12d = struct.Struct("<12d") 01325 _struct_9d = struct.Struct("<9d") 01326 _struct_BI = struct.Struct("<BI") 01327 _struct_3f = struct.Struct("<3f") 01328 _struct_2f = struct.Struct("<2f") 01329 _struct_2I = struct.Struct("<2I") 01330 _struct_4IB = struct.Struct("<4IB")