Got pan/tilt and tower combo node done. Other misc changes.

software/firmware/tower/tower.ino

@@ -2,9 +2,6 @@
 #include <ModbusRtu.h>
 #include <Adafruit_BNO055_t3.h>
 #include <ArduinoJson.h>
-#include "FastLED.h"
-
-#include <NMEAGPS.h>
 
 /*
    Imu/data (Imu)
@@ -44,7 +41,7 @@ enum HARDWARE {
   IMU_SDA = 18,
   IMU_SCL = 19,
 
-  WS2812_DATA = 11,
+  WHITE_LED_CONTROL = 11,
   C02_SENSOR = A7,
   MISC_PIN = A8,
 
@@ -52,7 +49,16 @@ enum HARDWARE {
 };
 
 enum MODBUS_REGISTERS {
-  DIRECTION = 0,  // Input
+  LED_CONTROL = 0,  // Input
+  CO2_READING_PPM = 1
+};
+
+enum LIGHT_STATES {
+  NO_CHANGE = 0,
+  LIGHT_OFF = 1,
+  LIGHT_FLASH = 2,
+  LIGHT_MED = 3,
+  LIGHT_HIGH = 4
 };
 
 #define GPS_SERIAL_PORT Serial3
@@ -88,13 +94,12 @@ Modbus slave(node_id, mobus_serial_port_number, HARDWARE::COMMS_RS485_EN);
 
 Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_IMM);
 
-NMEAGPS gps;
-
 const char baud115200[] = "PUBX,41,1,3,3,115200,0";
 
 void setup() {
   // Debugging
   Serial.begin(9600);
+//  while (!Serial);
 
   // Raw pin setup
   setup_hardware();
@@ -134,6 +139,8 @@ void loop() {
   set_leds();
   send_imu_stream_line(root);
   process_gps_and_send_if_ready(root);
+  process_white_led_command();
+  get_co2_data();
 
   // Print JSON and newline
   root.printTo(GPS_IMU_STREAMING_PORT);
@@ -144,13 +151,14 @@ void loop() {
 
 void setup_hardware() {
   // Setup pins as inputs / outputs
-  pinMode(HARDWARE::WS2812_DATA, OUTPUT);
+  pinMode(HARDWARE::WHITE_LED_CONTROL, OUTPUT);
   pinMode(HARDWARE::C02_SENSOR, INPUT);
   pinMode(HARDWARE::MISC_PIN, OUTPUT);
 
   pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
 
   // Set default pin states
+  digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
   digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
 
   // Set teensy to increased analog resolution
@@ -217,6 +225,52 @@ void process_gps_and_send_if_ready(JsonObject &root) {
 
 }
 
+void process_white_led_command() {
+  uint16_t light_command = modbus_data[MODBUS_REGISTERS::LED_CONTROL];
+
+
+  if (light_command == LIGHT_STATES::LIGHT_MED) {
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
+    delay(100);               // wait for a second
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
+    delay(100);               // wait for a second
+  } else if (light_command == LIGHT_STATES::LIGHT_FLASH) {
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
+    delay(100);               // wait for a second
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
+    delay(100);               // wait for a second
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
+    delay(100);               // wait for a second
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
+    delay(100);
+  } else if (light_command == LIGHT_STATES::LIGHT_HIGH) {
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
+    delay(750);               // wait for a second
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
+    delay(100);
+  } else if (light_command == LIGHT_STATES::LIGHT_OFF) {
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, HIGH);
+    delay(2000);               // wait for a second
+    digitalWrite(HARDWARE::WHITE_LED_CONTROL, LOW);
+    delay(2000);
+  }
+
+  modbus_data[MODBUS_REGISTERS::LED_CONTROL] = LIGHT_STATES::NO_CHANGE;
+}
+
+void get_co2_data() {
+  int voltage = ((analogRead(HARDWARE::C02_SENSOR) / 8192.0) * 3300);
+
+  if (voltage < 400 || voltage > 2000) {
+    modbus_data[MODBUS_REGISTERS::CO2_READING_PPM] = 9999;
+  } else {
+    modbus_data[MODBUS_REGISTERS::CO2_READING_PPM] = map(voltage, 400, 2000, 0, 5000);
+  }
+
+  //  Serial.println(modbus_data[MODBUS_REGISTERS::CO2_READING_PPM]);
+
+}
+
 void poll_modbus() {
   poll_state = slave.poll(modbus_data, num_modbus_registers);
   communication_good = !slave.getTimeOutState();

software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py

@@ -91,7 +91,7 @@ class SensorCore(QtCore.QThread):
         self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback)
 
         self.camera_msg = CameraStatuses()
-        self.bogie_msg = BogieStatuses()
+        self.bogie_msg = None # BogieStatuses()
         self.FrSky_msg = FrSkyStatus()
         self.GPS_msg = GPSInfo()
         self.jetson_msg = JetsonInfo()

software/ros_packages/ground_station/src/Framework/StatusSystems/UbiquitiStatusCore.py

@@ -13,7 +13,7 @@ import json
 #####################################
 # Global Variables
 #####################################
-THREAD_HERTZ = 2
+THREAD_HERTZ = 5
 
 ACCESS_POINT_IP = "192.168.1.20"  # The channel only has to be set on the access point. The staion will adjust.
 ACCESS_POINT_USER = "ubnt"

software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui

@@ -264,7 +264,11 @@
               <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
              </property>
              <property name="text">
-              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;TX2 EMMC&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+              <string>TX2 EMMC
+-- %</string>
+             </property>
+             <property name="textFormat">
+              <enum>Qt::PlainText</enum>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -797,7 +801,11 @@ Disconnected</string>
               <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
              </property>
              <property name="text">
-              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;TX2 CPU&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+              <string>TX2 CPU
+-- %</string>
+             </property>
+             <property name="textFormat">
+              <enum>Qt::PlainText</enum>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -817,7 +825,11 @@ Disconnected</string>
               <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
              </property>
              <property name="text">
-              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-size:9pt; font-weight:600;&quot;&gt;TX2 TEMP&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+              <string>TX2 Temp
+ -- °C</string>
+             </property>
+             <property name="textFormat">
+              <enum>Qt::PlainText</enum>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -963,7 +975,11 @@ Connected</string>
               <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
              </property>
              <property name="text">
-              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;center&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;TX2 RAM&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+              <string>TX2 RAM
+-- %</string>
+             </property>
+             <property name="textFormat">
+              <enum>Qt::PlainText</enum>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -1047,7 +1063,7 @@ Connected</string>
              </property>
              <property name="text">
               <string>Connection Quality
-0.0%</string>
+-- %</string>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -1088,8 +1104,8 @@ Connected</string>
               <enum>QFrame::NoFrame</enum>
              </property>
              <property name="text">
-              <string>RX Rate: 240.0 Mbps
+              <string>RX Rate: --- Mbps
-TX Rate: 300 Mbps</string>
+TX Rate: --- Mbps</string>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -1131,7 +1147,7 @@ TX Rate: 300 Mbps</string>
              </property>
              <property name="text">
               <string>Successful Transmit
-0.0%</string>
+-- %</string>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>
@@ -1172,8 +1188,8 @@ TX Rate: 300 Mbps</string>
               <enum>QFrame::NoFrame</enum>
              </property>
              <property name="text">
-              <string>TX Latency: 10000 ms
+              <string>TX Latency: ----- ms
-RX Latency: 10000 ms</string>
+RX Latency: ----- ms</string>
              </property>
              <property name="alignment">
               <set>Qt::AlignCenter</set>

software/ros_packages/rover_control/src/drive_control/drive_control.py

@@ -119,7 +119,7 @@ class DriveControl(object):
                 self.get_drive_status()
 
             except Exception, error:
-                print "Error occurred:", error
+                pass
 
             if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT:
                 print "Bogie not seen for", BOGIE_LAST_SEEN_TIMEOUT, "seconds. Exiting."

software/ros_packages/rover_control/src/iris_controller/iris_controller.py

@@ -70,6 +70,8 @@ REGISTER_STATE_MAPPING = {
     "DRIVE_VS_ARM": "SE_SWITCH"
 }
 
+IRIS_LAST_SEEN_TIMEOUT = 1  # seconds
+
 
 #####################################
 # IrisController Class Definition
@@ -98,6 +100,8 @@ class IrisController(object):
 
         self.iris_connected = False
 
+        self.iris_last_seen_time = time()
+
         self.run()
 
     def __setup_minimalmodbus_for_485(self):
@@ -114,11 +118,16 @@ class IrisController(object):
                 self.broadcast_drive_if_current_mode()
                 self.broadcast_arm_if_current_mode()
                 self.broadcast_iris_status()
+                self.iris_last_seen_time = time()
 
             except Exception, error:
                 print "Error occurred:", error
                 return
 
+            if (time() - self.iris_last_seen_time) > IRIS_LAST_SEEN_TIMEOUT:
+                print "Iris not seen for", IRIS_LAST_SEEN_TIMEOUT, "seconds. Exiting."
+                return  # Exit so respawn can take over
+
             time_diff = time() - start_time
 
             sleep(max(self.wait_time - time_diff, 0))

software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_and_pan_tilt_control.py

@@ -10,6 +10,8 @@ from time import time, sleep
 import serial.rs485
 import minimalmodbus
 
+from std_msgs.msg import UInt8, UInt16
+
 # Custom Imports
 from rover_control.msg import TowerPanTiltControlMessage
 
@@ -23,18 +25,21 @@ DEFAULT_BAUD = 115200
 
 DEFAULT_INVERT = False
 
-DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control"
+DEFAULT_TOWER_LIGHT_CONTROL_TOPIC = "tower/light/control"
+DEFAULT_TOWER_CO2_STATUS_TOPIC = "tower/status/co2"
+DEFAULT_PAN_TILT_CONTROL_TOPIC = "pan_tilt/control"
 
-NODE_ID = 2
+TOWER_NODE_ID = 1
+PAN_TILT_NODE_ID = 2
 
-COMMUNICATIONS_TIMEOUT = 0.01  # Seconds
+COMMUNICATIONS_TIMEOUT = 0.02  # Seconds
 
-RX_DELAY = 0.01
+RX_DELAY = 0.02
-TX_DELAY = 0.01
+TX_DELAY = 0.02
 
 DEFAULT_HERTZ = 20
 
-MODBUS_REGISTERS = {
+PAN_TILT_MODBUS_REGISTERS = {
     "CENTER_ALL": 0,
 
     "PAN_ADJUST_POSITIVE": 1,
@@ -43,6 +48,11 @@ MODBUS_REGISTERS = {
     "TILT_ADJUST_NEGATIVE": 4
 }
 
+TOWER_MODBUS_REGISTERS = {
+    "LED_CONTROL": 0,
+    "CO2_READING_PPM": 1
+}
+
 PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
     0,  # No centering
     0,  # No pan positive adjustment
@@ -51,7 +61,19 @@ PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
     0  # No tilt negative adjustement
 ]
 
-PAN_TILT_LAST_SEEN_TIMEOUT = 2  # seconds
+TOWER_LIGHT_STATES = {
+    "NO_CHANGE": 0,
+    "LIGHT_OFF": 1,
+    "LIGHT_FLASH": 2,
+    "LIGHT_MED": 3,
+    "LIGHT_HIGH": 4
+}
+
+TOWER_CONTROL_DEFAULT_MESSAGE = [
+    TOWER_LIGHT_STATES["LIGHT_OFF"]  # Light off
+]
+
+NODE_LAST_SEEN_TIMEOUT = 2  # seconds
 
 
 #####################################
@@ -64,23 +86,40 @@ class TowerPanTiltControl(object):
         self.port = rospy.get_param("~port", DEFAULT_PORT)
         self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
 
-        self.node_id = rospy.get_param("~node_id", NODE_ID)
+        self.tower_node_id = rospy.get_param("~tower_node_id", TOWER_NODE_ID)
+        self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID)
 
-        self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic", DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC)
+        self.tower_light_control_subscriber_topic = rospy.get_param("~tower_light_control_topic",
+                                                                    DEFAULT_TOWER_LIGHT_CONTROL_TOPIC)
+        self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic",
+                                                                 DEFAULT_PAN_TILT_CONTROL_TOPIC)
+
+        self.tower_co2_publisher_topic = rospy.get_param("~tower_co2_status_topic",
+                                                         DEFAULT_TOWER_CO2_STATUS_TOPIC)
 
         self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
 
         self.pan_tilt_node = None
+        self.tower_node = None
 
-        self.connect_to_pan_tilt()
+        self.connect_to_pan_tilt_and_tower()
 
-        self.pan_tilt_control_subscriber = \
+        self.pan_tilt_control_subscriber = rospy.Subscriber(self.pan_tilt_control_subscriber_topic,
-            rospy.Subscriber(self.pan_tilt_control_subscriber_topic, TowerPanTiltControlMessage, self.pan_tilt_control_callback)
+                                                            TowerPanTiltControlMessage,
+                                                            self.pan_tilt_control_callback)
+
+        self.tower_light_control_subscriber = rospy.Subscriber(self.tower_light_control_subscriber_topic, UInt8,
+                                                               self.tower_light_control_callback)
+
+        self.tower_co2_publisher = rospy.Publisher(self.tower_co2_publisher_topic, UInt16, queue_size=1)
 
         self.pan_tilt_control_message = None
-        self.new_control_message = False
+        self.tower_light_control_message = None
 
-        self.pan_tilt_last_seen = time()
+        self.new_pan_tilt_control_message = False
+        self.new_tower_light_control_message = False
+
+        self.modbus_nodes_seen_time = time()
 
         self.run()
 
@@ -90,63 +129,99 @@ class TowerPanTiltControl(object):
                                                                           delay_before_rx=RX_DELAY,
                                                                           delay_before_tx=TX_DELAY)
 
+        self.tower_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.tower_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                       delay_before_rx=RX_DELAY,
+                                                                       delay_before_tx=TX_DELAY)
+
     def run(self):
-        self.send_startup_centering_command()
+        self.send_startup_centering_and_lights_off_command()
 
         while not rospy.is_shutdown():
             start_time = time()
 
             try:
                 self.send_pan_tilt_control_message()
-                self.pan_tilt_last_seen = time()
+                self.modbus_nodes_seen_time = time()
 
             except Exception, error:
-                print "Error occurred:", error
+                pass
+                # print "Error occurred:", error
 
-            if (time() - self.pan_tilt_last_seen) > PAN_TILT_LAST_SEEN_TIMEOUT:
+            try:
-                print "Tower pan/tilt not seen for", PAN_TILT_LAST_SEEN_TIMEOUT, "seconds. Exiting."
+                self.send_tower_control_message()
+                self.broadcast_co2_reading_message()
+                self.modbus_nodes_seen_time = time()
+
+            except Exception, error:
+                pass
+                # print "Error occurred:", error
+
+            if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
+                print "Tower pan/tilt not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
                 return  # Exit so respawn can take over
 
             time_diff = time() - start_time
 
             sleep(max(self.wait_time - time_diff, 0))
 
-    def connect_to_pan_tilt(self):
+    def connect_to_pan_tilt_and_tower(self):
-        self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.node_id))
+        self.tower_node = minimalmodbus.Instrument(self.port, int(self.tower_node_id))
+        self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id))
         self.__setup_minimalmodbus_for_485()
 
-    def send_startup_centering_command(self):
+    def send_startup_centering_and_lights_off_command(self):
+        try:
             registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
-        registers[MODBUS_REGISTERS["CENTER_ALL"]] = 1
+            registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = 1
             self.pan_tilt_node.write_registers(0, registers)
 
+            self.tower_node.write_register(0, TOWER_LIGHT_STATES["LIGHT_OFF"])
+        except:
+            pass
+
     def send_pan_tilt_control_message(self):
-        if self.new_control_message:
+        if self.new_pan_tilt_control_message:
             pan_tilt_control_message = self.pan_tilt_control_message  # type: TowerPanTiltControlMessage
 
             registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
-            registers[MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center)
+            registers[PAN_TILT_MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center)
 
             if pan_tilt_control_message.relative_pan_adjustment >= 0:
-                registers[MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment
+                registers[
+                    PAN_TILT_MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment
             else:
-                registers[MODBUS_REGISTERS["PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment
+                registers[PAN_TILT_MODBUS_REGISTERS[
+                    "PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment
 
             if pan_tilt_control_message.relative_tilt_adjustment >= 0:
-                registers[MODBUS_REGISTERS["TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment
+                registers[PAN_TILT_MODBUS_REGISTERS[
+                    "TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment
             else:
-                registers[MODBUS_REGISTERS["TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment
+                registers[PAN_TILT_MODBUS_REGISTERS[
+                    "TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment
 
             self.pan_tilt_node.write_registers(0, registers)
 
-            self.new_control_message = False
+            self.new_pan_tilt_control_message = False
         else:
             self.pan_tilt_node.write_registers(0, PAN_TILT_CONTROL_DEFAULT_MESSAGE)
 
+    def broadcast_co2_reading_message(self):
+        self.tower_co2_publisher.publish(UInt16(data=self.tower_node.read_register(1)))
+
+    def send_tower_control_message(self):
+        if self.new_tower_light_control_message:
+            self.tower_node.write_register(0, self.tower_light_control_message.data)
+            self.new_tower_light_control_message = False
+
     def pan_tilt_control_callback(self, pan_tilt_control):
         self.pan_tilt_control_message = pan_tilt_control
-        self.new_control_message = True
+        self.new_pan_tilt_control_message = True
 
+    def tower_light_control_callback(self, light_control):
+        self.tower_light_control_message = light_control
+        self.new_tower_light_control_message = True
 
 if __name__ == "__main__":
     TowerPanTiltControl()

software/ros_packages/rover_main/launch/rover.launch

@@ -13,6 +13,6 @@
     <include file="$(find rover_main)/launch/rover/topic_transport_receivers.launch"/>
 
     <!-- ########## Start Odometry Nodes ########## -->
-    <include file="$(find rover_main)/launch/rover/odometry.launch"/>
+    <include file="$(find rover_main)/launch/rover/odometry2.launch"/>
 
 </launch>

software/ros_packages/rover_main/launch/rover/odometry2.launch

@@ -1,10 +1,12 @@
 <launch>
     <!-- https://github.com/vikiboy/AGV_Localization/blob/master/robot_localization/launch/ekf.launch -->
     <!-- https://answers.ros.org/question/241222/fusing-imu-gps-with-robot_location-package/ -->
+
+    <group ns="rover_odometry">
             <!-- ########## Processes GPS and IMU Messages ########## -->
         <node name="gps_and_imu" pkg="rover_odometry" type="odometry.py" respawn="true" output="screen" >
-        <param name="gps_sentence_topic" value="/nmea_sentence"/>
+            <!--<param name="gps_sentence_topic" value="/nmea_sentence"/>-->
-        <param name="imu_data_topic" value="/imu/data"/>
+            <!--<param name="imu_data_topic" value="/imu/data"/>-->
             <!--<remap from="nmea_sentence" to="gps/sentence"/>-->
             <!--<remap from="imu/data" to="/imu/data"/>-->
         </node>
@@ -13,56 +15,57 @@
             <remap from="nmea_sentence" to="gps/sentence"/>
         </node>
 
-    <node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link imu 20"/>
+        <!--<node pkg="tf" type="static_transform_publisher" name="imu_tf" args="0 0 0 0 0 0 1 base_link imu 20"/>-->
-    <node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps 20"/>
+        <!--<node pkg="tf" type="static_transform_publisher" name="gps_tf" args="0 0 0 0 0 0 1 base_link gps 20"/>-->
 
-    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
+        <!--<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">-->
-        <param name="output_frame" value="odom"/>
+            <!--<param name="output_frame" value="odom"/>-->
-        <param name="frequency" value="20"/>
+            <!--<param name="frequency" value="20"/>-->
-        <param name="odom_used" value="true"/>
+            <!--<param name="odom_used" value="true"/>-->
-        <param name="imu_used" value="true"/>
+            <!--<param name="imu_used" value="true"/>-->
-        <param name="vo_used" value="false"/>
+            <!--<param name="vo_used" value="false"/>-->
-        <param name="sensor_timeout" value="0.1"/>
+            <!--<param name="sensor_timeout" value="0.1"/>-->
-        <param name="two_d_mode" value="false"/>
+            <!--<param name="two_d_mode" value="false"/>-->
 
-        <param name="map_frame"       value="map"/>
+            <!--<param name="map_frame"       value="map"/>-->
-        <param name="odom_frame"      value="odom"/>
+            <!--<param name="odom_frame"      value="odom"/>-->
-        <param name="base_link_frame" value="base_link"/>
+            <!--<param name="base_link_frame" value="base_link"/>-->
-        <param name="world_frame"     value="odom"/>
+            <!--<param name="world_frame"     value="odom"/>-->
 
-        <param name="odom0" value="/odometry/gps"/>
+            <!--<param name="odom0" value="odometry/gps"/>-->
-        <param name="imu0"  value="/imu/data"/>
+            <!--<param name="imu0"  value="imu/data"/>-->
 
-        <rosparam param="odom0_config">[true, true, false,
+            <!--<rosparam param="odom0_config">[true, true, false,-->
-                                      false, false, false,
+                                          <!--false, false, false,-->
-                                      false , false, false,
+                                          <!--false , false, false,-->
-                                      false, false, false,
+                                          <!--false, false, false,-->
-                                      false, false, false]</rosparam>
+                                          <!--false, false, false]</rosparam>-->
 
-        <rosparam param="imu0_config">[false, false, false,
+            <!--<rosparam param="imu0_config">[false, false, false,-->
-                                     true , true , true,
+                                         <!--true , true , true,-->
-                                     false, false, false,
+                                         <!--false, false, false,-->
-                                     true , true , true ,
+                                         <!--true , true , true ,-->
-                                     true , true , true ]</rosparam>
+                                         <!--true , true , true ]</rosparam>-->
 
-        <param name="odom0_differential" value="false"/>
+            <!--<param name="odom0_differential" value="false"/>-->
-        <param name="imu0_differential"  value="false"/>
+            <!--<param name="imu0_differential"  value="false"/>-->
 
-        <param name="imu0_remove_gravitational_acceleration" value="true"/>
+            <!--<param name="imu0_remove_gravitational_acceleration" value="true"/>-->
 
-        <param name="odom0_relative" value="false"/>
+            <!--<param name="odom0_relative" value="false"/>-->
-        <param name="imu0_relative"  value="false"/>
+            <!--<param name="imu0_relative"  value="false"/>-->
 
-        <param name="print_diagnostics" value="true"/>
+            <!--<param name="print_diagnostics" value="true"/>-->
 
-        <!-- ======== ADVANCED PARAMETERS ======== -->
+            <!--&lt;!&ndash; ======== ADVANCED PARAMETERS ======== &ndash;&gt;-->
 
-        <param name="odom0_queue_size" value="2"/>
+            <!--<param name="odom0_queue_size" value="2"/>-->
-        <param name="imu0_queue_size" value="10"/>
+            <!--<param name="imu0_queue_size" value="10"/>-->
 
-    </node>
+        <!--</node>-->
 
-    <node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node" output="screen">
+        <!--<node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node" output="screen">-->
-            <param name="broadcast_utm_transform" value="true"/>
+                <!--<param name="broadcast_utm_transform" value="true"/>-->
-    </node>
+        <!--</node>-->
+    </group>
 </launch>

software/ros_packages/rover_odometry/src/odometry.py

@@ -75,9 +75,11 @@ class Odometry(object):
             imu = temp.get('imu', None)
 
             if gps:
+                print gps
                 self.broadcast_gps(gps)
 
             if imu:
+                # print imu
                 self.broadcast_imu(imu)
 
     def broadcast_gps(self, gps):