diff --git a/src/external_ic/IMU.h b/src/external_ic/IMU.h
index 064a4b3..89beeac 100644
--- a/src/external_ic/IMU.h
+++ b/src/external_ic/IMU.h
@@ -4,23 +4,32 @@
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/MagneticField.h"
 #include "sensor_msgs/Temperature.h"
+#include "svea_teensy.h"
 
 #include <Adafruit_BNO055.h>
 #include <Adafruit_Sensor.h>
 #include <SPI.h>
 #include <Wire.h>
+
 #include <ros.h>
 #include <ros/time.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Header.h>
 #include <std_msgs/String.h>
+
 #include <utility/imumaths.h>
+
+std_msgs::Header header;
+
 #define BNO055_SAMPLERATE_DELAY_MS (100)
 Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire1);
 bool setupIMU() {
+    header.frame_id = "imu";
+    header.seq = 0;
     if (!bno.begin()) {
         /* There was a problem detecting the BNO055 ... check your connections */
         Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+        header.frame_id = "imu-disconnected";
         return false;
     }
     bno.setExtCrystalUse(true);
@@ -61,24 +70,24 @@ void IMUReadingToMsg() {
 
     MSG_TEMP.temperature = bno.getTemp();
 
+    // TODO, make more efficient or make sensible covarience, or both
     int fakeCovariance = 0;
     for (int i = 0; i < 9; ++i) {
-        msg.orientation_covariance[i] = fakeCovariance;
-        msg.angular_velocity_covariance[i] = fakeCovariance;
-        msg.linear_acceleration_covariance[i] = fakeCovariance;
+        MSG_IMU.orientation_covariance[i] = fakeCovariance;
+        MSG_IMU.angular_velocity_covariance[i] = fakeCovariance;
+        MSG_IMU.linear_acceleration_covariance[i] = fakeCovariance;
+        MSG_MAG.magnetic_field_covariance[i] = fakeCovariance;
     }
-    //nh.spinOnce();
-    //Serial.println("MSG_IMU: ");
+
     imu_pub.publish(&MSG_IMU);
     nh.spinOnce();
-    //Serial.println("MSG_MAG: ");
+
     imu_mag.publish(&MSG_MAG);
     nh.spinOnce();
-    //Serial.println("MSG_TEMP: ");
+
     imu_temp.publish(&MSG_TEMP);
     nh.spinOnce();
-    //Serial.printf("IMU: Orientation(%f, %f, %f, %f) Angular Velocity(%f, %f, %f) Linear Acceleration(%f, %f, %f)\n", msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w, msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z);
-
+    // Serial.printf("IMU: Orientation(%f, %f, %f, %f) Angular Velocity(%f, %f, %f) Linear Acceleration(%f, %f, %f)\n", msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w, msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z);
     return;
 }
 
diff --git a/src/external_ic/gpio_ext.h b/src/external_ic/gpio_ext.h
index 010b6f7..4e836bf 100644
--- a/src/external_ic/gpio_ext.h
+++ b/src/external_ic/gpio_ext.h
@@ -1,13 +1,12 @@
-#include <Arduino.h>
 #include <Adafruit_MCP23X08.h>
+#include <Arduino.h>
 
 /* GPIO extender variables */
 constexpr int GPIO_ADDRESS = 0x20;
 constexpr uint8_t SERVO_PWR_ENABLE_PIN = 3;
 Adafruit_MCP23X08 gpio_extender;
 
-void setup_gpio()
-{
+void setup_gpio() {
     gpio_extender.begin_I2C(GPIO_ADDRESS, &Wire1);
     gpio_extender.pinMode(SERVO_PWR_ENABLE_PIN, OUTPUT);
     buttons::setup(gpio_extender);
diff --git a/src/main.cpp b/src/main.cpp
index 97da1bd..7833909 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -14,7 +14,6 @@
 #include "external_ic/IMU.h"
 #include "external_ic/gpio_ext.h"
 
-
 #include "settings.h"
 #include "svea_teensy.h"
 
@@ -310,39 +309,48 @@ void rosSetup() {
     // NOTE: Putting advertise before subscribe destroys
     //       EVERYTHING :DDDD~~~~~
 
-    
     nh.subscribe(ctrl_request);
+    nh.negotiateTopics();
+
     nh.subscribe(emergency_request);
-   
-    nh.advertise(remote_pub);
-    
-    nh.advertise(imu_mag);
-    nh.advertise(imu_pub);
+    nh.negotiateTopics();
 
-    nh.advertise(imu_temp);
+    nh.advertise(remote_pub);
+    nh.negotiateTopics();
 
     nh.advertise(ctrl_actuated_pub);
-    nh.advertise(encoder_pub);
+    nh.negotiateTopics();
 
-  
+    nh.advertise(encoder_pub);
+    nh.negotiateTopics();
 
     nh.advertise(debug_pub);
     nh.negotiateTopics();
+
+    nh.advertise(imu_mag);
+    nh.negotiateTopics();
+
+    nh.advertise(imu_pub);
+    nh.negotiateTopics();
+
+    nh.advertise(imu_temp);
+    nh.negotiateTopics();
 }
 
+// Im sorry kaj, it will do for now
+bool imuConnected = false;
 //! Arduino setup function
 void setup() {
-    // Serial.begin(SERIAL_BAUD_RATE);
+    while(nh.connected()){
+        nh.spinOnce();
+    }
     setupActuation();
-    /* ROS setup */
-    
     pinMode(LED_BUILTIN, OUTPUT);
-
     Wire1.begin();
     setup_gpio();
     pwm_reader::setup();
     encoders::setup();
-    setupIMU();
+    imuConnected = setupIMU();
 
     rosSetup();
     Serial.println("Setup done");
@@ -374,9 +382,6 @@ void loop() {
         SW_IDLE = true;
     }
 
-    // If the remote control is idle, the system is idle, there is no emergency, and the servo is not idle, turn off the servo
-
-    // NEW, if pwnTimeout is true, turn off the servo, check setPWMdriver function for more info
     if ((pwm_reader::REM_IDLE && SW_IDLE && !SW_EMERGENCY) && !servo_idle) {
         actuate(IDLE_ACTUATION);
         gpio_extender.digitalWrite(SERVO_PWR_ENABLE_PIN, LOW);
@@ -392,9 +397,11 @@ void loop() {
     if (encoders::processEncoderTicks(reading)) {
         EncoderReadingToMsg(reading, MSG_ENCODER);
         encoder_pub.publish(&MSG_ENCODER);
+        nh.spinOnce();
+    }
+    if (imuConnected) {
+        IMUReadingToMsg();
     }
-
-    IMUReadingToMsg();
 
     // PCB LED Logic
     buttons::updateButtons();