class ImuHandler:
def __init__(self):
#publishing rate
self.rate = 30 #30Hz
node_name = 'imu_mpu9150_handler'
rospy.init_node(node_name)
rospy.loginfo(rospy.get_caller_id() + 'Initialising {} node'.format(node_name))
# Get all parameters from config (rosparam)
address = int(rospy.get_param('imu/address', 0x69))
gyro_scale = int(rospy.get_param('imu/gyroscope/scale', 250))
acc_scale = int(rospy.get_param('imu/accelerometer/scale', 2))
# Initialize Madgwick Algorithm
#self.madgwick = MadgwickAHRS(1/30, 2.7101, 0.2);
self.quaternion = Madgwick(0.041, self.rate)
rospy.loginfo('Test {} address'.format(address))
self.imu = Mpu9150(address, gyro_scale, acc_scale)
#self.imu = Mpu9250(gyro_scale, acc_scale)
self.imu.calibrate();
rospy.loginfo("Compass calibration; move the IMU in random positions during the calibration");
(self.mag_offset, self.mag_scale) = self.imu.magCalibration();
rospy.loginfo("Calibration done");
def publish(self):
queue_size = rospy.get_param('imu/queue', 1);
self.pub_imu = rospy.Publisher('imu_readings', Imu, queue_size=queue_size);
self.pub_mag = rospy.Publisher('mag_readings', MagneticField, queue_size=queue_size);
#self.rate = rospy.Rate(rospy.get_param('imu/rate', 10))
#self.rate = rospy.Rate(20)
self.rate = rospy.Rate(30)
(self.msg_imu, self.msg_mag) = self.msg_template()
while not rospy.is_shutdown():
#rospy.loginfo(rospy.get_caller_id() + "publishing IMU data")
self.imu.update()
self.pack_message()
self.pub_imu.publish(self.msg_imu)
self.pub_mag.publish(self.msg_mag)
self.rate.sleep()
def msg_template(self):
msg_imu = Imu()
msg_mag = MagneticField();
msg_imu.header.frame_id = "robocape"
msg_mag.header.frame_id = "robocape_mag"
msg_imu.linear_acceleration_covariance = (0.1, 0.0, 0.0,
0.0, 0.1, 0.0,
0.0, 0.0, 0.1)
msg_imu.angular_velocity_covariance = (1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0)
msg_imu.orientation_covariance = (10.0, 0.0, 0.0,
0.0, 10.0, 0.0,
0.0, 0.0, 10.0)
msg_mag.magnetic_field_covariance = (10.0, 0.0, 0.0,
0.0, 10.0, 0.0,
0.0, 0.0, 10.0)
return (msg_imu, msg_mag)
def pack_message(self):
Euler = [0.0, 0.0, 0.0]
Compassmag = [0.0, 0.0, 0.0]
now = rospy.get_rostime()
self.msg_imu.header.stamp.secs = now.secs
self.msg_imu.header.stamp.nsecs = now.nsecs
self.msg_mag.header.stamp.secs = now.secs
self.msg_mag.header.stamp.nsecs = now.nsecs
self.msg_imu.linear_acceleration.x = self.imu.accel[0]
self.msg_imu.linear_acceleration.y = self.imu.accel[1]
self.msg_imu.linear_acceleration.z = self.imu.accel[2]
self.msg_imu.angular_velocity.x = self.imu.gyro[0]
self.msg_imu.angular_velocity.y = self.imu.gyro[1]
self.msg_imu.angular_velocity.z = self.imu.gyro[2]
Compassmag[0] = self.imu.compass[0] - self.mag_offset[0];
Compassmag[1] = self.imu.compass[1] + self.mag_offset[1];
Compassmag[2] = self.imu.compass[2] - self.mag_offset[2];
#Euler = self.madgwick.EulerUpdateFilter(self.imu.gyro, self.imu.accel, self.imu.compass)
#Euler = self.madgwick.EulerUpdateFilterSimple(self.imu.gyro, self.imu.accel, Compassmag)
accel = {'x':self.imu.accel[0]/9.81,'y':self.imu.accel[1]/9.81,'z':self.imu.accel[2]/9.81};
gyro = {'x':self.imu.gyro[0],'y':self.imu.gyro[1],'z':self.imu.gyro[2]};
compass = {'x':Compassmag[0]/100,'y':Compassmag[1]/100,'z':Compassmag[2]/100};
self.quaternion.update(accel, gyro, compass)
Eulerdi = self.quaternion.to_euler()
rad2deg = 57.2957795130824
Euler[0] = Eulerdi['x'] * rad2deg
Euler[1] = Eulerdi['y'] * rad2deg
Euler[2] = Eulerdi['z'] * rad2deg
self.msg_imu.orientation.x = Euler[0];
self.msg_imu.orientation.y = Euler[1];
self.msg_imu.orientation.z = Euler[2];
self.msg_imu.orientation.w = 0;
self.msg_mag.magnetic_field.x = Compassmag[0];
self.msg_mag.magnetic_field.y = Compassmag[1];
self.msg_mag.magnetic_field.z = Compassmag[2];
class ImuHandler:
def __init__(self):
#publishing rate
self.rate = 30 #30Hz
node_name = 'imu_mpu9150_handler'
rospy.init_node(node_name)
rospy.loginfo(rospy.get_caller_id() +
'Initialising {} node'.format(node_name))
# Get all parameters from config (rosparam)
address = int(rospy.get_param('imu/address', 0x69))
gyro_scale = int(rospy.get_param('imu/gyroscope/scale', 250))
acc_scale = int(rospy.get_param('imu/accelerometer/scale', 2))
# Initialize Madgwick Algorithm
#self.madgwick = MadgwickAHRS(1/30, 2.7101, 0.2);
self.quaternion = Madgwick(0.041, self.rate)
rospy.loginfo('Test {} address'.format(address))
self.imu = Mpu9150(address, gyro_scale, acc_scale)
#self.imu = Mpu9250(gyro_scale, acc_scale)
self.imu.calibrate()
rospy.loginfo(
"Compass calibration; move the IMU in random positions during the calibration"
)
(self.mag_offset, self.mag_scale) = self.imu.magCalibration()
rospy.loginfo("Calibration done")
def publish(self):
queue_size = rospy.get_param('imu/queue', 1)
self.pub_imu = rospy.Publisher('imu_readings',
Imu,
queue_size=queue_size)
self.pub_mag = rospy.Publisher('mag_readings',
MagneticField,
queue_size=queue_size)
#self.rate = rospy.Rate(rospy.get_param('imu/rate', 10))
#self.rate = rospy.Rate(20)
self.rate = rospy.Rate(30)
(self.msg_imu, self.msg_mag) = self.msg_template()
while not rospy.is_shutdown():
#rospy.loginfo(rospy.get_caller_id() + "publishing IMU data")
self.imu.update()
self.pack_message()
self.pub_imu.publish(self.msg_imu)
self.pub_mag.publish(self.msg_mag)
self.rate.sleep()
def msg_template(self):
msg_imu = Imu()
msg_mag = MagneticField()
msg_imu.header.frame_id = "robocape"
msg_mag.header.frame_id = "robocape_mag"
msg_imu.linear_acceleration_covariance = (0.1, 0.0, 0.0, 0.0, 0.1, 0.0,
0.0, 0.0, 0.1)
msg_imu.angular_velocity_covariance = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0)
msg_imu.orientation_covariance = (10.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0,
0.0, 10.0)
msg_mag.magnetic_field_covariance = (10.0, 0.0, 0.0, 0.0, 10.0, 0.0,
0.0, 0.0, 10.0)
return (msg_imu, msg_mag)
def pack_message(self):
Euler = [0.0, 0.0, 0.0]
Compassmag = [0.0, 0.0, 0.0]
now = rospy.get_rostime()
self.msg_imu.header.stamp.secs = now.secs
self.msg_imu.header.stamp.nsecs = now.nsecs
self.msg_mag.header.stamp.secs = now.secs
self.msg_mag.header.stamp.nsecs = now.nsecs
self.msg_imu.linear_acceleration.x = self.imu.accel[0]
self.msg_imu.linear_acceleration.y = self.imu.accel[1]
self.msg_imu.linear_acceleration.z = self.imu.accel[2]
self.msg_imu.angular_velocity.x = self.imu.gyro[0]
self.msg_imu.angular_velocity.y = self.imu.gyro[1]
self.msg_imu.angular_velocity.z = self.imu.gyro[2]
Compassmag[0] = self.imu.compass[0] - self.mag_offset[0]
Compassmag[1] = self.imu.compass[1] + self.mag_offset[1]
Compassmag[2] = self.imu.compass[2] - self.mag_offset[2]
#Euler = self.madgwick.EulerUpdateFilter(self.imu.gyro, self.imu.accel, self.imu.compass)
#Euler = self.madgwick.EulerUpdateFilterSimple(self.imu.gyro, self.imu.accel, Compassmag)
accel = {
'x': self.imu.accel[0] / 9.81,
'y': self.imu.accel[1] / 9.81,
'z': self.imu.accel[2] / 9.81
}
gyro = {
'x': self.imu.gyro[0],
'y': self.imu.gyro[1],
'z': self.imu.gyro[2]
}
compass = {
'x': Compassmag[0] / 100,
'y': Compassmag[1] / 100,
'z': Compassmag[2] / 100
}
self.quaternion.update(accel, gyro, compass)
Eulerdi = self.quaternion.to_euler()
rad2deg = 57.2957795130824
Euler[0] = Eulerdi['x'] * rad2deg
Euler[1] = Eulerdi['y'] * rad2deg
Euler[2] = Eulerdi['z'] * rad2deg
self.msg_imu.orientation.x = Euler[0]
self.msg_imu.orientation.y = Euler[1]
self.msg_imu.orientation.z = Euler[2]
self.msg_imu.orientation.w = 0
self.msg_mag.magnetic_field.x = Compassmag[0]
self.msg_mag.magnetic_field.y = Compassmag[1]
self.msg_mag.magnetic_field.z = Compassmag[2]
imu.compass[0] -= mag_offset[0];
imu.compass[1] += mag_offset[1];
imu.compass[2] -= mag_offset[2];
#accel[0] = imu.accel[0]/9.81
#accel[1] = imu.accel[1]/9.81
#accel[2] = imu.accel[2]/9.81
#compass[0] = imu.compass[0]/100
#compass[1] = imu.compass[1]/100
#compass[2] = imu.compass[2]/100
accel = {'x':imu.accel[0]/9.81,'y':imu.accel[1]/9.81,'z':imu.accel[2]/9.81};
gyro = {'x':imu.gyro[0],'y':imu.gyro[1],'z':imu.gyro[2]};
compass = {'x':imu.compass[0]/100,'y':imu.compass[1]/100,'z':imu.compass[2]/100};
quaternion.update(accel, gyro)
Eulerdi = quaternion.to_euler()
Euler[0] = Eulerdi['x'] * (180/3.14159265358979)
Euler[1] = Eulerdi['y'] * (180/3.14159265358979)
Euler[2] = Eulerdi['z'] * (180/3.14159265358979)
#(SEq, b, wb) = madgwick.filterUpdate(imu.gyro, accel, compass, SEq, b, wb)
#Euler = madgwick.quatern2euler(madgwick.quaternConj(SEq))
#Euler[0] *= (180/3.14159265358979)
#Euler[1] *= (180/3.14159265358979)
#Euler[2] *= (180/3.14159265358979)
#print Eulerdi
print Euler
#Euler2 = madgwick.EulerUpdateFilter(imu.gyro, imu.accel, imu.compass)
#print Euler2
#print imu.raw_data();
time.sleep(0.02);
length = time.time() - begin
'y': imu.accel[1] / 9.81,
'z': imu.accel[2] / 9.81
}
gyro = {
'x': imu.gyro[0],
'y': imu.gyro[1],
'z': imu.gyro[2]
}
compass = {
'x': imu.compass[0] / 100,
'y': imu.compass[1] / 100,
'z': imu.compass[2] / 100
}
quaternion.update(accel, gyro)
Eulerdi = quaternion.to_euler()
Euler[0] = Eulerdi['x'] * (180 / 3.14159265358979)
Euler[1] = Eulerdi['y'] * (180 / 3.14159265358979)
Euler[2] = Eulerdi['z'] * (180 / 3.14159265358979)
#(SEq, b, wb) = madgwick.filterUpdate(imu.gyro, accel, compass, SEq, b, wb)
#Euler = madgwick.quatern2euler(madgwick.quaternConj(SEq))
#Euler[0] *= (180/3.14159265358979)
#Euler[1] *= (180/3.14159265358979)
#Euler[2] *= (180/3.14159265358979)
#print Eulerdi
print Euler
#Euler2 = madgwick.EulerUpdateFilter(imu.gyro, imu.accel, imu.compass)
#print Euler2
#print imu.raw_data();
time.sleep(0.02)
length = time.time() - begin