# convert imu to odom

I have implemented my odom node like this:

```
#!/usr/bin/env python
# license removed for brevity
import rospy
from nav_msgs.msg import Odometry
import smbus
import math
from math import sin, cos, pi
import tf
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
# Register
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
def read_byte(reg):
return bus.read_byte_data(address, reg)
def read_word(reg):
h = bus.read_byte_data(address, reg)
l = bus.read_byte_data(address, reg+1)
value = (h << 8) + l
return value
def read_word_2c(reg):
val = read_word(reg)
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val
def dist(a,b):
return math.sqrt((a*a)+(b*b))
def get_y_rotation(x,y,z):
radians = math.atan2(x, dist(y,z))
return -math.degrees(radians)
def get_x_rotation(x,y,z):
radians = math.atan2(y, dist(x,z))
return math.degrees(radians)
bus = smbus.SMBus(1) # bus = smbus.SMBus(0) fuer Revision 1
address = 0x68 # via i2cdetect
# Aktivieren, um das Modul ansprechen zu koennen
bus.write_byte_data(address, power_mgmt_1, 0)
def talker():
pub = rospy.Publisher('odom', Odometry, queue_size=10)
odom_broadcaster = tf.TransformBroadcaster()
rospy.init_node('odom_node', anonymous=True)
start_time = rospy.Time.now()
current_time = rospy.Time.now()
last_time = rospy.Time.now()
x = 0.0
y = 0.0
th = 0.0
rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): odom = Odometry()
while not rospy.is_shutdown():
gyroskop_xout = read_word_2c(0x43)
gyroskop_yout = read_word_2c(0x45)
gyroskop_zout = read_word_2c(0x47)
accleration_xout = read_word_2c(0x3b)
accleration_yout = read_word_2c(0x3d)
accleration_zout = read_word_2c(0x3f)
accleration_xout_skaliert = accleration_xout / 16384.0
accleration_yout_skaliert = accleration_yout / 16384.0
accleration_zout_skaliert = accleration_zout / 16384.0
current_time = rospy.Time.now()
vx = accleration_xout_skaliert
vy = accleration_yout_skaliert
vth = gyroskop_zout
# compute odometry in a typical way given the velocities of the robot
dt = (current_time - last_time).to_sec()
delta_x = (vx * cos(th) - vy * sin(th)) * dt
delta_y = (vx * sin(th) + vy * cos(th)) * dt
delta_th = vth * dt
x += delta_x
y += delta_y
th += delta_th
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform(
(x, y, 0.),
odom_quat,
current_time,
"base_link",
"odom"
)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
pub.publish(odom)
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
```

Here i have accessed the linear and angular velocity change with Mpu6050 imu sensor. Here is the data:

```
header:
seq: 1008
stamp:
secs: 0
nsecs: 0
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: 238.075342952
y: 510.232761159
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.528979737974
w: 0.848634454175
covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ...
```