00001 """autogenerated by genmsg_py from LaserMeasurement.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import calibration_msgs.msg
00007 import sensor_msgs.msg
00008 import roslib.rostime
00009 import std_msgs.msg
00010
00011 class LaserMeasurement(roslib.message.Message):
00012 _md5sum = "03ce2d57df2a0cedc1a91dc4b40d7729"
00013 _type = "calibration_msgs/LaserMeasurement"
00014 _has_header = True
00015 _full_text = """Header header
00016 string laser_id
00017 sensor_msgs/JointState[] joint_points
00018
00019 # True -> The extra debugging fields are populated
00020 bool verbose
00021
00022 # Extra, partially processed data. Only needed for debugging
00023 calibration_msgs/DenseLaserSnapshot snapshot
00024 sensor_msgs/Image laser_image
00025 calibration_msgs/CalibrationPattern image_features
00026 calibration_msgs/JointStateCalibrationPattern joint_features
00027
00028 ================================================================================
00029 MSG: std_msgs/Header
00030 # Standard metadata for higher-level stamped data types.
00031 # This is generally used to communicate timestamped data
00032 # in a particular coordinate frame.
00033 #
00034 # sequence ID: consecutively increasing ID
00035 uint32 seq
00036 #Two-integer timestamp that is expressed as:
00037 # * stamp.secs: seconds (stamp_secs) since epoch
00038 # * stamp.nsecs: nanoseconds since stamp_secs
00039 # time-handling sugar is provided by the client library
00040 time stamp
00041 #Frame this data is associated with
00042 # 0: no frame
00043 # 1: global frame
00044 string frame_id
00045
00046 ================================================================================
00047 MSG: sensor_msgs/JointState
00048 # This is a message that holds data to describe the state of a set of torque controlled joints.
00049 #
00050 # The state of each joint (revolute or prismatic) is defined by:
00051 # * the position of the joint (rad or m),
00052 # * the velocity of the joint (rad/s or m/s) and
00053 # * the effort that is applied in the joint (Nm or N).
00054 #
00055 # Each joint is uniquely identified by its name
00056 # The header specifies the time at which the joint states were recorded. All the joint states
00057 # in one message have to be recorded at the same time.
00058 #
00059 # This message consists of a multiple arrays, one for each part of the joint state.
00060 # The goal is to make each of the fields optional. When e.g. your joints have no
00061 # effort associated with them, you can leave the effort array empty.
00062 #
00063 # All arrays in this message should have the same size, or be empty.
00064 # This is the only way to uniquely associate the joint name with the correct
00065 # states.
00066
00067
00068 Header header
00069
00070 string[] name
00071 float64[] position
00072 float64[] velocity
00073 float64[] effort
00074
00075 ================================================================================
00076 MSG: calibration_msgs/DenseLaserSnapshot
00077 # Provides all the state & sensor information for
00078 # a single sweep of laser attached to some mechanism.
00079 # Most likely, this will be used with PR2's tilting laser mechanism
00080 Header header
00081
00082 # Store the laser intrinsics. This is very similar to the
00083 # intrinsics commonly stored in
00084 float32 angle_min # start angle of the scan [rad]
00085 float32 angle_max # end angle of the scan [rad]
00086 float32 angle_increment # angular distance between measurements [rad]
00087 float32 time_increment # time between measurements [seconds]
00088 float32 range_min # minimum range value [m]
00089 float32 range_max # maximum range value [m]
00090
00091 # Define the size of the binary data
00092 uint32 readings_per_scan # (Width)
00093 uint32 num_scans # (Height)
00094
00095 # 2D Arrays storing laser data.
00096 # We can think of each type data as being a single channel image.
00097 # Each row of the image has data from a single scan, and scans are
00098 # concatenated to form the entire 'image'.
00099 float32[] ranges # (Image data)
00100 float32[] intensities # (Image data)
00101
00102 # Store the start time of each scan
00103 time[] scan_start
00104
00105 ================================================================================
00106 MSG: sensor_msgs/Image
00107 # This message contains an uncompressed image
00108 # (0, 0) is at top-left corner of image
00109 #
00110
00111 Header header # Header timestamp should be acquisition time of image
00112 # Header frame_id should be optical frame of camera
00113 # origin of frame should be optical center of cameara
00114 # +x should point to the right in the image
00115 # +y should point down in the image
00116 # +z should point into to plane of the image
00117 # If the frame_id here and the frame_id of the CameraInfo
00118 # message associated with the image conflict
00119 # the behavior is undefined
00120
00121 uint32 height # image height, that is, number of rows
00122 uint32 width # image width, that is, number of columns
00123
00124 # The legal values for encoding are in file src/image_encodings.cpp
00125 # If you want to standardize a new string format, join
00126 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00127
00128 string encoding # Encoding of pixels -- channel meaning, ordering, size
00129 # taken from the list of strings in src/image_encodings.cpp
00130
00131 uint8 is_bigendian # is this data bigendian?
00132 uint32 step # Full row length in bytes
00133 uint8[] data # actual matrix data, size is (step * rows)
00134
00135 ================================================================================
00136 MSG: calibration_msgs/CalibrationPattern
00137 Header header
00138 geometry_msgs/Point32[] object_points
00139 ImagePoint[] image_points
00140 uint8 success
00141
00142 ================================================================================
00143 MSG: geometry_msgs/Point32
00144 # This contains the position of a point in free space(with 32 bits of precision).
00145 # It is recommeded to use Point wherever possible instead of Point32.
00146 #
00147 # This recommendation is to promote interoperability.
00148 #
00149 # This message is designed to take up less space when sending
00150 # lots of points at once, as in the case of a PointCloud.
00151
00152 float32 x
00153 float32 y
00154 float32 z
00155 ================================================================================
00156 MSG: calibration_msgs/ImagePoint
00157 float32 x
00158 float32 y
00159
00160 ================================================================================
00161 MSG: calibration_msgs/JointStateCalibrationPattern
00162 Header header
00163 geometry_msgs/Point32[] object_points
00164 sensor_msgs/JointState[] joint_points
00165
00166
00167 """
00168 __slots__ = ['header','laser_id','joint_points','verbose','snapshot','laser_image','image_features','joint_features']
00169 _slot_types = ['Header','string','sensor_msgs/JointState[]','bool','calibration_msgs/DenseLaserSnapshot','sensor_msgs/Image','calibration_msgs/CalibrationPattern','calibration_msgs/JointStateCalibrationPattern']
00170
00171 def __init__(self, *args, **kwds):
00172 """
00173 Constructor. Any message fields that are implicitly/explicitly
00174 set to None will be assigned a default value. The recommend
00175 use is keyword arguments as this is more robust to future message
00176 changes. You cannot mix in-order arguments and keyword arguments.
00177
00178 The available fields are:
00179 header,laser_id,joint_points,verbose,snapshot,laser_image,image_features,joint_features
00180
00181 @param args: complete set of field values, in .msg order
00182 @param kwds: use keyword arguments corresponding to message field names
00183 to set specific fields.
00184 """
00185 if args or kwds:
00186 super(LaserMeasurement, self).__init__(*args, **kwds)
00187
00188 if self.header is None:
00189 self.header = std_msgs.msg._Header.Header()
00190 if self.laser_id is None:
00191 self.laser_id = ''
00192 if self.joint_points is None:
00193 self.joint_points = []
00194 if self.verbose is None:
00195 self.verbose = False
00196 if self.snapshot is None:
00197 self.snapshot = calibration_msgs.msg.DenseLaserSnapshot()
00198 if self.laser_image is None:
00199 self.laser_image = sensor_msgs.msg.Image()
00200 if self.image_features is None:
00201 self.image_features = calibration_msgs.msg.CalibrationPattern()
00202 if self.joint_features is None:
00203 self.joint_features = calibration_msgs.msg.JointStateCalibrationPattern()
00204 else:
00205 self.header = std_msgs.msg._Header.Header()
00206 self.laser_id = ''
00207 self.joint_points = []
00208 self.verbose = False
00209 self.snapshot = calibration_msgs.msg.DenseLaserSnapshot()
00210 self.laser_image = sensor_msgs.msg.Image()
00211 self.image_features = calibration_msgs.msg.CalibrationPattern()
00212 self.joint_features = calibration_msgs.msg.JointStateCalibrationPattern()
00213
00214 def _get_types(self):
00215 """
00216 internal API method
00217 """
00218 return self._slot_types
00219
00220 def serialize(self, buff):
00221 """
00222 serialize message into buffer
00223 @param buff: buffer
00224 @type buff: StringIO
00225 """
00226 try:
00227 _x = self
00228 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00229 _x = self.header.frame_id
00230 length = len(_x)
00231 buff.write(struct.pack('<I%ss'%length, length, _x))
00232 _x = self.laser_id
00233 length = len(_x)
00234 buff.write(struct.pack('<I%ss'%length, length, _x))
00235 length = len(self.joint_points)
00236 buff.write(_struct_I.pack(length))
00237 for val1 in self.joint_points:
00238 _v1 = val1.header
00239 buff.write(_struct_I.pack(_v1.seq))
00240 _v2 = _v1.stamp
00241 _x = _v2
00242 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00243 _x = _v1.frame_id
00244 length = len(_x)
00245 buff.write(struct.pack('<I%ss'%length, length, _x))
00246 length = len(val1.name)
00247 buff.write(_struct_I.pack(length))
00248 for val2 in val1.name:
00249 length = len(val2)
00250 buff.write(struct.pack('<I%ss'%length, length, val2))
00251 length = len(val1.position)
00252 buff.write(_struct_I.pack(length))
00253 pattern = '<%sd'%length
00254 buff.write(struct.pack(pattern, *val1.position))
00255 length = len(val1.velocity)
00256 buff.write(_struct_I.pack(length))
00257 pattern = '<%sd'%length
00258 buff.write(struct.pack(pattern, *val1.velocity))
00259 length = len(val1.effort)
00260 buff.write(_struct_I.pack(length))
00261 pattern = '<%sd'%length
00262 buff.write(struct.pack(pattern, *val1.effort))
00263 _x = self
00264 buff.write(_struct_B3I.pack(_x.verbose, _x.snapshot.header.seq, _x.snapshot.header.stamp.secs, _x.snapshot.header.stamp.nsecs))
00265 _x = self.snapshot.header.frame_id
00266 length = len(_x)
00267 buff.write(struct.pack('<I%ss'%length, length, _x))
00268 _x = self
00269 buff.write(_struct_6f2I.pack(_x.snapshot.angle_min, _x.snapshot.angle_max, _x.snapshot.angle_increment, _x.snapshot.time_increment, _x.snapshot.range_min, _x.snapshot.range_max, _x.snapshot.readings_per_scan, _x.snapshot.num_scans))
00270 length = len(self.snapshot.ranges)
00271 buff.write(_struct_I.pack(length))
00272 pattern = '<%sf'%length
00273 buff.write(struct.pack(pattern, *self.snapshot.ranges))
00274 length = len(self.snapshot.intensities)
00275 buff.write(_struct_I.pack(length))
00276 pattern = '<%sf'%length
00277 buff.write(struct.pack(pattern, *self.snapshot.intensities))
00278 length = len(self.snapshot.scan_start)
00279 buff.write(_struct_I.pack(length))
00280 for val1 in self.snapshot.scan_start:
00281 _x = val1
00282 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00283 _x = self
00284 buff.write(_struct_3I.pack(_x.laser_image.header.seq, _x.laser_image.header.stamp.secs, _x.laser_image.header.stamp.nsecs))
00285 _x = self.laser_image.header.frame_id
00286 length = len(_x)
00287 buff.write(struct.pack('<I%ss'%length, length, _x))
00288 _x = self
00289 buff.write(_struct_2I.pack(_x.laser_image.height, _x.laser_image.width))
00290 _x = self.laser_image.encoding
00291 length = len(_x)
00292 buff.write(struct.pack('<I%ss'%length, length, _x))
00293 _x = self
00294 buff.write(_struct_BI.pack(_x.laser_image.is_bigendian, _x.laser_image.step))
00295 _x = self.laser_image.data
00296 length = len(_x)
00297
00298 if type(_x) in [list, tuple]:
00299 buff.write(struct.pack('<I%sB'%length, length, *_x))
00300 else:
00301 buff.write(struct.pack('<I%ss'%length, length, _x))
00302 _x = self
00303 buff.write(_struct_3I.pack(_x.image_features.header.seq, _x.image_features.header.stamp.secs, _x.image_features.header.stamp.nsecs))
00304 _x = self.image_features.header.frame_id
00305 length = len(_x)
00306 buff.write(struct.pack('<I%ss'%length, length, _x))
00307 length = len(self.image_features.object_points)
00308 buff.write(_struct_I.pack(length))
00309 for val1 in self.image_features.object_points:
00310 _x = val1
00311 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00312 length = len(self.image_features.image_points)
00313 buff.write(_struct_I.pack(length))
00314 for val1 in self.image_features.image_points:
00315 _x = val1
00316 buff.write(_struct_2f.pack(_x.x, _x.y))
00317 _x = self
00318 buff.write(_struct_B3I.pack(_x.image_features.success, _x.joint_features.header.seq, _x.joint_features.header.stamp.secs, _x.joint_features.header.stamp.nsecs))
00319 _x = self.joint_features.header.frame_id
00320 length = len(_x)
00321 buff.write(struct.pack('<I%ss'%length, length, _x))
00322 length = len(self.joint_features.object_points)
00323 buff.write(_struct_I.pack(length))
00324 for val1 in self.joint_features.object_points:
00325 _x = val1
00326 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00327 length = len(self.joint_features.joint_points)
00328 buff.write(_struct_I.pack(length))
00329 for val1 in self.joint_features.joint_points:
00330 _v3 = val1.header
00331 buff.write(_struct_I.pack(_v3.seq))
00332 _v4 = _v3.stamp
00333 _x = _v4
00334 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00335 _x = _v3.frame_id
00336 length = len(_x)
00337 buff.write(struct.pack('<I%ss'%length, length, _x))
00338 length = len(val1.name)
00339 buff.write(_struct_I.pack(length))
00340 for val2 in val1.name:
00341 length = len(val2)
00342 buff.write(struct.pack('<I%ss'%length, length, val2))
00343 length = len(val1.position)
00344 buff.write(_struct_I.pack(length))
00345 pattern = '<%sd'%length
00346 buff.write(struct.pack(pattern, *val1.position))
00347 length = len(val1.velocity)
00348 buff.write(_struct_I.pack(length))
00349 pattern = '<%sd'%length
00350 buff.write(struct.pack(pattern, *val1.velocity))
00351 length = len(val1.effort)
00352 buff.write(_struct_I.pack(length))
00353 pattern = '<%sd'%length
00354 buff.write(struct.pack(pattern, *val1.effort))
00355 except struct.error, se: self._check_types(se)
00356 except TypeError, te: self._check_types(te)
00357
00358 def deserialize(self, str):
00359 """
00360 unpack serialized message in str into this message instance
00361 @param str: byte array of serialized message
00362 @type str: str
00363 """
00364 try:
00365 if self.header is None:
00366 self.header = std_msgs.msg._Header.Header()
00367 if self.snapshot is None:
00368 self.snapshot = calibration_msgs.msg.DenseLaserSnapshot()
00369 if self.laser_image is None:
00370 self.laser_image = sensor_msgs.msg.Image()
00371 if self.image_features is None:
00372 self.image_features = calibration_msgs.msg.CalibrationPattern()
00373 if self.joint_features is None:
00374 self.joint_features = calibration_msgs.msg.JointStateCalibrationPattern()
00375 end = 0
00376 _x = self
00377 start = end
00378 end += 12
00379 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00380 start = end
00381 end += 4
00382 (length,) = _struct_I.unpack(str[start:end])
00383 start = end
00384 end += length
00385 self.header.frame_id = str[start:end]
00386 start = end
00387 end += 4
00388 (length,) = _struct_I.unpack(str[start:end])
00389 start = end
00390 end += length
00391 self.laser_id = str[start:end]
00392 start = end
00393 end += 4
00394 (length,) = _struct_I.unpack(str[start:end])
00395 self.joint_points = []
00396 for i in xrange(0, length):
00397 val1 = sensor_msgs.msg.JointState()
00398 _v5 = val1.header
00399 start = end
00400 end += 4
00401 (_v5.seq,) = _struct_I.unpack(str[start:end])
00402 _v6 = _v5.stamp
00403 _x = _v6
00404 start = end
00405 end += 8
00406 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00407 start = end
00408 end += 4
00409 (length,) = _struct_I.unpack(str[start:end])
00410 start = end
00411 end += length
00412 _v5.frame_id = str[start:end]
00413 start = end
00414 end += 4
00415 (length,) = _struct_I.unpack(str[start:end])
00416 val1.name = []
00417 for i in xrange(0, length):
00418 start = end
00419 end += 4
00420 (length,) = _struct_I.unpack(str[start:end])
00421 start = end
00422 end += length
00423 val2 = str[start:end]
00424 val1.name.append(val2)
00425 start = end
00426 end += 4
00427 (length,) = _struct_I.unpack(str[start:end])
00428 pattern = '<%sd'%length
00429 start = end
00430 end += struct.calcsize(pattern)
00431 val1.position = struct.unpack(pattern, str[start:end])
00432 start = end
00433 end += 4
00434 (length,) = _struct_I.unpack(str[start:end])
00435 pattern = '<%sd'%length
00436 start = end
00437 end += struct.calcsize(pattern)
00438 val1.velocity = struct.unpack(pattern, str[start:end])
00439 start = end
00440 end += 4
00441 (length,) = _struct_I.unpack(str[start:end])
00442 pattern = '<%sd'%length
00443 start = end
00444 end += struct.calcsize(pattern)
00445 val1.effort = struct.unpack(pattern, str[start:end])
00446 self.joint_points.append(val1)
00447 _x = self
00448 start = end
00449 end += 13
00450 (_x.verbose, _x.snapshot.header.seq, _x.snapshot.header.stamp.secs, _x.snapshot.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end])
00451 self.verbose = bool(self.verbose)
00452 start = end
00453 end += 4
00454 (length,) = _struct_I.unpack(str[start:end])
00455 start = end
00456 end += length
00457 self.snapshot.header.frame_id = str[start:end]
00458 _x = self
00459 start = end
00460 end += 32
00461 (_x.snapshot.angle_min, _x.snapshot.angle_max, _x.snapshot.angle_increment, _x.snapshot.time_increment, _x.snapshot.range_min, _x.snapshot.range_max, _x.snapshot.readings_per_scan, _x.snapshot.num_scans,) = _struct_6f2I.unpack(str[start:end])
00462 start = end
00463 end += 4
00464 (length,) = _struct_I.unpack(str[start:end])
00465 pattern = '<%sf'%length
00466 start = end
00467 end += struct.calcsize(pattern)
00468 self.snapshot.ranges = struct.unpack(pattern, str[start:end])
00469 start = end
00470 end += 4
00471 (length,) = _struct_I.unpack(str[start:end])
00472 pattern = '<%sf'%length
00473 start = end
00474 end += struct.calcsize(pattern)
00475 self.snapshot.intensities = struct.unpack(pattern, str[start:end])
00476 start = end
00477 end += 4
00478 (length,) = _struct_I.unpack(str[start:end])
00479 self.snapshot.scan_start = []
00480 for i in xrange(0, length):
00481 val1 = roslib.rostime.Time()
00482 _x = val1
00483 start = end
00484 end += 8
00485 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00486 self.snapshot.scan_start.append(val1)
00487 _x = self
00488 start = end
00489 end += 12
00490 (_x.laser_image.header.seq, _x.laser_image.header.stamp.secs, _x.laser_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00491 start = end
00492 end += 4
00493 (length,) = _struct_I.unpack(str[start:end])
00494 start = end
00495 end += length
00496 self.laser_image.header.frame_id = str[start:end]
00497 _x = self
00498 start = end
00499 end += 8
00500 (_x.laser_image.height, _x.laser_image.width,) = _struct_2I.unpack(str[start:end])
00501 start = end
00502 end += 4
00503 (length,) = _struct_I.unpack(str[start:end])
00504 start = end
00505 end += length
00506 self.laser_image.encoding = str[start:end]
00507 _x = self
00508 start = end
00509 end += 5
00510 (_x.laser_image.is_bigendian, _x.laser_image.step,) = _struct_BI.unpack(str[start:end])
00511 start = end
00512 end += 4
00513 (length,) = _struct_I.unpack(str[start:end])
00514 start = end
00515 end += length
00516 self.laser_image.data = str[start:end]
00517 _x = self
00518 start = end
00519 end += 12
00520 (_x.image_features.header.seq, _x.image_features.header.stamp.secs, _x.image_features.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00521 start = end
00522 end += 4
00523 (length,) = _struct_I.unpack(str[start:end])
00524 start = end
00525 end += length
00526 self.image_features.header.frame_id = str[start:end]
00527 start = end
00528 end += 4
00529 (length,) = _struct_I.unpack(str[start:end])
00530 self.image_features.object_points = []
00531 for i in xrange(0, length):
00532 val1 = geometry_msgs.msg.Point32()
00533 _x = val1
00534 start = end
00535 end += 12
00536 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00537 self.image_features.object_points.append(val1)
00538 start = end
00539 end += 4
00540 (length,) = _struct_I.unpack(str[start:end])
00541 self.image_features.image_points = []
00542 for i in xrange(0, length):
00543 val1 = calibration_msgs.msg.ImagePoint()
00544 _x = val1
00545 start = end
00546 end += 8
00547 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
00548 self.image_features.image_points.append(val1)
00549 _x = self
00550 start = end
00551 end += 13
00552 (_x.image_features.success, _x.joint_features.header.seq, _x.joint_features.header.stamp.secs, _x.joint_features.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end])
00553 start = end
00554 end += 4
00555 (length,) = _struct_I.unpack(str[start:end])
00556 start = end
00557 end += length
00558 self.joint_features.header.frame_id = str[start:end]
00559 start = end
00560 end += 4
00561 (length,) = _struct_I.unpack(str[start:end])
00562 self.joint_features.object_points = []
00563 for i in xrange(0, length):
00564 val1 = geometry_msgs.msg.Point32()
00565 _x = val1
00566 start = end
00567 end += 12
00568 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00569 self.joint_features.object_points.append(val1)
00570 start = end
00571 end += 4
00572 (length,) = _struct_I.unpack(str[start:end])
00573 self.joint_features.joint_points = []
00574 for i in xrange(0, length):
00575 val1 = sensor_msgs.msg.JointState()
00576 _v7 = val1.header
00577 start = end
00578 end += 4
00579 (_v7.seq,) = _struct_I.unpack(str[start:end])
00580 _v8 = _v7.stamp
00581 _x = _v8
00582 start = end
00583 end += 8
00584 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00585 start = end
00586 end += 4
00587 (length,) = _struct_I.unpack(str[start:end])
00588 start = end
00589 end += length
00590 _v7.frame_id = str[start:end]
00591 start = end
00592 end += 4
00593 (length,) = _struct_I.unpack(str[start:end])
00594 val1.name = []
00595 for i in xrange(0, length):
00596 start = end
00597 end += 4
00598 (length,) = _struct_I.unpack(str[start:end])
00599 start = end
00600 end += length
00601 val2 = str[start:end]
00602 val1.name.append(val2)
00603 start = end
00604 end += 4
00605 (length,) = _struct_I.unpack(str[start:end])
00606 pattern = '<%sd'%length
00607 start = end
00608 end += struct.calcsize(pattern)
00609 val1.position = struct.unpack(pattern, str[start:end])
00610 start = end
00611 end += 4
00612 (length,) = _struct_I.unpack(str[start:end])
00613 pattern = '<%sd'%length
00614 start = end
00615 end += struct.calcsize(pattern)
00616 val1.velocity = struct.unpack(pattern, str[start:end])
00617 start = end
00618 end += 4
00619 (length,) = _struct_I.unpack(str[start:end])
00620 pattern = '<%sd'%length
00621 start = end
00622 end += struct.calcsize(pattern)
00623 val1.effort = struct.unpack(pattern, str[start:end])
00624 self.joint_features.joint_points.append(val1)
00625 return self
00626 except struct.error, e:
00627 raise roslib.message.DeserializationError(e)
00628
00629
00630 def serialize_numpy(self, buff, numpy):
00631 """
00632 serialize message with numpy array types into buffer
00633 @param buff: buffer
00634 @type buff: StringIO
00635 @param numpy: numpy python module
00636 @type numpy module
00637 """
00638 try:
00639 _x = self
00640 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00641 _x = self.header.frame_id
00642 length = len(_x)
00643 buff.write(struct.pack('<I%ss'%length, length, _x))
00644 _x = self.laser_id
00645 length = len(_x)
00646 buff.write(struct.pack('<I%ss'%length, length, _x))
00647 length = len(self.joint_points)
00648 buff.write(_struct_I.pack(length))
00649 for val1 in self.joint_points:
00650 _v9 = val1.header
00651 buff.write(_struct_I.pack(_v9.seq))
00652 _v10 = _v9.stamp
00653 _x = _v10
00654 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00655 _x = _v9.frame_id
00656 length = len(_x)
00657 buff.write(struct.pack('<I%ss'%length, length, _x))
00658 length = len(val1.name)
00659 buff.write(_struct_I.pack(length))
00660 for val2 in val1.name:
00661 length = len(val2)
00662 buff.write(struct.pack('<I%ss'%length, length, val2))
00663 length = len(val1.position)
00664 buff.write(_struct_I.pack(length))
00665 pattern = '<%sd'%length
00666 buff.write(val1.position.tostring())
00667 length = len(val1.velocity)
00668 buff.write(_struct_I.pack(length))
00669 pattern = '<%sd'%length
00670 buff.write(val1.velocity.tostring())
00671 length = len(val1.effort)
00672 buff.write(_struct_I.pack(length))
00673 pattern = '<%sd'%length
00674 buff.write(val1.effort.tostring())
00675 _x = self
00676 buff.write(_struct_B3I.pack(_x.verbose, _x.snapshot.header.seq, _x.snapshot.header.stamp.secs, _x.snapshot.header.stamp.nsecs))
00677 _x = self.snapshot.header.frame_id
00678 length = len(_x)
00679 buff.write(struct.pack('<I%ss'%length, length, _x))
00680 _x = self
00681 buff.write(_struct_6f2I.pack(_x.snapshot.angle_min, _x.snapshot.angle_max, _x.snapshot.angle_increment, _x.snapshot.time_increment, _x.snapshot.range_min, _x.snapshot.range_max, _x.snapshot.readings_per_scan, _x.snapshot.num_scans))
00682 length = len(self.snapshot.ranges)
00683 buff.write(_struct_I.pack(length))
00684 pattern = '<%sf'%length
00685 buff.write(self.snapshot.ranges.tostring())
00686 length = len(self.snapshot.intensities)
00687 buff.write(_struct_I.pack(length))
00688 pattern = '<%sf'%length
00689 buff.write(self.snapshot.intensities.tostring())
00690 length = len(self.snapshot.scan_start)
00691 buff.write(_struct_I.pack(length))
00692 for val1 in self.snapshot.scan_start:
00693 _x = val1
00694 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00695 _x = self
00696 buff.write(_struct_3I.pack(_x.laser_image.header.seq, _x.laser_image.header.stamp.secs, _x.laser_image.header.stamp.nsecs))
00697 _x = self.laser_image.header.frame_id
00698 length = len(_x)
00699 buff.write(struct.pack('<I%ss'%length, length, _x))
00700 _x = self
00701 buff.write(_struct_2I.pack(_x.laser_image.height, _x.laser_image.width))
00702 _x = self.laser_image.encoding
00703 length = len(_x)
00704 buff.write(struct.pack('<I%ss'%length, length, _x))
00705 _x = self
00706 buff.write(_struct_BI.pack(_x.laser_image.is_bigendian, _x.laser_image.step))
00707 _x = self.laser_image.data
00708 length = len(_x)
00709
00710 if type(_x) in [list, tuple]:
00711 buff.write(struct.pack('<I%sB'%length, length, *_x))
00712 else:
00713 buff.write(struct.pack('<I%ss'%length, length, _x))
00714 _x = self
00715 buff.write(_struct_3I.pack(_x.image_features.header.seq, _x.image_features.header.stamp.secs, _x.image_features.header.stamp.nsecs))
00716 _x = self.image_features.header.frame_id
00717 length = len(_x)
00718 buff.write(struct.pack('<I%ss'%length, length, _x))
00719 length = len(self.image_features.object_points)
00720 buff.write(_struct_I.pack(length))
00721 for val1 in self.image_features.object_points:
00722 _x = val1
00723 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00724 length = len(self.image_features.image_points)
00725 buff.write(_struct_I.pack(length))
00726 for val1 in self.image_features.image_points:
00727 _x = val1
00728 buff.write(_struct_2f.pack(_x.x, _x.y))
00729 _x = self
00730 buff.write(_struct_B3I.pack(_x.image_features.success, _x.joint_features.header.seq, _x.joint_features.header.stamp.secs, _x.joint_features.header.stamp.nsecs))
00731 _x = self.joint_features.header.frame_id
00732 length = len(_x)
00733 buff.write(struct.pack('<I%ss'%length, length, _x))
00734 length = len(self.joint_features.object_points)
00735 buff.write(_struct_I.pack(length))
00736 for val1 in self.joint_features.object_points:
00737 _x = val1
00738 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00739 length = len(self.joint_features.joint_points)
00740 buff.write(_struct_I.pack(length))
00741 for val1 in self.joint_features.joint_points:
00742 _v11 = val1.header
00743 buff.write(_struct_I.pack(_v11.seq))
00744 _v12 = _v11.stamp
00745 _x = _v12
00746 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00747 _x = _v11.frame_id
00748 length = len(_x)
00749 buff.write(struct.pack('<I%ss'%length, length, _x))
00750 length = len(val1.name)
00751 buff.write(_struct_I.pack(length))
00752 for val2 in val1.name:
00753 length = len(val2)
00754 buff.write(struct.pack('<I%ss'%length, length, val2))
00755 length = len(val1.position)
00756 buff.write(_struct_I.pack(length))
00757 pattern = '<%sd'%length
00758 buff.write(val1.position.tostring())
00759 length = len(val1.velocity)
00760 buff.write(_struct_I.pack(length))
00761 pattern = '<%sd'%length
00762 buff.write(val1.velocity.tostring())
00763 length = len(val1.effort)
00764 buff.write(_struct_I.pack(length))
00765 pattern = '<%sd'%length
00766 buff.write(val1.effort.tostring())
00767 except struct.error, se: self._check_types(se)
00768 except TypeError, te: self._check_types(te)
00769
00770 def deserialize_numpy(self, str, numpy):
00771 """
00772 unpack serialized message in str into this message instance using numpy for array types
00773 @param str: byte array of serialized message
00774 @type str: str
00775 @param numpy: numpy python module
00776 @type numpy: module
00777 """
00778 try:
00779 if self.header is None:
00780 self.header = std_msgs.msg._Header.Header()
00781 if self.snapshot is None:
00782 self.snapshot = calibration_msgs.msg.DenseLaserSnapshot()
00783 if self.laser_image is None:
00784 self.laser_image = sensor_msgs.msg.Image()
00785 if self.image_features is None:
00786 self.image_features = calibration_msgs.msg.CalibrationPattern()
00787 if self.joint_features is None:
00788 self.joint_features = calibration_msgs.msg.JointStateCalibrationPattern()
00789 end = 0
00790 _x = self
00791 start = end
00792 end += 12
00793 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00794 start = end
00795 end += 4
00796 (length,) = _struct_I.unpack(str[start:end])
00797 start = end
00798 end += length
00799 self.header.frame_id = str[start:end]
00800 start = end
00801 end += 4
00802 (length,) = _struct_I.unpack(str[start:end])
00803 start = end
00804 end += length
00805 self.laser_id = str[start:end]
00806 start = end
00807 end += 4
00808 (length,) = _struct_I.unpack(str[start:end])
00809 self.joint_points = []
00810 for i in xrange(0, length):
00811 val1 = sensor_msgs.msg.JointState()
00812 _v13 = val1.header
00813 start = end
00814 end += 4
00815 (_v13.seq,) = _struct_I.unpack(str[start:end])
00816 _v14 = _v13.stamp
00817 _x = _v14
00818 start = end
00819 end += 8
00820 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00821 start = end
00822 end += 4
00823 (length,) = _struct_I.unpack(str[start:end])
00824 start = end
00825 end += length
00826 _v13.frame_id = str[start:end]
00827 start = end
00828 end += 4
00829 (length,) = _struct_I.unpack(str[start:end])
00830 val1.name = []
00831 for i in xrange(0, length):
00832 start = end
00833 end += 4
00834 (length,) = _struct_I.unpack(str[start:end])
00835 start = end
00836 end += length
00837 val2 = str[start:end]
00838 val1.name.append(val2)
00839 start = end
00840 end += 4
00841 (length,) = _struct_I.unpack(str[start:end])
00842 pattern = '<%sd'%length
00843 start = end
00844 end += struct.calcsize(pattern)
00845 val1.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00846 start = end
00847 end += 4
00848 (length,) = _struct_I.unpack(str[start:end])
00849 pattern = '<%sd'%length
00850 start = end
00851 end += struct.calcsize(pattern)
00852 val1.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00853 start = end
00854 end += 4
00855 (length,) = _struct_I.unpack(str[start:end])
00856 pattern = '<%sd'%length
00857 start = end
00858 end += struct.calcsize(pattern)
00859 val1.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00860 self.joint_points.append(val1)
00861 _x = self
00862 start = end
00863 end += 13
00864 (_x.verbose, _x.snapshot.header.seq, _x.snapshot.header.stamp.secs, _x.snapshot.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end])
00865 self.verbose = bool(self.verbose)
00866 start = end
00867 end += 4
00868 (length,) = _struct_I.unpack(str[start:end])
00869 start = end
00870 end += length
00871 self.snapshot.header.frame_id = str[start:end]
00872 _x = self
00873 start = end
00874 end += 32
00875 (_x.snapshot.angle_min, _x.snapshot.angle_max, _x.snapshot.angle_increment, _x.snapshot.time_increment, _x.snapshot.range_min, _x.snapshot.range_max, _x.snapshot.readings_per_scan, _x.snapshot.num_scans,) = _struct_6f2I.unpack(str[start:end])
00876 start = end
00877 end += 4
00878 (length,) = _struct_I.unpack(str[start:end])
00879 pattern = '<%sf'%length
00880 start = end
00881 end += struct.calcsize(pattern)
00882 self.snapshot.ranges = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
00883 start = end
00884 end += 4
00885 (length,) = _struct_I.unpack(str[start:end])
00886 pattern = '<%sf'%length
00887 start = end
00888 end += struct.calcsize(pattern)
00889 self.snapshot.intensities = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
00890 start = end
00891 end += 4
00892 (length,) = _struct_I.unpack(str[start:end])
00893 self.snapshot.scan_start = []
00894 for i in xrange(0, length):
00895 val1 = roslib.rostime.Time()
00896 _x = val1
00897 start = end
00898 end += 8
00899 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00900 self.snapshot.scan_start.append(val1)
00901 _x = self
00902 start = end
00903 end += 12
00904 (_x.laser_image.header.seq, _x.laser_image.header.stamp.secs, _x.laser_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00905 start = end
00906 end += 4
00907 (length,) = _struct_I.unpack(str[start:end])
00908 start = end
00909 end += length
00910 self.laser_image.header.frame_id = str[start:end]
00911 _x = self
00912 start = end
00913 end += 8
00914 (_x.laser_image.height, _x.laser_image.width,) = _struct_2I.unpack(str[start:end])
00915 start = end
00916 end += 4
00917 (length,) = _struct_I.unpack(str[start:end])
00918 start = end
00919 end += length
00920 self.laser_image.encoding = str[start:end]
00921 _x = self
00922 start = end
00923 end += 5
00924 (_x.laser_image.is_bigendian, _x.laser_image.step,) = _struct_BI.unpack(str[start:end])
00925 start = end
00926 end += 4
00927 (length,) = _struct_I.unpack(str[start:end])
00928 start = end
00929 end += length
00930 self.laser_image.data = str[start:end]
00931 _x = self
00932 start = end
00933 end += 12
00934 (_x.image_features.header.seq, _x.image_features.header.stamp.secs, _x.image_features.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00935 start = end
00936 end += 4
00937 (length,) = _struct_I.unpack(str[start:end])
00938 start = end
00939 end += length
00940 self.image_features.header.frame_id = str[start:end]
00941 start = end
00942 end += 4
00943 (length,) = _struct_I.unpack(str[start:end])
00944 self.image_features.object_points = []
00945 for i in xrange(0, length):
00946 val1 = geometry_msgs.msg.Point32()
00947 _x = val1
00948 start = end
00949 end += 12
00950 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00951 self.image_features.object_points.append(val1)
00952 start = end
00953 end += 4
00954 (length,) = _struct_I.unpack(str[start:end])
00955 self.image_features.image_points = []
00956 for i in xrange(0, length):
00957 val1 = calibration_msgs.msg.ImagePoint()
00958 _x = val1
00959 start = end
00960 end += 8
00961 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end])
00962 self.image_features.image_points.append(val1)
00963 _x = self
00964 start = end
00965 end += 13
00966 (_x.image_features.success, _x.joint_features.header.seq, _x.joint_features.header.stamp.secs, _x.joint_features.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end])
00967 start = end
00968 end += 4
00969 (length,) = _struct_I.unpack(str[start:end])
00970 start = end
00971 end += length
00972 self.joint_features.header.frame_id = str[start:end]
00973 start = end
00974 end += 4
00975 (length,) = _struct_I.unpack(str[start:end])
00976 self.joint_features.object_points = []
00977 for i in xrange(0, length):
00978 val1 = geometry_msgs.msg.Point32()
00979 _x = val1
00980 start = end
00981 end += 12
00982 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00983 self.joint_features.object_points.append(val1)
00984 start = end
00985 end += 4
00986 (length,) = _struct_I.unpack(str[start:end])
00987 self.joint_features.joint_points = []
00988 for i in xrange(0, length):
00989 val1 = sensor_msgs.msg.JointState()
00990 _v15 = val1.header
00991 start = end
00992 end += 4
00993 (_v15.seq,) = _struct_I.unpack(str[start:end])
00994 _v16 = _v15.stamp
00995 _x = _v16
00996 start = end
00997 end += 8
00998 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00999 start = end
01000 end += 4
01001 (length,) = _struct_I.unpack(str[start:end])
01002 start = end
01003 end += length
01004 _v15.frame_id = str[start:end]
01005 start = end
01006 end += 4
01007 (length,) = _struct_I.unpack(str[start:end])
01008 val1.name = []
01009 for i in xrange(0, length):
01010 start = end
01011 end += 4
01012 (length,) = _struct_I.unpack(str[start:end])
01013 start = end
01014 end += length
01015 val2 = str[start:end]
01016 val1.name.append(val2)
01017 start = end
01018 end += 4
01019 (length,) = _struct_I.unpack(str[start:end])
01020 pattern = '<%sd'%length
01021 start = end
01022 end += struct.calcsize(pattern)
01023 val1.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01024 start = end
01025 end += 4
01026 (length,) = _struct_I.unpack(str[start:end])
01027 pattern = '<%sd'%length
01028 start = end
01029 end += struct.calcsize(pattern)
01030 val1.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01031 start = end
01032 end += 4
01033 (length,) = _struct_I.unpack(str[start:end])
01034 pattern = '<%sd'%length
01035 start = end
01036 end += struct.calcsize(pattern)
01037 val1.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01038 self.joint_features.joint_points.append(val1)
01039 return self
01040 except struct.error, e:
01041 raise roslib.message.DeserializationError(e)
01042
01043 _struct_I = roslib.message.struct_I
01044 _struct_BI = struct.Struct("<BI")
01045 _struct_3f = struct.Struct("<3f")
01046 _struct_3I = struct.Struct("<3I")
01047 _struct_B3I = struct.Struct("<B3I")
01048 _struct_6f2I = struct.Struct("<6f2I")
01049 _struct_2I = struct.Struct("<2I")
01050 _struct_2f = struct.Struct("<2f")