def connection(self):
"""
Function that connects to IMU port. Repeats until found.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
self.log("Searching for IMU", 4)
try:
self.ser = IDMindSerial("/dev/idmind-artemis", baudrate=self.baudrate, timeout=0.5)
self.log("OpenLog Artemis found.", 4)
connected = True
except SerialException as err:
if err.errno == 2:
self.log("Openlog Artemis IMU not found", 2, alert="warn")
rospy.sleep(1)
elif "Inappropriate ioctl" in str(err):
self.log("Restarting USB port", 2, alert="error")
fd = os.open("/dev/idmind-artemis", os.O_WRONLY)
try:
fcntl.ioctl(fd, USBDEVFS_RESET, 0)
finally:
os.close(fd)
rospy.sleep(1)
else:
self.log("Exception connecting to IMU: {}".format(err), 2, alert="warn")
rospy.sleep(1)
return connected
def shutdown(self):
"""
Stops the IMU readings and closes the serial port
:return:
"""
if self.ser.send_command(bytearray([0x50, 0])) == 2:
buffer_imu = self.ser.read_command(4)
if buffer_imu[0] == 0x50:
self.ser.flush()
IDMindSerial.__del__(self)
def connection(self):
"""
Function that connects to IMU port. Tries /dev/idmind-imu (created by udev rules) and then tries all ttyACM ports
Repeats until found
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
try:
self.ser = IDMindSerial("/dev/idmind-imu",
baudrate=115200,
timeout=1)
connected = True
except SerialException:
self.log("Unable to connect to /dev/idmind-imu.", 2)
except Exception as serial_exc:
self.log(serial_exc, 2)
if not connected:
for i in range(0, 10):
try:
self.ser = IDMindSerial("/dev/ttyACM" + str(i),
baudrate=115200,
timeout=0.5)
connected = True
break
except KeyboardInterrupt:
self.log("Node shutdown by user.", 2)
raise KeyboardInterrupt()
except SerialException:
self.log("Unable to connect to /dev/ttyACM" + str(i),
2)
except Exception as serial_exc:
self.log(serial_exc, 2)
if not connected:
self.log("IMU not found. Waiting 5 secs to try again.", 1)
rospy.sleep(5)
try:
res = self.ser.command([0x50, self.mode], 4)
if ord(res[0]) == 0x50:
self.ser.flush()
else:
print "IDMindImu: Failed to set IMU mode"
except Exception:
print "IDMindImu: IMU communication failed"
return connected
def connection(self):
"""
Function that connects to IMU port, searching all available ports
Repeats until found. Flags for calibration.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
self.log("Searching for IMU", 5)
for addr in [
comport.device
for comport in serial.tools.list_ports.comports()
]:
# If the lsof call returns an output, then the port is already in use!
try:
subprocess.check_output(['lsof', '+wt', addr])
continue
except subprocess.CalledProcessError:
self.ser = IDMindSerial(addr=addr,
baudrate=115200,
timeout=0.5)
rospy.sleep(2)
self.ser.flush()
self.ser.reset_input_buffer()
self.ser.reset_output_buffer()
self.ser.write(bytearray(['f']))
rospy.sleep(0.5)
imu_data = self.ser.readline()
# self.log("Data: {}".format(imu_data), 2)
if "IDMind OpenLog Artemis" in imu_data:
connected = True
self.log("Imu found on {}".format(addr), 5)
self.publish_diagnostic(
0, "IMU Detected on {}".format(addr))
break
else:
try:
self.ser.close()
except:
pass
finally:
self.log("Imu not found on {}".format(addr), 7)
except KeyboardInterrupt:
self.log("Node shutdown by user.", 2)
raise KeyboardInterrupt()
except SerialException:
self.log("Unable to connect to " + addr, 2)
except Exception as serial_exc:
self.log(serial_exc, 2)
else:
self.log("No other devices found.", 5)
if not connected:
self.log("IMU not found. Waiting 5 secs to try again.", 1)
self.publish_diagnostic(2, "IMU not found")
rospy.sleep(5)
else:
self.calibration = True
return connected
class IDMindIMU:
"""
This class extracts data from the Sparkfun OpenLog Artemis (with ICM 20948)
The OpenLogArtemis sketch must be uploaded to the unit. It will publish to /imu the values of orientation,
angular velocity and linear acceleration.
In case the connection is lost, it will try to reconnect.
TODO: Allow for calibration of components
"""
def __init__(self):
self.ready = False
rospy.Service("~ready", Trigger, self.report_ready)
# Logging
self.logging = rospy.Publisher("/idmind_logging", Log, queue_size=10)
self.diag_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=10)
self.val_exc = 0
self.imu_data = ""
self.last_imu = Imu()
self.imu_reading = Imu()
self.calibration = True
self.tf_prefix = rospy.get_param("~tf_prefix", "")
# Connect to IMU
self.ser = None
self.connection()
self.imu_pub = rospy.Publisher("~imu", Imu, queue_size=10)
self.imu_euler_pub = rospy.Publisher("~euler_string",
String,
queue_size=10)
self.ready = True
###############
# CALLBACKS #
###############
def report_ready(self, _req):
""" Simple Service callback to show node is ready """
self.log("Replying to 'ready' request", 7)
return TriggerResponse(
self.ready,
rospy.get_name() + " is " +
("ready" if self.ready else "not ready"))
def connection(self):
"""
Function that connects to IMU port, searching all available ports
Repeats until found. Flags for calibration.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
self.log("Searching for IMU", 5)
for addr in [
comport.device
for comport in serial.tools.list_ports.comports()
]:
# If the lsof call returns an output, then the port is already in use!
try:
subprocess.check_output(['lsof', '+wt', addr])
continue
except subprocess.CalledProcessError:
self.ser = IDMindSerial(addr=addr,
baudrate=115200,
timeout=0.5)
rospy.sleep(2)
self.ser.flush()
self.ser.reset_input_buffer()
self.ser.reset_output_buffer()
self.ser.write(bytearray(['f']))
rospy.sleep(0.5)
imu_data = self.ser.readline()
# self.log("Data: {}".format(imu_data), 2)
if "IDMind OpenLog Artemis" in imu_data:
connected = True
self.log("Imu found on {}".format(addr), 5)
self.publish_diagnostic(
0, "IMU Detected on {}".format(addr))
break
else:
try:
self.ser.close()
except:
pass
finally:
self.log("Imu not found on {}".format(addr), 7)
except KeyboardInterrupt:
self.log("Node shutdown by user.", 2)
raise KeyboardInterrupt()
except SerialException:
self.log("Unable to connect to " + addr, 2)
except Exception as serial_exc:
self.log(serial_exc, 2)
else:
self.log("No other devices found.", 5)
if not connected:
self.log("IMU not found. Waiting 5 secs to try again.", 1)
self.publish_diagnostic(2, "IMU not found")
rospy.sleep(5)
else:
self.calibration = True
return connected
#########################
# AUXILIARY FUNCTIONS #
#########################
def log(self, msg, msg_level, log_level=-1, alert="info"):
"""
Log function that publish in screen and in topic
:param msg: Message to be published
:param msg_level: Message level (1-10, where 1 is most important)
:param log_level: Message level for logging (1-10, optional, -1 uses the same as msg_level)
:param alert: Alert level of message - "info", "warn" or "error"
:return:
"""
if VERBOSE >= msg_level:
if alert == "info":
rospy.loginfo("{}: {}".format(rospy.get_name(), msg))
elif alert == "warn":
rospy.logwarn("{}: {}".format(rospy.get_name(), msg))
elif alert == "error":
rospy.logerr("{}: {}".format(rospy.get_name(), msg))
if LOGS >= (log_level if log_level != -1 else msg_level):
self.logging.publish(rospy.Time.now().to_sec(), rospy.get_name(),
msg)
def get_imu_data(self):
"""
This method will read the buffer from the IMU until it finds EOL.
This should be able to handle the message flow
"""
data = ""
self.ser.write(bytearray(['r']))
while not rospy.is_shutdown():
data += self.ser.readline()
if len(data) > 0 and data[-1] == "\n":
self.log("Got a complete line", 9)
self.imu_data = data[:-2]
return self.imu_data
else:
self.log("Waiting for a complete line", 7, alert="warn")
return data
def parse_msg(self):
try:
dev_data = self.imu_data.split(" ")
msg = []
for elem in dev_data:
msg.append(float(elem.split(":")[1]))
return msg
except IndexError as err:
self.log("Short IMU message: {}".format(err), 2, alert="warn")
except ValueError as err:
self.log("Bad IMU message: {}".format(err), 2, alert="warn")
except Exception as err:
self.log("Unknown Exception parsing IMU message: {}".format(err),
2,
alert="error")
raise err
def compute_imu_msg(self):
# Create new message
try:
# Get data
self.get_imu_data()
[
q1, q2, q3, accuracy, roll, pitch, yaw, w_x, w_y, w_z, acc_x,
acc_y, acc_z
] = self.parse_msg()
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix + "imu"
imu_msg.header.stamp = rospy.Time.now() # + rospy.Duration(0.5)
if ((q1 * q1) + (q2 * q2) + (q3 * q3)) < 1:
q4 = [
q1, q2, q3,
np.sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)))
]
else:
self.log("Inconsistent readings from IMU", 2, alert="warn")
return True
# Compute the Orientation Quaternion
new_q = Quaternion()
new_q.x = q1
new_q.y = q2
new_q.z = q3
new_q.w = q4
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001
]
# Angular Velocity
imu_msg.angular_velocity.x = round(w_x, 4)
imu_msg.angular_velocity.y = round(w_y, 4)
imu_msg.angular_velocity.z = round(w_z, 4)
# Datasheet says:
# - Noise Spectral Density: 0.015dps/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(0.015/np.sqrt(20), 2)
# factor = 0.02
# imu_msg.angular_velocity_covariance = [
# diag, w_x*factor, w_x*factor,
# w_y*factor, diag, w_y*factor,
# w_z*factor, w_z*factor, diag
# ]
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = 0.0001
imu_msg.angular_velocity_covariance[4] = 0.0001
imu_msg.angular_velocity_covariance[8] = 0.0001
# imu_msg.angular_velocity_covariance = [-1] * 9
# Linear Acceleration
imu_msg.linear_acceleration.x = round(acc_x, 4)
imu_msg.linear_acceleration.y = round(acc_y, 4)
imu_msg.linear_acceleration.z = round(acc_z, 4)
# imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = 9.82
# imu_msg.linear_acceleration_covariance = [-1] * 9
# Datasheet says:
# - Noise Spectral Density: 230microg/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(230e-6/np.sqrt(20), 2)/256.
# factor = 0.02/256.
# imu_msg.linear_acceleration_covariance = [
# diag, acc_x*factor, acc_x*factor,
# acc_y*factor, diag, acc_y*factor,
# acc_z*factor, acc_z*factor, diag
# ]
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.001
imu_msg.linear_acceleration_covariance[4] = 0.001
imu_msg.linear_acceleration_covariance[8] = 0.001
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion(
[new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(
r, p, y))
except SerialException as serial_exc:
self.log(
"SerialException while reading from IMU: {}".format(
serial_exc), 3)
self.calibration = True
except ValueError as val_err:
self.log("Value error from IMU data - {}".format(val_err), 5)
self.val_exc = self.val_exc + 1
except Exception as imu_exc:
self.log(imu_exc, 3)
raise imu_exc
def calibrate_imu(self):
"""
This method will save the current orientation as the offset.
All future publications will be adjusted in relation to the saved orientation
:return:
"""
self.log("Calibrating IMU", 3)
r = rospy.Rate(20)
calibrated = False
reads = 0
while not calibrated and not rospy.is_shutdown():
try:
reads = reads + 1
self.get_imu_data()
if reads > 50:
self.get_imu_data()
[
q1, q2, q3, accuracy, roll, pitch, yaw, w_x, w_y, w_z,
acc_x, acc_y, acc_z
] = self.parse_msg()
q = [
q1, q2, q3,
np.sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)))
]
calibrated = True
self.calibration = False
else:
if reads == 1:
self.log("Discarding 50 readings for calibration", 7)
# rospy.loginfo(reads)
r.sleep()
except KeyboardInterrupt:
raise KeyboardInterrupt()
def publish_diagnostic(self, level, message):
""" Auxiliary method to publish Diagnostic messages """
diag_msg = DiagnosticArray()
diag_msg.header.frame_id = "imu"
diag_msg.header.stamp = rospy.Time.now()
imu_msg = DiagnosticStatus()
imu_msg.name = "IMU"
imu_msg.hardware_id = "OpenLog Artemis IMU"
imu_msg.level = level
imu_msg.message = message
diag_msg.status.append(imu_msg)
self.diag_pub.publish(diag_msg)
def start(self):
r = rospy.Rate(20)
while not rospy.is_shutdown():
try:
self.log("Bytes waiting: {}".format(self.ser.in_waiting), 7)
if self.calibration:
self.calibrate_imu()
else:
# self.get_imu_data()
# self.parse_msg()
self.compute_imu_msg()
self.publish_diagnostic(0, "OK")
r.sleep()
except KeyboardInterrupt:
self.log("{}: Shutting down by user".format(rospy.get_name()),
2)
break
except IOError as io_exc:
self.publish_diagnostic(1, "Lost connection to IMU")
self.log("Lost connection to IMU", 3)
if not self.connection():
rospy.sleep(2)
class IDMindIMU:
"""
This class extracts data from IDMind's Imu Board for Sparkfun MP6050 IMU.
In case the connection is lost, it will try to reconnect.
"""
def __init__(self, mode="sample", freq=20.0):
"""
Initiates the IDMindImu Class, using a default "sample" mode at 20Hz
:param mode:
:param freq:
"""
self.mode = 0x01 if mode == "stream" else 0x02
self.rate = 110.0 if self.mode == "stream" else freq
self.imu_lock = Lock()
# Logging
self.logging = rospy.Publisher("/idmind_logging", Log, queue_size=10)
self.val_exc = 0
# Connect to IMU
self.connection()
self.imu_reading = Imu()
self.calibration = False
self.imu_offset = Quaternion()
self.imu_offset.w = -1
self.imu_pub = rospy.Publisher("/imu", Imu, queue_size=10)
rospy.Service("/idmind_razor/calibration", Trigger,
self.request_calibration)
def connection(self):
"""
Function that connects to IMU port. Tries /dev/idmind-imu (created by udev rules) and then tries all ttyACM ports
Repeats until found
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
try:
self.ser = IDMindSerial("/dev/idmind-imu",
baudrate=115200,
timeout=1)
connected = True
except SerialException:
self.log("Unable to connect to /dev/idmind-imu.", 2)
except Exception as serial_exc:
self.log(serial_exc, 2)
if not connected:
for i in range(0, 10):
try:
self.ser = IDMindSerial("/dev/ttyACM" + str(i),
baudrate=115200,
timeout=0.5)
connected = True
break
except KeyboardInterrupt:
self.log("Node shutdown by user.", 2)
raise KeyboardInterrupt()
except SerialException:
self.log("Unable to connect to /dev/ttyACM" + str(i),
2)
except Exception as serial_exc:
self.log(serial_exc, 2)
if not connected:
self.log("IMU not found. Waiting 5 secs to try again.", 1)
rospy.sleep(5)
try:
res = self.ser.command([0x50, self.mode], 4)
if ord(res[0]) == 0x50:
self.ser.flush()
else:
print "IDMindImu: Failed to set IMU mode"
except Exception:
print "IDMindImu: IMU communication failed"
return connected
def log(self, msg, msg_level, log_level=-1):
"""
Logging method for both verbose and logging topic
:param msg: Message to be logged/displayed
:param msg_level: if this value is lower than VERBOSE, display message on screen
:param log_level: (optional) if this value is lower than LOGS, publish message
:return:
"""
if VERBOSE >= msg_level:
rospy.loginfo("{}: {}".format(rospy.get_name(), msg))
if LOGS >= (log_level if log_level != -1 else msg_level):
self.logging.publish(rospy.Time.now().to_sec(), rospy.get_name(),
msg)
def request_calibration(self, _req):
self.calibration = True
return TriggerResponse(True, "Requesting calibration")
def read_data(self):
"""
This methods reads a full message from the buffer.
In case of "sample" mode, sends a request and returns the 10 byte response
In case of "stream" mode, reads one byte until finding @ and then reads 9 more
:return [yaw, pitch, roll]:
"""
buffer_imu = []
if self.mode == 0x02: # Request Sample
self.ser.send_command([0x40])
buffer_imu = self.ser.read_command(10)
else: # Read from stream
b = ""
while ord(b) != 0x40:
b = self.ser.read_command(1)
buffer_imu = self.ser.read_command(9)
buffer_imu = b.extend(buffer_imu)
if ord(buffer_imu[0]) == 0x40:
yaw = (pi / 180.0) * round(
self.ser.to_num(buffer_imu[1], buffer_imu[2]), 5) / 100
pitch = (pi / 180.0) * round(
self.ser.to_num(buffer_imu[3], buffer_imu[4]), 5) / 100
roll = (pi / 180.0) * round(
self.ser.to_num(buffer_imu[5], buffer_imu[7]), 5) / 100
return [yaw, pitch, roll]
else:
return ["inf", "inf", "inf"]
def calibrate_imu(self):
"""
This method will save the current orientation as the offset. All future publications will be adjusted in relation
to the saved orientation. Readings com in [yaw, pitch, roll]
:return:
"""
self.log("Calibrating IMU", 3)
r = rospy.Rate(20)
calibrated = False
reads = 0
while not calibrated and not rospy.is_shutdown():
try:
reads = reads + 1
imu_data = self.read_data()
if reads > 100:
q_data = transformations.quaternion_from_euler(
imu_data[2], imu_data[1], imu_data[0])
self.imu_offset.x = float(q_data[0])
self.imu_offset.y = float(q_data[1])
self.imu_offset.z = float(q_data[2])
self.imu_offset.w = -float(q_data[3])
calibrated = True
self.calibration = False
else:
if reads == 1:
self.log("Discarding 100 readings for calibration", 3)
r.sleep()
except KeyboardInterrupt:
raise KeyboardInterrupt()
def update_imu(self):
"""
Reads all characters in the buffer until finding \r\n
Messages should have the following format: "Q: w x y z | A: x y z | G: x y z"
:return:
"""
# Create new message
try:
imuMsg = Imu()
# Set the sensor covariances
imuMsg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
imuMsg.angular_velocity_covariance = [-1., 0, 0, 0, 0, 0, 0, 0, 0]
imuMsg.linear_acceleration_covariance = [
-1., 0, 0, 0, 0, 0, 0, 0, 0
]
imu_data = self.read_data()
if len(imu_data) == 0:
self.log("IMU is not answering", 2)
return
q_data = transformations.quaternion_from_euler(
imu_data[2], imu_data[1], imu_data[0])
q1 = Quaternion()
q1.x = float(q_data[0])
q1.y = float(q_data[1])
q1.z = float(q_data[2])
q1.w = float(q_data[3])
q_off = self.imu_offset
new_q = transformations.quaternion_multiply(
[q1.x, q1.y, q1.z, q1.w], [q_off.x, q_off.y, q_off.z, q_off.w])
imuMsg.orientation.x = new_q[0]
imuMsg.orientation.y = new_q[1]
imuMsg.orientation.z = new_q[2]
imuMsg.orientation.w = new_q[3]
# Handle message header
imuMsg.header.frame_id = "base_link_imu"
imuMsg.header.stamp = rospy.Time.now() + rospy.Duration(nsecs=5000)
self.imu_reading = imuMsg
except SerialException as serial_exc:
self.log(
"SerialException while reading from IMU: {}".format(
serial_exc), 3)
self.calibration = True
except ValueError as val_err:
self.log("Value error from IMU data - {}".format(val_err), 5)
self.val_exc = self.val_exc + 1
except Exception as imu_exc:
self.log(imu_exc, 3)
raise imu_exc
def publish_imu(self):
self.imu_pub.publish(self.imu_reading)
def start(self):
r = rospy.Rate(20)
while not rospy.is_shutdown():
try:
self.log("Bytes waiting: {}".format(self.ser.in_waiting), 5)
if self.calibration:
self.calibrate_imu()
else:
self.update_imu()
self.publish_imu()
r.sleep()
except KeyboardInterrupt:
self.log("{}: Shutting down by user".format(rospy.get_name()),
2)
self.shutdown()
break
except IOError as io_exc:
self.log("Lost connection to IMU", 3)
if not self.connection():
rospy.sleep(2)
def shutdown(self):
"""
Stops the IMU readings and closes the serial port
:return:
"""
if self.ser.send_command(bytearray([0x50, 0])) == 2:
buffer_imu = self.ser.read_command(4)
if buffer_imu[0] == 0x50:
self.ser.flush()
IDMindSerial.__del__(self)
def __del__(self):
self.shutdown()
def connection(self):
"""
Function that connects to IMU port.
Tries /dev/idmind-imu (created by udev rules) and then tries all available ports
Repeats until found. Flags for calibration.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
try:
self.ser = IDMindSerial("/dev/idmind-imu",
baudrate=115200,
timeout=1)
connected = True
except SerialException:
self.log("Unable to connect to /dev/idmind-imu.", 2)
except Exception as serial_exc:
self.log("Exception caught: {}".format(serial_exc), 2)
if not connected:
self.log("Searching on other ports", 5)
for addr in [
comport.device
for comport in serial.tools.list_ports.comports()
]:
# If the lsof call returns an output, then the port is already in use!
try:
subprocess.check_output(['lsof', '+wt', addr])
continue
except subprocess.CalledProcessError:
self.ser = IDMindSerial(addr=addr,
baudrate=115200,
timeout=0.5)
imu_data = self.ser.read_until("\r\n")
try:
if self.parse_msg(imu_data):
connected = True
self.log("Imu found on {}".format(addr), 2)
self.publish_diagnostic(
0, "IMU Detected on {}".format(addr))
break
else:
self.log("Imu not found on {}".format(addr), 5)
except IMUException:
self.log("Imu not found on {}".format(addr), 5)
except KeyboardInterrupt:
self.log("Node shutdown by user.", 2)
raise KeyboardInterrupt()
except SerialException:
self.log("Unable to connect to " + addr, 2)
except Exception as serial_exc:
self.log(serial_exc, 2)
else:
self.log("No other devices found.", 5)
if not connected:
self.log("IMU not found. Waiting 5 secs to try again.", 1)
rospy.sleep(5)
else:
self.calibration = True
return connected
class IDMindIMU:
"""
This class extracts data from the Sparkfun Razor M0 9DoF IMU.
The .ino file must be uploaded to the unit. It will publish to /imu the values of orientation, angular velocity
and linear acceleration.
In case the connection is lost, it will try to reconnect.
TODO: Allow for calibration of components
"""
def __init__(self):
self.ready = False
rospy.Service("~ready", Trigger, self.report_ready)
# Logging
self.logging = rospy.Publisher("/idmind_logging", Log, queue_size=10)
self.diag_pub = rospy.Publisher("diagnostics",
DiagnosticArray,
queue_size=10)
self.val_exc = 0
self.imu_data = ""
self.last_imu = Imu()
self.imu_reading = Imu()
self.calibration = True
self.tf_prefix = rospy.get_param("~tf_prefix", "")
# Connect to IMU
self.ser = None
self.connection()
self.tf_buffer = tf2_ros.Buffer()
self.transform_listener = tf2_ros.TransformListener(self.tf_buffer)
self.imu_pub = rospy.Publisher("{}/imu".format(rospy.get_name()),
Imu,
queue_size=10)
self.imu_euler_pub = rospy.Publisher("{}/euler_string".format(
rospy.get_name()),
String,
queue_size=10)
self.ready = True
###############
# CALLBACKS #
###############
def report_ready(self, _req):
""" Simple Service callback to show node is ready """
self.log("Replying to 'ready' request", 7)
return TriggerResponse(
self.ready,
rospy.get_name() + " is " +
("ready" if self.ready else "not ready"))
def connection(self):
"""
Function that connects to IMU port.
Tries /dev/idmind-imu (created by udev rules) and then tries all available ports
Repeats until found. Flags for calibration.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
try:
self.ser = IDMindSerial("/dev/idmind-imu",
baudrate=115200,
timeout=1)
connected = True
except SerialException:
self.log("Unable to connect to /dev/idmind-imu.", 2)
except Exception as serial_exc:
self.log("Exception caught: {}".format(serial_exc), 2)
if not connected:
self.log("Searching on other ports", 5)
for addr in [
comport.device
for comport in serial.tools.list_ports.comports()
]:
# If the lsof call returns an output, then the port is already in use!
try:
subprocess.check_output(['lsof', '+wt', addr])
continue
except subprocess.CalledProcessError:
self.ser = IDMindSerial(addr=addr,
baudrate=115200,
timeout=0.5)
imu_data = self.ser.read_until("\r\n")
try:
if self.parse_msg(imu_data):
connected = True
self.log("Imu found on {}".format(addr), 2)
self.publish_diagnostic(
0, "IMU Detected on {}".format(addr))
break
else:
self.log("Imu not found on {}".format(addr), 5)
except IMUException:
self.log("Imu not found on {}".format(addr), 5)
except KeyboardInterrupt:
self.log("Node shutdown by user.", 2)
raise KeyboardInterrupt()
except SerialException:
self.log("Unable to connect to " + addr, 2)
except Exception as serial_exc:
self.log(serial_exc, 2)
else:
self.log("No other devices found.", 5)
if not connected:
self.log("IMU not found. Waiting 5 secs to try again.", 1)
rospy.sleep(5)
else:
self.calibration = True
return connected
def log(self, msg, msg_level, log_level=-1, alert="info"):
"""
Log function that publish in screen and in topic
:param msg: Message to be published
:param msg_level: Message level (1-10, where 1 is most important)
:param log_level: Message level for logging (1-10, optional, -1 uses the same as msg_level)
:param alert: Alert level of message - "info", "warn" or "error"
:return:
"""
if VERBOSE >= msg_level:
if alert == "info":
rospy.loginfo("{}: {}".format(rospy.get_name(), msg))
elif alert == "warn":
rospy.logwarn("{}: {}".format(rospy.get_name(), msg))
elif alert == "error":
rospy.logerr("{}: {}".format(rospy.get_name(), msg))
if LOGS >= (log_level if log_level != -1 else msg_level):
self.logging.publish(rospy.Time.now().to_sec(), rospy.get_name(),
msg)
def publish_diagnostic(self, level, message):
""" Auxiliary method to publish Diagnostic messages """
diag_msg = DiagnosticArray()
diag_msg.header.frame_id = "imu"
diag_msg.header.stamp = rospy.Time.now()
imu_msg = DiagnosticStatus()
imu_msg.name = "IMU"
imu_msg.hardware_id = "Razor IMU"
imu_msg.level = level
imu_msg.message = message
diag_msg.status.append(imu_msg)
self.diag_pub.publish(diag_msg)
def parse_msg(self, imu_data):
new_q = []
a = []
w = []
dev_data = imu_data.split(" | ")
for d in dev_data:
values = d.split(" ")
if values[0] == "Q:":
q1 = Quaternion()
q1.x = float(values[2])
q1.y = float(values[3])
q1.z = float(values[4])
q1.w = float(values[1])
elif values[0] == "A:":
a = [float(values[1]), float(values[2]), float(values[3])]
elif values[0] == "G:":
w = [float(values[1]), float(values[2]), float(values[3])]
else:
self.log("Exception parsing message - {}".format(imu_data), 5)
raise IMUException(
"razor", "Exception parsing message - {}".format(imu_data))
return [q1, a, w]
def calibrate_imu(self):
"""
This method will discard initial readings
to the saved orientation
:return:
"""
self.log("Calibrating IMU", 3)
r = rospy.Rate(20)
calibrated = False
reads = 0
while not calibrated and not rospy.is_shutdown():
try:
reads = reads + 1
imu_data = self.ser.read_until("\r\n").split(" | ")
if reads > 100:
s = imu_data[0]
s_data = s.split(" ")
if s_data[0] == "Q:":
calibrated = True
self.calibration = False
else:
rospy.logwarn(
"{}: IMU is giving bad answers - {}".format(
rospy.get_name(), s_data[0]))
else:
if reads == 1:
self.log("Discarding 100 readings for calibration", 3)
# rospy.loginfo(reads)
r.sleep()
except KeyboardInterrupt:
raise KeyboardInterrupt()
def update_imu(self):
"""
Reads all characters in the buffer until finding \r\n
Messages should have the following format: "Q: w x y z | A: x y z | G: x y z"
:return:
"""
# Create new message
try:
imu_msg = Imu()
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
imu_msg.angular_velocity_covariance = [
0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02
]
imu_msg.linear_acceleration_covariance = [
0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04
]
imu_data = self.ser.read_until("\r\n")
if len(imu_data) == 0:
self.log("IMU is not answering", 2)
raise IMUException("razor", "IMU is not answering")
try:
[q, a, w] = self.parse_msg(imu_data)
imu_msg.orientation.x = q[0]
imu_msg.orientation.y = q[1]
imu_msg.orientation.z = q[2]
imu_msg.orientation.w = q[3]
imu_msg.linear_acceleration.x = a[0]
imu_msg.linear_acceleration.y = a[1]
imu_msg.linear_acceleration.z = a[2]
imu_msg.angular_velocity.x = w[0]
imu_msg.angular_velocity.y = w[1]
imu_msg.angular_velocity.z = w[2]
except IMUException as err:
raise IMUException
except Exception as err:
self.log("Exception generating IMU Message - {}".format(err),
5)
raise IMUException(
"razor",
"Exception generating IMU Message - {}".format(err))
# Handle message header
imu_msg.header.frame_id = self.tf_prefix + "imu"
imu_msg.header.stamp = rospy.Time.now() + rospy.Duration(0.5)
self.publish_euler_imu(imu_msg)
self.imu_reading = imu_msg
except SerialException as serial_exc:
self.log(
"SerialException while reading from IMU: {}".format(
serial_exc), 3)
self.calibration = True
except ValueError as val_err:
self.log("Value error from IMU data - {}".format(val_err), 5)
self.val_exc = self.val_exc + 1
except Exception as imu_exc:
self.log(imu_exc, 3)
raise imu_exc
def publish_imu(self):
self.imu_pub.publish(self.imu_reading)
def publish_euler_imu(self, imu_reading):
euler = transformations.euler_from_quaternion([
imu_reading.orientation.x, imu_reading.orientation.y,
imu_reading.orientation.z, imu_reading.orientation.w
])
imu_quaternion = "quaternion = ({}, {}, {}, {})".format(
imu_reading.orientation.x, imu_reading.orientation.y,
imu_reading.orientation.z, imu_reading.orientation.w)
imu_euler = "euler = ({}, {}, {})".format(euler[0], euler[1], euler[2])
imu_euler_deg = "euler_deg = ({}, {}, {})".format(
degrees(euler[0]), degrees(euler[1]), degrees(euler[2]))
euler_imu_string = imu_quaternion + "\n" + imu_euler + "\n" + imu_euler_deg
self.imu_euler_pub.publish(euler_imu_string)
def start(self):
r = rospy.Rate(20)
imu_exception = 0
while not rospy.is_shutdown():
try:
self.log("Bytes waiting: {}".format(self.ser.in_waiting), 7)
if self.calibration:
self.publish_diagnostic(1, "Calibrating")
self.calibrate_imu()
else:
self.update_imu()
self.publish_imu()
imu_exception = 0
self.publish_diagnostic(0, "OK")
r.sleep()
except IMUException as err:
imu_exception = imu_exception + 1
self.publish_diagnostic(1, "Error reading IMU")
if imu_exception > 10:
self.log("Failure connecting to IMU. Restarting..", 3)
self.publish_diagnostic(2, "Failure connecting to IMU")
if not self.connection():
rospy.sleep(2)
except KeyboardInterrupt:
self.log("{}: Shutting down by user".format(rospy.get_name()),
2)
break
except IOError as io_exc:
self.publish_diagnostic(1, "Lost connection to IMU")
self.log("Lost connection to IMU", 3)
if not self.connection():
rospy.sleep(2)
class IDMindIMU:
"""
This class extracts data from the Sparkfun OpenLog Artemis (with ICM 20948)
The OpenLogArtemis sketch must be uploaded to the unit. It will publish to /imu the values of orientation,
angular velocity and linear acceleration.
In case the connection is lost, it will try to reconnect.
TODO: Allow for calibration of components
"""
def __init__(self):
# Logging
self.ready = False
rospy.Service("~ready", Trigger, self.report_ready)
self.logging = rospy.Publisher("/idmind_logging", Log, queue_size=10)
self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
self.euler = [0.0]*3
self.imu_data = ""
self.calibration = True
self.last_imu = [Imu()]*10
self.acc_hist = [[0, 0, 9.82]] * 100
self.acc_ratio = 1.0
self.tf_prefix = rospy.get_param("~tf_prefix", "")
self.imu_pub = rospy.Publisher("~imu", Imu, queue_size=10)
self.imu_euler_pub = rospy.Publisher("~euler_string", String, queue_size=10)
self.baudrate = rospy.get_param("~baudrate", 230400)
# Connect to IMU
self.fails = 0
self.ser = None
self.connection()
self.imu_rate = rospy.get_param("~imu_rate", 100)
self.control_freq = rospy.get_param("~control_freq", 100)
self.dynamic_server = dynamic_reconfigure.server.Server(ImuParamsConfig, callback=self.update_imu_params)
self.update_rates = True
self.ready = True
self.log("Node is ready", 4)
###############
# CALLBACKS #
###############
def report_ready(self, _req):
""" Simple Service callback to show node is ready """
self.log("Replying to 'ready' request", 7)
return TriggerResponse(self.ready, rospy.get_name()+" is " + ("ready" if self.ready else "not ready"))
def connection(self):
"""
Function that connects to IMU port. Repeats until found.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
self.log("Searching for IMU", 4)
try:
self.ser = IDMindSerial("/dev/idmind-artemis", baudrate=self.baudrate, timeout=0.5)
self.log("OpenLog Artemis found.", 4)
connected = True
except SerialException as err:
if err.errno == 2:
self.log("Openlog Artemis IMU not found", 2, alert="warn")
rospy.sleep(1)
elif "Inappropriate ioctl" in str(err):
self.log("Restarting USB port", 2, alert="error")
fd = os.open("/dev/idmind-artemis", os.O_WRONLY)
try:
fcntl.ioctl(fd, USBDEVFS_RESET, 0)
finally:
os.close(fd)
rospy.sleep(1)
else:
self.log("Exception connecting to IMU: {}".format(err), 2, alert="warn")
rospy.sleep(1)
return connected
def update_imu_params(self, config, level):
""" Callback to an update in the motor dynamic parameters of the platform """
self.log("Updating: {} to {} and {} to {}".format(self.control_freq, config.control_freq, self.imu_rate, config.imu_freq), 2)
self.control_freq = config.control_freq
self.rate = rospy.Rate(config.control_freq)
if self.imu_rate != config.imu_freq:
self.update_rates = True
self.imu_rate = config.imu_freq
return config
#########################
# AUXILIARY FUNCTIONS #
#########################
def log(self, msg, msg_level, log_level=-1, alert="info"):
"""
Log function that publish in screen and in topic
:param msg: Message to be published
:param msg_level: Message level (1-10, where 1 is most important)
:param log_level: Message level for logging (1-10, optional, -1 uses the same as msg_level)
:param alert: Alert level of message - "info", "warn" or "error"
:return:
"""
if VERBOSE >= msg_level:
if alert == "info":
rospy.loginfo("{}: {}".format(rospy.get_name(), msg))
elif alert == "warn":
rospy.logwarn("{}: {}".format(rospy.get_name(), msg))
elif alert == "error":
rospy.logerr("{}: {}".format(rospy.get_name(), msg))
if LOGS >= (log_level if log_level != -1 else msg_level):
self.logging.publish(rospy.Time.now().to_sec(), rospy.get_name(), msg)
def calibrate_imu(self):
"""
This method will wait for the IMU to output values and wait for the values to get steady
:return:
"""
self.log("Calibrating IMU", 5)
activated = False
calibrated = False
reads = 0
# Wait for the IMU to start outputting values
while not activated and not rospy.is_shutdown():
try:
data = self.ser.readline()
if len(data.split(",")) == 16:
self.log("IMU is outputting values", 7)
activated = True
else:
reads = reads + 1
if reads > 50:
self.log("IMU is not outputting values. Restarting.", 2, alert="warn")
self.connection()
reads = 0
except KeyboardInterrupt:
raise KeyboardInterrupt()
finally:
self.rate.sleep()
# Wait for yaw values to stabilize
# rtcDate,rtcTime,Q6_1,Q6_2,Q6_3,RawAX,RawAY,RawAZ,RawGX,RawGY,RawGZ,RawMX,RawMY,RawMZ,output_Hz,\r\n
reads = 0
th_hist = [1e5]*3
while not calibrated and not rospy.is_shutdown():
try:
data = self.ser.readline().split(",")
q = [float(data[2]), float(data[3]), float(data[4]), 0]
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) <= 1.0:
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
else:
continue
th_hist.pop(0)
th_hist.append(transformations.euler_from_quaternion(q)[2])
# Compute second derivative
deriv = (th_hist[2]-th_hist[1]) - (th_hist[1]-th_hist[0])
if reads > 50 and abs(deriv) < 1e-6:
self.log("IMU is calibrated.", 5)
acc_x = float(data[5])
acc_y = float(data[6])
acc_z = float(data[7])
self.acc_ratio = 9.82/np.linalg.norm([acc_x, acc_y, acc_z])
calibrated = True
else:
self.log("IMU is calibrating.", 7)
reads = reads + 1
except KeyboardInterrupt:
raise KeyboardInterrupt()
finally:
self.rate.sleep()
self.calibration = False
return True
def set_imu_rate(self):
""" This method must send the appropiate message to set a new rate for IMU """
# Send character to enter menu
self.ser.write("a\r\n")
rospy.sleep(1)
# Send character '1' to enter "Confirue Terminal Output"
self.ser.write("1\r\n")
rospy.sleep(1)
# Send character '4' to enter "Set IMU Rate"
self.ser.write("4\r\n")
rospy.sleep(1)
# Send rate in Hz
self.ser.write("{}\r\n".format(self.imu_rate))
rospy.sleep(1)
self.ser.write("x\r\n")
rospy.sleep(1)
self.ser.write("x\r\n")
rospy.sleep(1)
# Wait for IMU to publish compliant messages
self.ser.reset_input_buffer()
self.ser.reset_output_buffer()
return True
def compute_imu_msg(self):
"""
This method reads data from the Openlog Artemis output.
We receive 3 coord Quaternion, which causes discontinuities in rotation.
RPY coords are stored and reused to compute a new quaternion
"""
# Get data
# rtcDate,rtcTime,Q6_1,Q6_2,Q6_3,RawAX,RawAY,RawAZ,RawGX,RawGY,RawGZ,RawMX,RawMY,RawMZ,output_Hz,\r\n
try:
data = self.ser.readline().split(",")
except SerialException:
self.log("Error reading data from IMU", 2, alert="warn")
self.fails = self.fails + 1
if self.fails > 10:
self.connection()
return
else:
self.fails = 0
if len(data) < 16:
self.log("IMU Communication failed", 4, alert="warn")
return
# Compute Absolute Quaternion
try:
q = [float(data[2]), float(data[3]), float(data[4]), 0]
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) > 1.0:
self.log("Inconsistent IMU readings", 4, alert="warn")
return
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
except ValueError:
self.log("Error converting IMU message - {}".format(data), 5, alert="warn")
return
# Compute Linear Acceleration
acc = [round(float(data[5])*self.acc_ratio, 3), round(float(data[6])*self.acc_ratio, 3), round(float(data[7])*self.acc_ratio, 3)]
self.acc_hist.pop(0)
self.acc_hist.append(acc)
# Compute Angular Velocity
# w_x = float(data[8])*3.14/180
# w_y = float(data[9])*3.14/180
# w_z = float(data[10])*3.14/180
# Compute Angular Velocity from Quat6
lq = self.last_imu[-1].orientation
euler1 = transformations.euler_from_quaternion([lq.x, lq.y, lq.z, lq.w])
euler2 = transformations.euler_from_quaternion(q)
curr_time = rospy.Time.now()
dt = (curr_time - self.last_imu[-1].header.stamp).to_sec()
w = []
for i in range(0, 3):
dth = euler2[i] - euler1[i]
# The IMU Quaternion jumps need to be handled
while (3.14 < dth) or (dth < -3.14):
dth = dth - np.sign(dth)*2*np.pi
# Keep euler angles in [-2p and 2pi]
self.euler[i] += dth
while (2*np.pi < self.euler[i]) or (self.euler[i] < -2*np.pi):
self.euler[i] = self.euler[i] - np.sign(self.euler[i])*2*np.pi
w.append(round(dth/dt, 4))
q_est = transformations.quaternion_from_euler(self.euler[0], self.euler[1], self.euler[2])
new_q = Quaternion()
new_q.x = q_est[0]
new_q.y = q_est[1]
new_q.z = q_est[2]
new_q.w = q_est[3]
# Compute IMU Msg
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix+"imu"
imu_msg.header.stamp = curr_time
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.001, 0, 0,
0, 0.001, 0,
0, 0, 0.001
]
# Angular Velocity
imu_msg.angular_velocity.x = w[0]
imu_msg.angular_velocity.y = w[1]
imu_msg.angular_velocity.z = w[2]
# Datasheet says:
# - Noise Spectral Density: 0.015dps/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(0.015/np.sqrt(20), 2)
# factor = 0.02
# imu_msg.angular_velocity_covariance = [
# diag, w_x*factor, w_x*factor,
# w_y*factor, diag, w_y*factor,
# w_z*factor, w_z*factor, diag
# ]
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = -1
imu_msg.angular_velocity_covariance[0] = 0.01
imu_msg.angular_velocity_covariance[4] = 0.01
imu_msg.angular_velocity_covariance[8] = 0.05
# imu_msg.angular_velocity_covariance = [-1] * 9
# Linear Acceleration
acc = [0, 0, 0]
for idx in range(0, 3):
data = [a[idx] for a in self.acc_hist]
res = butter_lowpass_filter(data, cutoff=0.5, fs=10.0, order=1)
acc[idx] = res[-1]
imu_msg.linear_acceleration.x = acc[0]
imu_msg.linear_acceleration.y = acc[1]
imu_msg.linear_acceleration.z = acc[2]
# imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = 9.82
# imu_msg.linear_acceleration_covariance = [-1] * 9
# Datasheet says:
# - Noise Spectral Density: 230microg/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(230e-6/np.sqrt(20), 2)/256.
# factor = 0.02/256.
# imu_msg.linear_acceleration_covariance = [
# diag, acc_x*factor, acc_x*factor,
# acc_y*factor, diag, acc_y*factor,
# acc_z*factor, acc_z*factor, diag
# ]
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.01
imu_msg.linear_acceleration_covariance[4] = 0.01
imu_msg.linear_acceleration_covariance[8] = 0.05
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion([new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(r, p, y))
self.last_imu.pop(0)
self.last_imu.append(imu_msg)
def publish_diagnostic(self, level, message):
""" Auxiliary method to publish Diagnostic messages """
diag_msg = DiagnosticArray()
diag_msg.header.frame_id = "imu"
diag_msg.header.stamp = rospy.Time.now()
imu_msg = DiagnosticStatus()
imu_msg.name = "IMU"
imu_msg.hardware_id = "OpenLog Artemis IMU"
imu_msg.level = level
imu_msg.message = message
diag_msg.status.append(imu_msg)
self.diag_pub.publish(diag_msg)
def start(self):
self.rate = rospy.Rate(self.control_freq)
while not rospy.is_shutdown():
try:
if self.calibration:
self.calibrate_imu()
elif self.update_rates:
self.set_imu_rate()
self.update_rates = False
else:
self.compute_imu_msg()
self.publish_diagnostic(0, "OK")
self.rate.sleep()
except KeyboardInterrupt:
self.log("Shutting down by user", 2)
break
except IOError as io_exc:
self.publish_diagnostic(1, "Lost connection to IMU")
self.log("Lost connection to IMU", 3)
if not self.connection():
rospy.sleep(2)
class IDMindArtemisHub:
"""
This class extracts data from the Sparkfun OpenLog Artemis (with integrated ICM 20948)
The OpenLogArtemis Firmware must be uploaded to the unit and configured (explained in README.MD).
In case the connection is lost, it will try to reconnect and even close and reopen USB port.
It will publish sensors_msgs/Imu to ~imu and euler angles to ~euler_string.
If QWICC devices are connected, it can parse and publish the available values.
"""
def __init__(self):
# Logging
self.ready = False
rospy.Service("~ready", Trigger, self.report_ready)
self.logging = rospy.Publisher("/idmind_logging", Log, queue_size=10)
self.diag_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=10)
self.tf_prefix = rospy.get_param("~tf_prefix", "")
# Connect to Artemis HUB
self.fails = 0
self.reads = 0
self.ser = None
self.first_run = True
self.connection()
self.data_headers = {"Date": 0, "Time": 1, "Hz": -1,
"IMU": {"enabled": False},
"GPS": {"enabled": False},
}
# IMU
self.imu_calibrated = False
self.imu_pub = rospy.Publisher("~imu", Imu, queue_size=10)
self.imu_euler_pub = rospy.Publisher("~euler_string", String, queue_size=10)
# GPS
self.gps_pub = rospy.Publisher("~gps", NavSatFix, queue_size=10)
self.time_ref = rospy.Publisher("~time_reference", TimeReference, queue_size=10)
self.ready = True
self.log("Node is ready", 4)
###############
# CALLBACKS #
###############
def report_ready(self, _req):
""" Simple Service callback to show node is ready """
self.log("Replying to 'ready' request", 7)
return TriggerResponse(self.ready, rospy.get_name()+" is " + ("ready" if self.ready else "not ready"))
####################
# Initialization #
####################
def connection(self):
"""
Function that connects to IMU port. Repeats until found.
:return:
"""
connected = False
while not connected and not rospy.is_shutdown():
self.log("Searching for Hub", 4)
try:
self.ser = IDMindSerial("/dev/idmind-artemis", baudrate=115200, timeout=0.5)
self.log("OpenLog Artemis found.", 4)
connected = True
except SerialException as err:
if err.errno == 2:
self.log("Openlog Artemis Hub not found", 2, alert="warn")
rospy.sleep(1)
elif "Inappropriate ioctl" in str(err):
self.log("Restarting USB port", 2, alert="error")
self.usb_port_restart()
# fd = os.open("/dev/idmind-artemis", os.O_WRONLY)
# try:
# fcntl.ioctl(fd, USBDEVFS_RESET, 0)
# finally:
# os.close(fd)
rospy.sleep(1)
else:
self.log("Exception connecting to IMU: {}".format(err), 2, alert="warn")
rospy.sleep(1)
self.first_run = True
return connected
def usb_port_restart(self, port="/dev/idmind-artemis"):
fd = os.open(port, os.O_WRONLY)
try:
fcntl.ioctl(fd, USBDEVFS_RESET, 0)
finally:
os.close(fd)
def auto_detect_devices(self):
"""
This method will listen to the initial communications and detect connected Qwicc devices:
- IMU
- GPS
"""
headers = False
devices_detected = False
# Start by checking available devices
while (not headers) and (not rospy.is_shutdown()):
data = self.ser.readline()
if len(data.split(",")) > 5:
if not devices_detected:
self.log("Restarting USB port", 2, alert="error")
self.usb_port_restart()
self.log("Data headers received", 5)
headers = True
elif "IMU online" in data:
self.log("IMU Detected", 5)
devices_detected = True
self.data_headers["IMU"]["enabled"] = True
elif "GPS-ublox online" in data:
self.log("GPS Detected", 5)
devices_detected = True
self.data_headers["GPS"]["enabled"] = True
split_data = data.split(",")
# If IMU was detected, search for indexes for Q6_1, Q6_2, Q6_3, RawAX, RawAY, RawAZ, RawGX, RawGY, RawGZ and ignore Magnetometer.
if self.data_headers["IMU"]["enabled"]:
try:
self.imu_calibrated = False
self.imu_history = [0.0] * 3
self.data_headers["IMU"]["QX"] = split_data.index("Q6_1")
self.data_headers["IMU"]["QY"] = split_data.index("Q6_2")
self.data_headers["IMU"]["QZ"] = split_data.index("Q6_3")
self.data_headers["IMU"]["AX"] = split_data.index("RawAX")
self.data_headers["IMU"]["AY"] = split_data.index("RawAY")
self.data_headers["IMU"]["AZ"] = split_data.index("RawAZ")
self.data_headers["IMU"]["GX"] = split_data.index("RawGX")
self.data_headers["IMU"]["GY"] = split_data.index("RawGY")
self.data_headers["IMU"]["GZ"] = split_data.index("RawGZ")
self.log("IMU Header Indexes: {}".format(self.data_headers["IMU"]), 8)
except ValueError:
self.log("IMU Data not found on headers", 4, alert="warn")
self.data_headers["IMU"]["enabled"] = False
# If GPS was detected, search for indexes gps_Date,gps_Time,gps_Lat,gps_Long,gps_Alt,gps_SIV,gps_FixType,gps_GroundSpeed,gps_Heading,gps_pDOP
if self.data_headers["GPS"]["enabled"]:
try:
self.data_headers["GPS"]["Date"] = split_data.index("gps_Date")
self.data_headers["GPS"]["Time"] = split_data.index("gps_Time")
self.data_headers["GPS"]["Lat"] = split_data.index("gps_Lat")
self.data_headers["GPS"]["Long"] = split_data.index("gps_Long")
self.data_headers["GPS"]["Alt"] = split_data.index("gps_Alt")
self.data_headers["GPS"]["SIV"] = split_data.index("gps_SIV")
self.data_headers["GPS"]["FixType"] = split_data.index("gps_FixType")
self.data_headers["GPS"]["GroundSpeed"] = split_data.index("gps_GroundSpeed")
self.data_headers["GPS"]["Heading"] = split_data.index("gps_Heading")
self.data_headers["GPS"]["pDOP"] = split_data.index("gps_pDOP")
self.log("GPS Header Indexes: {}".format(self.data_headers["GPS"]), 8)
except ValueError:
self.log("GPS Data not found on headers", 4, alert="warn")
self.data_headers["GPS"]["enabled"] = False
msg = "Devices detected: \n"
for dev in self.data_headers.keys():
if type(self.data_headers[dev]) == dict:
if self.data_headers[dev]["enabled"]:
msg += "\t{}\n".format(dev)
self.log(msg, 3)
return True
def handle_imu(self, data, timestamp):
"""
Accelerometer aX,aY,aZ milli g
Gyro gX,gY,gZ Degrees per Second
Magnetometer mX,mY,mZ micro Tesla
Temperature imu_degC Degrees Centigrade
"""
# Compute Quaternion, if data is consistent
q = [float(data[self.data_headers["IMU"]["QX"]]),
float(data[self.data_headers["IMU"]["QY"]]),
float(data[self.data_headers["IMU"]["QZ"]]),
0]
# Data may be inconsistent, try to fix it
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) > 1.0:
q_norm = (q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])
q[0] = q[0]/q_norm
q[1] = q[1]/q_norm
q[2] = q[2]/q_norm
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
# Save IMU history for calibration
self.imu_history.pop(0)
self.imu_history.append(transformations.euler_from_quaternion(q)[2])
if not self.imu_calibrated:
# Wait for heading readings to stabilize
if self.reads > 50 and abs((self.imu_history[2]-self.imu_history[1]) - (self.imu_history[1]-self.imu_history[0])) < 1e-6:
self.log("IMU is calibrated.", 5)
self.imu_calibrated = True
else:
new_q = Quaternion()
new_q.x = q[0]
new_q.y = q[1]
new_q.z = q[2]
new_q.w = q[3]
# Compute Linear Acceleration - converting to ms-2
acc_x = float(data[self.data_headers["IMU"]["AX"]])/1000
acc_y = float(data[self.data_headers["IMU"]["AY"]])/1000
acc_z = float(data[self.data_headers["IMU"]["AZ"]])/1000
# Compute Angular Velocity
w_x = float(data[self.data_headers["IMU"]["GX"]])*np.pi/180
w_y = float(data[self.data_headers["IMU"]["GY"]])*np.pi/180
w_z = float(data[self.data_headers["IMU"]["GY"]])*np.pi/180
# Compute IMU Msg
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix+"imu"
imu_msg.header.stamp = timestamp
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.01, 0, 0,
0, 0.01, 0,
0, 0, 0.01
]
# Angular Velocity
imu_msg.angular_velocity.x = w_x
imu_msg.angular_velocity.y = w_y
imu_msg.angular_velocity.z = w_z
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = -1
# Linear Acceleration
imu_msg.linear_acceleration.x = acc_x
imu_msg.linear_acceleration.y = acc_y
imu_msg.linear_acceleration.z = acc_z
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.01
imu_msg.linear_acceleration_covariance[4] = 0.01
imu_msg.linear_acceleration_covariance[8] = 0.01
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion([new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(r, p, y))
return True
def handle_gps(self, data, timestamp):
"""
Date gps_Date MM/DD/YYYY or DD/MM/YYYY
Time gps_Time HH:MM:SS.SSS
Lat & Lon gps_Lat,gps_Long Degrees-7
Altitude gps_Alt mm
Altitude MSL gps_AltMSL mm
SIV gps_SIV Count
Fix Type gps_FixType 0-5
Carrier Soln. gps_CarrierSolution 0-2
Ground Speed gps_GroundSpeed mm/s
Heading gps_Heading Degrees-5
PDOP gps_pDOP 10-2 (dimensionless)
Time Of Week gps_iTOW ms
Lat = Latitude
Lon = Longitude
MSL = Metres above Sea Level
SIV = Satellites In View
PDOP = Positional Dilution Of Precision
Fix Type:
0: No
1: Dead Reckoning Only
2: 2D
3: 3D
4: GNSS + Dead Reckoning
5: Time Only
Carrier Solution:
0: No
1: Float Solution
2: Fixed Solution
"""
gps_msg = NavSatFix()
gps_msg.header.frame_id = self.tf_prefix+"world"
gps_msg.header.stamp = timestamp
gps_msg.status = NavSatStatus(status=0, service=1)
gps_msg.latitude = float(data[self.data_headers["GPS"]["Lat"]])
gps_msg.longitude = float(data[self.data_headers["GPS"]["Long"]])
gps_msg.altitude = float(data[self.data_headers["GPS"]["Alt"]])
# COVARIANCE_TYPE_UNKNOWN = 0, COVARIANCE_TYPE_APPROXIMATED = 1
# COVARIANCE_TYPE_DIAGONAL_KNOWN = 2, COVARIANCE_TYPE_KNOWN = 3
gps_msg.position_covariance = [0.0] * 9
gps_msg.position_covariance_type = 0
self.gps_pub.publish(gps_msg)
# Time Reference
time_msg = TimeReference()
time_msg.header.stamp = timestamp
gps_time = datetime.strptime("{} {}".format(data[self.data_headers["GPS"]["Date"]], data[self.data_headers["GPS"]["Time"]]),
"%d/%m/%Y %H:%M:%S.%f")
total_secs = (gps_time - epoch).total_seconds()
time_msg.time_ref.secs = int(total_secs)
time_msg.time_ref.nsecs = total_secs-int(total_secs)
time_msg.source = "GPS"
self.time_ref.publish(time_msg)
return True
#########################
# AUXILIARY FUNCTIONS #
#########################
def log(self, msg, msg_level, log_level=-1, alert="info"):
"""
Log function that publish in screen and in topic
:param msg: Message to be published
:param msg_level: Message level (1-10, where 1 is most important)
:param log_level: Message level for logging (1-10, optional, -1 uses the same as msg_level)
:param alert: Alert level of message - "info", "warn" or "error"
:return:
"""
if VERBOSE >= msg_level:
if alert == "info":
rospy.loginfo("{}: {}".format(rospy.get_name(), msg))
elif alert == "warn":
rospy.logwarn("{}: {}".format(rospy.get_name(), msg))
elif alert == "error":
rospy.logerr("{}: {}".format(rospy.get_name(), msg))
if LOGS >= (log_level if log_level != -1 else msg_level):
self.logging.publish(rospy.Time.now().to_sec(), rospy.get_name(), msg)
def publish_diagnostics(self, level=0, message=""):
""" Auxiliary method to publish Diagnostic messages """
diag_msg = DiagnosticArray()
diag_msg.header.frame_id = self.tf_prefix+"artemis"
diag_msg.header.stamp = rospy.Time.now()
if level == 0:
# IMU Diagnostics
if self.data_headers["IMU"]["enabled"]:
imu_msg = DiagnosticStatus()
imu_msg.name = "IMU"
imu_msg.hardware_id = "ICM 20948"
imu_msg.level = 0 if self.imu_calibrated else 3
imu_msg.message = "OK" if self.imu_calibrated else "Calibrating"
diag_msg.status.append(imu_msg)
# GPS Diagnostics
if self.data_headers["GPS"]["enabled"]:
gps_msg = DiagnosticStatus()
gps_msg.name = "GPS"
gps_msg.hardware_id = "GPS NEO-M9N"
gps_msg.level = 0
gps_msg.message = "OK"
diag_msg.status.append(gps_msg)
else:
artemis_msg = DiagnosticStatus()
artemis_msg.name = "Openlog Artemis"
artemis_msg.hardware_id = "Openlog Artemis"
artemis_msg.level = level
artemis_msg.message = message
diag_msg.status.append(artemis_msg)
self.diag_pub.publish(diag_msg)
def publish_device_data(self):
# TODO: Ensure message is recent and valid or discard everything
data = (self.ser.readline()).split(',')
timestamp = rospy.Time.now()
if len(data) > 5:
self.reads += 1
if self.data_headers["IMU"]["enabled"]:
self.handle_imu(data, timestamp)
if self.data_headers["GPS"]["enabled"]:
self.handle_gps(data, timestamp)
self.publish_diagnostics(level=0)
else:
self.publish_diagnostics(level=1, message="Invalid/Old data")
return True
def start(self):
r = rospy.Rate(20)
while not rospy.is_shutdown():
try:
if self.first_run:
self.auto_detect_devices()
self.first_run = False
self.publish_device_data()
r.sleep()
except KeyboardInterrupt:
self.log("Shutting down by user", 2)
self.ser.close()
break
except IOError as io_exc:
self.publish_diagnostics(1, "Lost connection to IMU")
self.log("Lost connection to IMU", 3)
if not self.connection():
rospy.sleep(2)