def imu_publisher():
global accl_x, accl_y, accl_z, gyro_x, gyro_y, gyro_z, comp_x, comp_y, comp_z
rospy.init_node(
'Imu_mpu9250_node', anonymous=False
) # if anonymous=True is to ensure that node is unique by adding random number in the end
r = rospy.Rate(20.0)
current_time = rospy.Time.now()
curn_time = time.time()
last_time = time.time()
new_time = 0.0
while not rospy.is_shutdown():
current_time = rospy.Time.now()
read_imu_raw_data()
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.angular_velocity = Vector3(gyro_x, gyro_y, gyro_z)
imu.linear_acceleration = Vector3(accl_x, accl_y, accl_z)
imu_data_pub.publish(imu)
mag = MagneticField()
mag.header.stamp = current_time
mag.header.frame_id = "imu/mag"
mag.magnetic_field = Vector3(comp_x, comp_y, comp_z)
imu_mag_pub.publish(mag)
r.sleep()
def publish_sensor_data(self, sensor_data):
acc = np.array(sensor_data.acceleration.data) / 1000.0 * g
ang_vel = np.array(sensor_data.angular_vel.data) / 180.0 * np.pi
# ang_vel = rotate_v(ang_vel.tolist(), self.rotation)
msg = Imu()
msg.header.frame_id = self.base_frame_id
msg.header.stamp = rospy.Time.now()
msg.linear_acceleration = Vector3(*acc)
# It happens that sensor_data.quaterion and pose.quaternion are NOT in the same frame
q = sensor_data.quaternion.data
q = [q[1], q[2], q[3], q[0]]
msg.orientation = Quaternion(*q)
# msg.orientation = Quaternion(*self.orientation)
msg.angular_velocity = Vector3(*ang_vel)
msg.linear_acceleration_covariance = self.linear_acceleration_cov
msg.orientation_covariance = self.orientation_cov
msg.angular_velocity_covariance = self.angular_velocity_cov
self.imu_pub.publish(msg)
mag = np.array(sensor_data.magnetic.data) * 1e-6
m_msg = MagneticField()
m_msg.header = msg.header
m_msg.magnetic_field = Vector3(*mag)
m_msg.magnetic_field_covariance = self.magnetic_field_cov
self.mag_pub.publish(m_msg)
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def get_mag(imu_raw):
mag_out = MagneticField()
mag_out.header.stamp=rospy.Time.now()
mag=Vector3()
mag.x = float(imu_raw[4])
mag.y = float(imu_raw[5])
mag.z = float(imu_raw[6])
mag_out.magnetic_field=mag
return mag_out
def callbackImu(msg):
# Grab accelerometer and gyro data.
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = 'imu_link'
q = np.array([msg.quaternion.x, msg.quaternion.y, msg.quaternion.z, msg.quaternion.w])
q = normalize(q)
euler = tf.transformations.euler_from_quaternion(q)
q = tf.transformations.quaternion_from_euler(-(euler[0] + math.pi / 2), euler[1], euler[2] - math.pi)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
euler_new = tf.transformations.euler_from_quaternion(q)
rospy.loginfo("euler[0] = %s, euler[1] = %s, euler = [2] = %s",
str(euler_new[0]), str(euler_new[1]), str(euler_new[2]))
imu_msg.orientation_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
imu_msg.angular_velocity = msg.gyro
imu_msg.angular_velocity_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
# Estimate gyro bias.
global gyro_readings
global MAX_GYRO_READINGS
global gyro_bias
if len(gyro_readings) < MAX_GYRO_READINGS:
gyro_readings.append(imu_msg.angular_velocity.z)
elif math.isnan(gyro_bias):
gyro_bias = sum(gyro_readings)/MAX_GYRO_READINGS
if not math.isnan(gyro_bias):
imu_msg.angular_velocity.z -= gyro_bias
imu_msg.linear_acceleration = msg.accelerometer
imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \
0, 0.90, 0, \
0, 0, 0.90]
# Grab magnetometer data.
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.magnetic_field = msg.magnetometer
# TODO(gbrooks): Add covariance.
# Publish sensor data.
imu_pub.publish(imu_msg)
mag_pub.publish(mag_msg)
def main():
rospy.init_node('em7180')
publisher = rospy.Publisher('imu/mag', MagneticField, queue_size=10)
# transformed_publisher = rospy.Publisher('imu/magTransformed', Vector3Stamped, queue_size=10)
rate = rospy.Rate(10)
em7180 = EM7180_Master(MAG_RATE, ACCEL_RATE, GYRO_RATE, BARO_RATE,
Q_RATE_DIVISOR)
listener = tf.TransformListener()
listener.waitForTransform("em7180_link", "imu_0", rospy.Time(0),
rospy.Duration(1.0))
# Start the EM7180 in master mode
if not em7180.begin():
print(em7180.getErrorString())
exit(1)
while not rospy.is_shutdown():
em7180.checkEventStatus()
if em7180.gotError():
print('ERROR: ' + em7180.getErrorString())
exit(1)
if em7180.gotMagnetometer():
mx, my, mz = em7180.readMagnetometer()
magnetic_vector = Vector3Stamped()
magnetic_vector.header.stamp = rospy.Time.now()
magnetic_vector.header.frame_id = "em7180_link"
magnetic_vector.vector.x = mx
magnetic_vector.vector.y = my
magnetic_vector.vector.z = mz
magnetic_vector_transformed = tf.transformVector3(
"imu_0", magnetic_vector)
magnetic_field_msg = MagneticField()
magnetic_field_msg.header = magnetic_vector_transformed.header
magnetic_field_msg.magnetic_field = magnetic_vector_transformed.vector
magnetic_field_msg.magnetic_field_covariance = [
700, 0, 0, 0, 700, 0, 0, 0, 700
]
publisher.publish(magnetic_field_msg)
rate.sleep()
def publish(self, data):
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = data.getOrientation()
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def callbackImu(msg):
# Grab accelerometer and gyro data.
imu_msg = Imu()
imu_msg.header = msg.header
imu_msg.header.frame_id = 'imu_link'
imu_msg.orientation_covariance = [0.09, 0, 0, \
0, 0.09, 0, \
0, 0, 0.09]
imu_msg.angular_velocity = msg.gyro
imu_msg.angular_velocity_covariance = [0.9, 0, 0, \
0, 0.9, 0, \
0, 0, 0.9]
# Estimate gyro bias.
global gyro_readings
global MAX_GYRO_READINGS
global gyro_bias
if len(gyro_readings) < MAX_GYRO_READINGS:
gyro_readings.append(imu_msg.angular_velocity.z)
print(len(gyro_readings))
elif math.isnan(gyro_bias):
gyro_bias = sum(gyro_readings) / MAX_GYRO_READINGS
if not math.isnan(gyro_bias):
imu_msg.angular_velocity.z -= gyro_bias
imu_msg.linear_acceleration = msg.accelerometer
imu_msg.linear_acceleration_covariance = [0.90, 0, 0, \
0, 0.90, 0, \
0, 0, 0.90]
# Grab magnetometer data.
mag_msg = MagneticField()
mag_msg.header = msg.header
mag_msg.magnetic_field = msg.magnetometer
# TODO(gbrooks): Add covariance.
# Publish sensor data.
imu_pub.publish(imu_msg)
mag_pub.publish(mag_msg)
def parse_imu_data(imu_data):
rospy.loginfo(imu_data)
imu_msg = Imu()
mag_msg = MagneticField()
mag_field = Vector3()
orient_field = Quaternion()
omega_dot_field = Vector3()
v_dot_field = Vector3()
imu_msg.header.stamp = rospy.Time.now()
mag_msg.header.stamp = rospy.Time.now()
sections = line.split(',')
yaw = radians(float(sections[1]))
pitch = radians(float(sections[2]))
roll = radians(float(sections[3]))
w, x, y, z = euler_to_quaternion(yaw, pitch, roll)
orient_field.x = x
orient_field.y = y
orient_field.z = z
orient_field.w = w
mag_field.x = float(sections[4])
mag_field.y = float(sections[5])
mag_field.z = float(sections[6])
v_dot_field.x = float(sections[7])
v_dot_field.y = float(sections[8])
v_dot_field.z = float(sections[9])
omega_dot_field.x = float(sections[10])
omega_dot_field.y = float(sections[11])
raw_omega_dot_Z = sections[12]
omega_dot_field.z = float(raw_omega_dot_Z[:raw_omega_dot_Z.find("*")])
imu_msg.orientation = orient_field
imu_msg.angular_velocity = omega_dot_field
imu_msg.linear_acceleration = v_dot_field
mag_msg.magnetic_field = mag_field
return imu_msg, mag_msg
def magnetometer():
i2c = board.I2C() # uses board.SCL and board.SDA
sensor = adafruit_lis3mdl.LIS3MDL(i2c)
rospy.loginfo("Initializing mag publisher")
mag_pub = rospy.Publisher('/mag', MagneticField, queue_size=5)
rospy.loginfo("Publishing MagneticField at: " + mag_pub.resolved_name)
rospy.init_node('mag_node')
rate = rospy.Rate(10) # 50hz
while not rospy.is_shutdown():
mag_x, mag_y, mag_z = sensor.magnetic
mag_x, mag_y, mag_z = calibrate(mag_x, mag_y, mag_z)
rospy.loginfo('X:{0:10.2f}, Y:{1:10.2f}, Z:{2:10.2f} uT'.format(mag_x, mag_y, mag_z))
rospy.loginfo("")
mag = MagneticField()
mag.header.stamp = rospy.Time.now()
mag.header.frame_id = 'LIS3MDL Triple-axis Magnetometer'
mag.magnetic_field_covariance = 0
mag.magnetic_field = create_vector3(mag_x, mag_y, mag_z)
mag_pub.publish(mag)
rate.sleep()
def run(self):
# parse time from log and create rostime instance
start_time = datetime.strptime(self.log_filename[4:18],
'%Y%m%d_%H%M%S')
ros_start_time = rospy.Time(secs=mktime(start_time.timetuple()))
bag = rosbag.Bag(self.save_folder + self.bag_name, 'w')
SCAN = 0
TIME = 1
WRITE = 2
CONTINUE = -1
STATE = CONTINUE
with open(self.save_folder + self.lidar_filename) as lidarData:
initialTimeStamp = 0
Laserscan_msg = LaserScan()
# generate the constant parts of the scan messages
Laserscan_msg.header.frame_id = 'laser'
Laserscan_msg.angle_increment = .125 * math.pi / 180
Laserscan_msg.angle_min = -95 * math.pi / 180
Laserscan_msg.angle_max = 95 * math.pi / 180
Laserscan_msg.range_min = .023
Laserscan_msg.range_max = 120.
Laserscan_msg.scan_time = 0.05
Laserscan_msg.time_increment = Laserscan_msg.angle_increment / (
2 * math.pi) * Laserscan_msg.scan_time
for line in lidarData:
if STATE == SCAN:
scan = [float(x) / 1000 for x in line.split(';')
] #convert to meters and float
Laserscan_msg.ranges = scan
STATE = WRITE
elif STATE == TIME:
if initialTimeStamp == 0:
initialTimeStamp = float(line) / 1000
laser_time = ros_start_time
else:
laser_time = ros_start_time + rospy.Duration.from_sec(
float(line) / 1000 -
initialTimeStamp) # add time difference from start
Laserscan_msg.header.stamp = laser_time
STATE = CONTINUE
if STATE == CONTINUE:
if re.match('\[timestamp\]', line):
STATE = TIME
elif re.match('\[scan\]', line):
STATE = SCAN
elif STATE == WRITE:
bag.write('/scan', Laserscan_msg, t=laser_time)
STATE = CONTINUE
#print('Laser duration: %.3s\n' % (laser_time-ros_start_time)/10**9)
with open(self.save_folder + self.log_filename, 'rb') as csvData:
excelDialect = csv.excel()
excelDialect.skipinitialspace = True
reader = csv.DictReader(csvData, dialect=excelDialect)
Imu0_msg = Imu()
Mag0_msg = MagneticField()
Imu1_msg = Imu()
Mag1_msg = MagneticField()
seq = 0
for line in reader:
line = self.fix_csv_type(line)
seq += 1
if len(line) != 0:
line_time = ros_start_time + rospy.Duration(
line['TimeFromStart']
) # add time difference from start
# define IMU0 in imu0_link
Imu0_msg.angular_velocity = Vector3(
x=line['AngRateX'] / 180,
y=line['AngRateY'] / 180,
z=line['AngRateZ'] / 180)
Imu0_msg.linear_acceleration = Vector3(x=line['AccX'] * G,
y=line['AccY'] * G,
z=line['AccZ'] * G)
Imu0_msg.header.stamp = line_time
Imu0_msg.header.frame_id = 'imu0_link'
# define MAG0 in imu0_link
Mag0_msg.magnetic_field = Vector3(
x=line['MagFieldX'] * 0.0001,
y=line['MagFieldY'] * 0.0001,
z=line['MagFieldZ'] * 0.0001)
Mag0_msg.header.stamp = line_time
Mag0_msg.header.frame_id = 'imu0_link'
# define IMU1 in imu1_link
Imu1_msg.angular_velocity = Vector3(
x=line['AngRateX_2'] / 180,
y=line['AngRateY_2'] / 180,
z=line['AngRateZ_2'] / 180)
Imu1_msg.linear_acceleration = Vector3(
x=line['AccX_2'] * G,
y=line['AccY_2'] * G,
z=line['AccZ_2'] * G)
Imu1_msg.header.stamp = line_time
Imu1_msg.header.frame_id = 'imu1_link'
# define MAG1 in imu1_link
Mag1_msg.magnetic_field = Vector3(
x=line['MagFieldX_2'] * 0.0001,
y=line['MagFieldY_2'] * 0.0001,
z=line['MagFieldZ_2'] * 0.0001)
Mag1_msg.header.stamp = line_time
Mag1_msg.header.frame_id = 'imu1_link'
# write in the bag
bag.write('/imu0/data_raw', Imu0_msg, t=line_time)
bag.write('/imu0/mag', Mag0_msg, t=line_time)
bag.write('/imu1/data_raw', Imu1_msg, t=line_time)
bag.write('/imu1/mag', Mag1_msg, t=line_time)
print 'Done'
bag.close()
def mag_callback(in_msg):
out_msg = Mag_msg()
out_msg.header = in_msg.header
out_msg.magnetic_field = in_msg.vector
set_covariance_as_identity(out_msg.magnetic_field_covariance)
mag_publisher.publish(out_msg)
def main():
#SETTINGS_FILE = "/home/ubuntu/catkin_ws/src/urc_2018/src/RTIMULib.ini"
SETTINGS_FILE = "/home/yonder/2018_URC/src/RTIMULib"
s = RTIMU.Settings(SETTINGS_FILE)
print(s)
imu = RTIMU.RTIMU(s)
if (not imu.IMUInit()):
print("Failed to init IMU")
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(False)
poll_interval = imu.IMUGetPollInterval()
pubIMU = rospy.Publisher('imu/data', Imu, queue_size=10)
#pubMag = rospy.Publisher('/imu/magnetic_field', MagneticField, queue_size=10)
#pubGPS = rospy.Publisher('/arduino/gps', NavSatFix, queue_size=10)
rospy.init_node('imu', anonymous=True)
r = rospy.Rate(1000 / imu.IMUGetPollInterval())
while not rospy.is_shutdown():
if imu.IMURead():
data = imu.getIMUData()
gyro = data["gyro"]
# subtract gravity
rot = pyquaternion.Quaternion(axis=[0, 0, 1], degrees=0)
q = pyquaternion.Quaternion(data["fusionQPose"][2],
data["fusionQPose"][1],
data["fusionQPose"][0],
data["fusionQPose"][3])
q1 = pyquaternion.Quaternion(data["fusionQPose"][0],
data["fusionQPose"][1],
data["fusionQPose"][2],
data["fusionQPose"][3])
q = q * rot
grav = q.rotate([0, 0, -1])
print(grav)
#print(np.multiply(grav, 9.80665))
accel = data["accel"]
#accel = np.add(accel, grav)
acc = Vector3()
acc.x = accel[0] * 9.80665
acc.y = accel[1] * -9.80665
acc.z = accel[2] * 9.80665
gyro = Vector3()
gyro.x = -data["gyro"][0]
gyro.y = data["gyro"][1]
gyro.z = data["gyro"][2]
msg = MagneticField()
msg.header.frame_id = IMU_FRAME_ID
mag = Vector3()
mag.x = data["compass"][0] / 1000000
mag.y = data["compass"][1] / 1000000
mag.z = data["compass"][2] / 1000000
msg.magnetic_field = mag
quat = Quaternion()
"""
quat.x = data["fusionQPose"][0]
quat.y = data["fusionQPose"][1]
quat.z = data["fusionQPose"][2]
quat.w = data["fusionQPose"][3]
"""
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
imu_dat = Imu()
imu_dat.header.frame_id = IMU_FRAME_ID
imu_dat.header.stamp = rospy.Time.now()
imu_dat.linear_acceleration = acc
imu_dat.orientation_covariance = [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1]
imu_dat.angular_velocity = gyro
imu_dat.angular_velocity_covariance = [
0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1
]
imu_dat.orientation = quat
imu_dat.linear_acceleration_covariance = [
0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1
]
pubIMU.publish(imu_dat)
print(imu_dat)
r.sleep()
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion((q.y, q.z, q.w, q.x))
#array = quaternion_from_euler(yaw + (self._angle * pi / 180), roll, pitch)
#array = quaternion_from_euler(yaw + (self._angle * pi / 180), -1 (roll - 180), -1 * pitch)
array = quaternion_from_euler((yaw + (self._angle * pi / 180)),
roll, -1 * pitch)
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
acc.x = accel[0] * 9.80665
acc.y = accel[1] * 9.80665
acc.z = accel[2] * 9.80665
gyro = Vector3()
gyro.x = -data["gyro"][0]
gyro.y = data["gyro"][1]
gyro.z = -data["gyro"][2]
msg = MagneticField()
msg.header.frame_id = IMU_FRAME_ID
mag = Vector3()
mag.x = data["compass"][0] / 1000000
mag.y = data["compass"][1] / 1000000
mag.z = data["compass"][2] / 1000000
msg.magnetic_field = mag
quat = Quaternion()
"""
quat.x = data["fusionQPose"][0]
quat.y = data["fusionQPose"][1]
quat.z = data["fusionQPose"][2]
quat.w = data["fusionQPose"][3]
"""
quat.x = q[1]
quat.y = q[2]
quat.z = q[3]
quat.w = q[0]
imu_dat = Imu()
imu_dat.header.frame_id = IMU_FRAME_ID
