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 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 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)