00001 """autogenerated by genmsg_py from Imu.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import geometry_msgs.msg
00006 import std_msgs.msg
00007
00008 class Imu(roslib.message.Message):
00009 _md5sum = "6a62c6daae103f4ff57a132d6f95cec2"
00010 _type = "sensor_msgs/Imu"
00011 _has_header = True
00012 _full_text = """# This is a message to hold data from an IMU (Inertial Measurement Unit)
00013 #
00014 # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
00015 #
00016 # If the covariance of the measurement is known, it should be filled in (if all you know is the variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
00017 # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the data a covariance will have to be assumed or gotten from some other source
00018 #
00019 # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation estimate), please set element 0 of the associated covariance matrix to -1
00020 # If you are interpreting this message, please check for a value of -1 in the first element of each covariance matrix, and disregard the associated estimate.
00021
00022 Header header
00023
00024 geometry_msgs/Quaternion orientation
00025 float64[9] orientation_covariance # Row major about x, y, z axes
00026
00027 geometry_msgs/Vector3 angular_velocity
00028 float64[9] angular_velocity_covariance # Row major about x, y, z axes
00029
00030 geometry_msgs/Vector3 linear_acceleration
00031 float64[9] linear_acceleration_covariance # Row major x, y z
00032
00033 ================================================================================
00034 MSG: std_msgs/Header
00035 # Standard metadata for higher-level stamped data types.
00036 # This is generally used to communicate timestamped data
00037 # in a particular coordinate frame.
00038 #
00039 # sequence ID: consecutively increasing ID
00040 uint32 seq
00041 #Two-integer timestamp that is expressed as:
00042 # * stamp.secs: seconds (stamp_secs) since epoch
00043 # * stamp.nsecs: nanoseconds since stamp_secs
00044 # time-handling sugar is provided by the client library
00045 time stamp
00046 #Frame this data is associated with
00047 # 0: no frame
00048 # 1: global frame
00049 string frame_id
00050
00051 ================================================================================
00052 MSG: geometry_msgs/Quaternion
00053 # This represents an orientation in free space in quaternion form.
00054
00055 float64 x
00056 float64 y
00057 float64 z
00058 float64 w
00059
00060 ================================================================================
00061 MSG: geometry_msgs/Vector3
00062 # This represents a vector in free space.
00063
00064 float64 x
00065 float64 y
00066 float64 z
00067 """
00068 __slots__ = ['header','orientation','orientation_covariance','angular_velocity','angular_velocity_covariance','linear_acceleration','linear_acceleration_covariance']
00069 _slot_types = ['Header','geometry_msgs/Quaternion','float64[9]','geometry_msgs/Vector3','float64[9]','geometry_msgs/Vector3','float64[9]']
00070
00071 def __init__(self, *args, **kwds):
00072 """
00073 Constructor. Any message fields that are implicitly/explicitly
00074 set to None will be assigned a default value. The recommend
00075 use is keyword arguments as this is more robust to future message
00076 changes. You cannot mix in-order arguments and keyword arguments.
00077
00078 The available fields are:
00079 header,orientation,orientation_covariance,angular_velocity,angular_velocity_covariance,linear_acceleration,linear_acceleration_covariance
00080
00081 @param args: complete set of field values, in .msg order
00082 @param kwds: use keyword arguments corresponding to message field names
00083 to set specific fields.
00084 """
00085 if args or kwds:
00086 super(Imu, self).__init__(*args, **kwds)
00087
00088 if self.header is None:
00089 self.header = std_msgs.msg._Header.Header()
00090 if self.orientation is None:
00091 self.orientation = geometry_msgs.msg.Quaternion()
00092 if self.orientation_covariance is None:
00093 self.orientation_covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.]
00094 if self.angular_velocity is None:
00095 self.angular_velocity = geometry_msgs.msg.Vector3()
00096 if self.angular_velocity_covariance is None:
00097 self.angular_velocity_covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.]
00098 if self.linear_acceleration is None:
00099 self.linear_acceleration = geometry_msgs.msg.Vector3()
00100 if self.linear_acceleration_covariance is None:
00101 self.linear_acceleration_covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.]
00102 else:
00103 self.header = std_msgs.msg._Header.Header()
00104 self.orientation = geometry_msgs.msg.Quaternion()
00105 self.orientation_covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.]
00106 self.angular_velocity = geometry_msgs.msg.Vector3()
00107 self.angular_velocity_covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.]
00108 self.linear_acceleration = geometry_msgs.msg.Vector3()
00109 self.linear_acceleration_covariance = [0.,0.,0.,0.,0.,0.,0.,0.,0.]
00110
00111 def _get_types(self):
00112 """
00113 internal API method
00114 """
00115 return self._slot_types
00116
00117 def serialize(self, buff):
00118 """
00119 serialize message into buffer
00120 @param buff: buffer
00121 @type buff: StringIO
00122 """
00123 try:
00124 _x = self
00125 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00126 _x = self.header.frame_id
00127 length = len(_x)
00128 buff.write(struct.pack('<I%ss'%length, length, _x))
00129 _x = self
00130 buff.write(_struct_4d.pack(_x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w))
00131 buff.write(_struct_9d.pack(*self.orientation_covariance))
00132 _x = self
00133 buff.write(_struct_3d.pack(_x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z))
00134 buff.write(_struct_9d.pack(*self.angular_velocity_covariance))
00135 _x = self
00136 buff.write(_struct_3d.pack(_x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z))
00137 buff.write(_struct_9d.pack(*self.linear_acceleration_covariance))
00138 except struct.error as se: self._check_types(se)
00139 except TypeError as te: self._check_types(te)
00140
00141 def deserialize(self, str):
00142 """
00143 unpack serialized message in str into this message instance
00144 @param str: byte array of serialized message
00145 @type str: str
00146 """
00147 try:
00148 if self.header is None:
00149 self.header = std_msgs.msg._Header.Header()
00150 if self.orientation is None:
00151 self.orientation = geometry_msgs.msg.Quaternion()
00152 if self.angular_velocity is None:
00153 self.angular_velocity = geometry_msgs.msg.Vector3()
00154 if self.linear_acceleration is None:
00155 self.linear_acceleration = geometry_msgs.msg.Vector3()
00156 end = 0
00157 _x = self
00158 start = end
00159 end += 12
00160 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00161 start = end
00162 end += 4
00163 (length,) = _struct_I.unpack(str[start:end])
00164 start = end
00165 end += length
00166 self.header.frame_id = str[start:end]
00167 _x = self
00168 start = end
00169 end += 32
00170 (_x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w,) = _struct_4d.unpack(str[start:end])
00171 start = end
00172 end += 72
00173 self.orientation_covariance = _struct_9d.unpack(str[start:end])
00174 _x = self
00175 start = end
00176 end += 24
00177 (_x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z,) = _struct_3d.unpack(str[start:end])
00178 start = end
00179 end += 72
00180 self.angular_velocity_covariance = _struct_9d.unpack(str[start:end])
00181 _x = self
00182 start = end
00183 end += 24
00184 (_x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z,) = _struct_3d.unpack(str[start:end])
00185 start = end
00186 end += 72
00187 self.linear_acceleration_covariance = _struct_9d.unpack(str[start:end])
00188 return self
00189 except struct.error as e:
00190 raise roslib.message.DeserializationError(e)
00191
00192
00193 def serialize_numpy(self, buff, numpy):
00194 """
00195 serialize message with numpy array types into buffer
00196 @param buff: buffer
00197 @type buff: StringIO
00198 @param numpy: numpy python module
00199 @type numpy module
00200 """
00201 try:
00202 _x = self
00203 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00204 _x = self.header.frame_id
00205 length = len(_x)
00206 buff.write(struct.pack('<I%ss'%length, length, _x))
00207 _x = self
00208 buff.write(_struct_4d.pack(_x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w))
00209 buff.write(self.orientation_covariance.tostring())
00210 _x = self
00211 buff.write(_struct_3d.pack(_x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z))
00212 buff.write(self.angular_velocity_covariance.tostring())
00213 _x = self
00214 buff.write(_struct_3d.pack(_x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z))
00215 buff.write(self.linear_acceleration_covariance.tostring())
00216 except struct.error as se: self._check_types(se)
00217 except TypeError as te: self._check_types(te)
00218
00219 def deserialize_numpy(self, str, numpy):
00220 """
00221 unpack serialized message in str into this message instance using numpy for array types
00222 @param str: byte array of serialized message
00223 @type str: str
00224 @param numpy: numpy python module
00225 @type numpy: module
00226 """
00227 try:
00228 if self.header is None:
00229 self.header = std_msgs.msg._Header.Header()
00230 if self.orientation is None:
00231 self.orientation = geometry_msgs.msg.Quaternion()
00232 if self.angular_velocity is None:
00233 self.angular_velocity = geometry_msgs.msg.Vector3()
00234 if self.linear_acceleration is None:
00235 self.linear_acceleration = geometry_msgs.msg.Vector3()
00236 end = 0
00237 _x = self
00238 start = end
00239 end += 12
00240 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00241 start = end
00242 end += 4
00243 (length,) = _struct_I.unpack(str[start:end])
00244 start = end
00245 end += length
00246 self.header.frame_id = str[start:end]
00247 _x = self
00248 start = end
00249 end += 32
00250 (_x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w,) = _struct_4d.unpack(str[start:end])
00251 start = end
00252 end += 72
00253 self.orientation_covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
00254 _x = self
00255 start = end
00256 end += 24
00257 (_x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z,) = _struct_3d.unpack(str[start:end])
00258 start = end
00259 end += 72
00260 self.angular_velocity_covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
00261 _x = self
00262 start = end
00263 end += 24
00264 (_x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z,) = _struct_3d.unpack(str[start:end])
00265 start = end
00266 end += 72
00267 self.linear_acceleration_covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
00268 return self
00269 except struct.error as e:
00270 raise roslib.message.DeserializationError(e)
00271
00272 _struct_I = roslib.message.struct_I
00273 _struct_3I = struct.Struct("<3I")
00274 _struct_3d = struct.Struct("<3d")
00275 _struct_4d = struct.Struct("<4d")
00276 _struct_9d = struct.Struct("<9d")