def main():
rospy.init_node("imu")
port = rospy.get_param("~port", "GPG3_AD1")
sensor = InertialMeasurementUnit(bus=port)
pub_imu = rospy.Publisher("~imu", Imu, queue_size=10)
pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10)
pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10)
br = TransformBroadcaster()
msg_imu = Imu()
msg_temp = Temperature()
msg_magn = MagneticField()
hdr = Header(stamp=rospy.Time.now(), frame_id="IMU")
rate = rospy.Rate(rospy.get_param('~hz', 30))
while not rospy.is_shutdown():
q = sensor.read_quaternion() # x,y,z,w
mag = sensor.read_magnetometer() # micro Tesla (µT)
gyro = sensor.read_gyroscope() # deg/second
accel = sensor.read_accelerometer() # m/s²
temp = sensor.read_temperature() # °C
msg_imu.header = hdr
hdr.stamp = rospy.Time.now()
msg_temp.header = hdr
msg_temp.temperature = temp
pub_temp.publish(msg_temp)
msg_imu.header = hdr
msg_imu.linear_acceleration.x = accel[0]
msg_imu.linear_acceleration.y = accel[1]
msg_imu.linear_acceleration.z = accel[2]
msg_imu.angular_velocity.x = math.radians(gyro[0])
msg_imu.angular_velocity.y = math.radians(gyro[1])
msg_imu.angular_velocity.z = math.radians(gyro[2])
msg_imu.orientation.w = q[3]
msg_imu.orientation.x = q[0]
msg_imu.orientation.y = q[1]
msg_imu.orientation.z = q[2]
pub_imu.publish(msg_imu)
msg_magn.header = hdr
msg_magn.magnetic_field.x = mag[0] * 1e-6
msg_magn.magnetic_field.y = mag[1] * 1e-6
msg_magn.magnetic_field.z = mag[2] * 1e-6
pub_magn.publish(msg_magn)
transform = TransformStamped(header=Header(stamp=rospy.Time.now(),
frame_id="world"),
child_frame_id="IMU")
transform.transform.rotation = msg_imu.orientation
br.sendTransformMessage(transform)
rate.sleep()
def configure_sensors(self):
bp = self.BP
self.config = {
} #create a dictionary that represents if the sensor is Configured
#set up colour sensor
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config['colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
self.config[
'colour'] = SensorStatus.DISABLED #SensorStatus.DISABLED
#set up ultrasonic
try:
bp.set_sensor_type(self.ultra, bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(1.5)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
self.config['ultra'] = SensorStatus.DISABLED
#set up thermal
try:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
self.__start_thermal_infrared_thread()
except Exception as error:
self.log("Thermal Sensor not found")
self.config['thermal'] = SensorStatus.DISABLED
#set up imu
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU sensor not found")
self.config['imu'] = SensorStatus.DISABLED
bp.set_motor_limits(self.mediummotor, 100,
600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
def __init__(self, lookback, calib_it):
self.imu = InertialMeasurementUnit()
self.imu.BNO055.get_calibration_status()
self.imu.BNO055.get_calibration()
sat = time.time()
calib = []
gyro_calib = []
for _ in range(calib_it):
calib.append(self.imu.read_linear_acceleration())
gyro_calib.append(self.imu.read_gyroscope())
eat = time.time()
self.dat = (eat - sat) / calib_it
self.calib = np.average(calib, axis=0)
print(self.calib)
self.gyro_calib = np.average(gyro_calib, axis=0)
print(self.gyro_calib)
self.lookback = lookback
self.__v = 0.0
import time
import numpy as np
from easygopigo3 import EasyGoPiGo3
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="GPG3_AD1")
print("This is the second test")
gpg = EasyGoPiGo3()
go = True
tstart = time.time()
n = 0
fdist = 1
tvec = []
tvec.append(0)
xddotvec = []
inac = imu.read_linear_acceleration()
inacx = inac[0]
#oldacx = inacx
xddotvec.append(inacx)
xdotvec = []
tvvec = []
tvvec.append(0)
xdotvec.append(0)
xvec = []
xvec.append(0)
while go == True:
n = n + 1
#print(t)
gpg.forward()
def configure_sensors(self,
motorports,
sensorports={
'thermal': None,
'colour': None,
'ultra': None,
'imu': 0
}):
bp = self.BP
self.thermal_thread = None
self.rightmotor = motorports['rightmotor']
self.leftmotor = motorports['leftmotor']
self.largemotors = motorports['rightmotor'] + motorports['leftmotor']
self.mediummotor = motorports['mediummotor']
#set up thermal - thermal sensor uses a thread because else it disables motors
self.thermal = sensorports['thermal']
self.config['thermal'] = SensorStatus.DISABLED
if self.thermal:
try:
if self.thermal_thread:
self.CurrentCommand = 'exit'
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
self.__start_thermal_infrared_thread()
except Exception as error:
self.log("Thermal Sensor not found")
#configure colour sensor
self.colour = sensorports['colour']
self.config['colour'] = SensorStatus.DISABLED
if self.colour:
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config[
'colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
#set up ultrasonic
self.ultra = sensorports['ultra']
self.config['ultra'] = SensorStatus.DISABLED
if self.ultra:
try:
bp.set_sensor_type(self.ultra,
bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(2)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
#set up imu
self.imu = sensorports['imu']
self.config['imu'] = SensorStatus.DISABLED
if self.imu:
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.log("IMU sensor not found")
self.config['imu'] = SensorStatus.DISABLED
bp.set_motor_limits(self.mediummotor, 100,
600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
def __init__(self):
self.imu = InertialMeasurementUnit(bus = "RPI_1") #RPI_1 GPG3_AD1
#!/usr/bin/env python
from __future__ import print_function
from __future__ import division
import math
import numpy as np
import time
from di_sensors.inertial_measurement_unit import InertialMeasurementUnit
imu = InertialMeasurementUnit(bus="RPI_1") #RPI_1 GPG3_AD1
x = []
y = []
z = []
corr_x = []
corr_y = []
corr_z = []
i = 0
j = 0
while j < 10:
mag = imu.read_magnetometer()
x.append(mag[0])
y.append(mag[1])
z.append(mag[2])
j += 1
del x[0]
del y[0]
del z[0]
while i < 1500:
mag = imu.read_magnetometer()
def __init__(self):
super().__init__()
self.imu = InertialMeasurementUnit(bus="GPG3_AD1")
self.ds = EasyDistanceSensor()
def configure_sensors(self, motorports, sensorports):
bp = self.BP
self.thread_running = False #end thread if its still running
self.rightmotor = motorports['rightmotor']
self.leftmotor = motorports['leftmotor']
self.largemotors = motorports['rightmotor'] + motorports['leftmotor']
self.mediummotor = motorports['mediummotor']
#set up thermal - thermal sensor uses a thread because else it disables motors
self.config['thermal'] = SensorStatus.DISABLED; self.thermal = None
if 'thermal' in sensorports:
self.thermal = sensorports['thermal']
if self.thermal != None:
try:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.I2C, [0, 20])
time.sleep(1)
self.config['thermal'] = SensorStatus.ENABLED
except Exception as error:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.NONE)
if self.config['thermal'] == SensorStatus.ENABLED:
self.get_thermal_sensor() #do one read
if self.config['thermal'] < SensorStatus.DISABLED:
print("STARTING THERMAL THREAD")
self.__start_thermal_infrared_thread() #thread is started
else:
bp.set_sensor_type(self.thermal, bp.SENSOR_TYPE.NONE)
#setup colour sensor
self.config['colour'] = SensorStatus.DISABLED; self.colour = None
if 'colour' in sensorports:
self.colour = sensorports['colour']
if self.colour:
try:
bp.set_sensor_type(self.colour, bp.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(1)
self.config['colour'] = SensorStatus.ENABLED #SensorStatus.ENABLED
except Exception as error:
self.log("Colour Sensor not found")
#set up ultrasonic
self.config['ultra'] = SensorStatus.DISABLED; self.ultra = None
if 'ultra' in sensorports:
self.ultra = sensorports['ultra']
if self.ultra:
try:
bp.set_sensor_type(self.ultra, bp.SENSOR_TYPE.EV3_ULTRASONIC_CM)
time.sleep(2)
self.config['ultra'] = SensorStatus.ENABLED
except Exception as error:
self.log("Ultrasonic Sensor not found")
#set up imu
self.config['imu'] = SensorStatus.DISABLED; self.imu = None
if 'imu' in sensorports:
self.imu = sensorports['imu']
if self.imu:
try:
self.imu = InertialMeasurementUnit()
time.sleep(1)
self.config['imu'] = SensorStatus.ENABLED
except Exception as error:
self.config['imu'] = SensorStatus.DISABLED
self.log("IMU sensor not found")
bp.set_motor_limits(self.mediummotor, 100, 600) #set power / speed limit
self.Configured = True #there is a 4 second delay - before robot is Configured
return
