def __init__(self):
rospy.init_node('berryimu', anonymous=True)
self.RAD_TO_DEG = 180 / math.pi
self.M_PI = 3.14159265358979323846
self.ACC_TO_MS2 = (1.0 / (0.101972 * 2**11)
) # 2^15 = 16G, 1G ~= 9.8m/s^2
self.G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly
self.GYRO_TO_RADS = 1 / self.RAD_TO_DEG
self.pub = rospy.Publisher("imu_data", Imu, queue_size=1000)
self.rate = rospy.Rate(int(rospy.get_param("~poll_rate", 20)))
# Special setup for virtual I2C device so doesn't conflict with actual device
is_virtual = int(rospy.get_param("~is_virtual", 0))
gyr_addr = int(rospy.get_param("~gyr_addr", 0x33))
mag_addr = int(rospy.get_param("~mag_addr", 0x44))
acc_addr = int(rospy.get_param("~acc_addr", 0x55))
if is_virtual:
self.IMU = IMU.BerryIMU(True, gyr_addr, mag_addr, acc_addr)
else:
self.IMU = IMU.BerryIMU(
) # Initialise the accelerometer, gyroscope and compass
