Initial odometry and localization working, though position grows without bound. It's a start though.

software/environment/rover/UDEV_rules/99-rover-usb-serial.rules

@@ -14,14 +14,14 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_0_3"
 
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_1_0"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_1_1"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyOdometry"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS_1_2"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyTowerAndPanTilt"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2HPA6V", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_1_3"
 
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_2_1"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_2_3"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART"
 
 
 ##### Mappings from OMSI / EXPO driving with individual adapters

software/firmware/motor_driver/motor_driver.ino

@@ -34,7 +34,7 @@ enum MODBUS_REGISTERS {
 };
 
 ////////// Global Variables //////////
-const uint8_t node_id = 1;
+const uint8_t node_id = 2;
 const uint8_t mobus_serial_port_number = 3;
 
 uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0};

software/ros_packages/rover_main/launch/rover.launch

@@ -12,4 +12,7 @@
     <include file="$(find rover_main)/launch/rover/topic_transport_senders.launch"/>
     <include file="$(find rover_main)/launch/rover/topic_transport_receivers.launch"/>
 
+    <!-- ########## Start Odometry Nodes ########## -->
+    <include file="$(find rover_main)/launch/rover/odometry.launch"/>
+
 </launch>

software/ros_packages/rover_main/launch/rover/odometry.launch

@@ -0,0 +1,71 @@
+<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="odometry">
+        <!-- ########## Processes GPS and IMU Messages ########## -->
+        <node name="gps_and_imu" pkg="rover_odometry" type="odometry.py" respawn="true" output="screen" />
+
+        <!-- ########## Converts GPS Sentences to GPS Fix data ########## -->
+        <node name="navsat_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen">
+            <remap from="nmea_sentence" to="gps/sentence"/>
+            <remap from="fix" to="gps/fix"/>
+        </node>
+
+        <!-- ########## Performs a tranform on the GPS and IMU frames so odom can use it ########## -->
+        <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"/>
+
+        <!-- ########## Main Odometry Localization Node ########## -->
+    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
+            <remap from="/gpx/fix" to="gps/fix"/>
+            <param name="output_frame" value="odom"/>
+            <param name="frequency" value="20"/>
+            <param name="odom_used" value="true"/>
+            <param name="imu_used" value="true"/>
+            <param name="vo_used" value="false"/>
+            <param name="sensor_timeout" value="0.1"/>
+            <param name="two_d_mode" value="false"/>
+
+            <param name="map_frame"       value="map"/>
+            <param name="odom_frame"      value="odom"/>
+            <param name="base_link_frame" value="base_link"/>
+            <param name="world_frame"     value="odom"/>
+
+            <param name="odom0" value="gps"/>
+            <param name="imu0"  value="imu/data"/>
+
+            <rosparam param="odom0_config">[true, true, true,
+                                          false, false, false,
+                                          false , false, false,
+                                          false, false, false,
+                                          false, false, false]</rosparam>
+
+            <rosparam param="imu0_config">[false, false, false,
+                                         true , true , true,
+                                         false, false, false,
+                                         true , true , true ,
+                                         true , true , true ]</rosparam>
+
+            <param name="odom0_differential" value="false"/>
+            <param name="imu0_differential"  value="false"/>
+
+            <param name="imu0_remove_gravitational_acceleration" value="true"/>
+
+            <param name="odom0_relative" value="false"/>
+            <param name="imu0_relative"  value="false"/>
+
+            <param name="print_diagnostics" value="true"/>
+
+            <!-- ======== ADVANCED PARAMETERS ======== -->
+
+            <param name="odom0_queue_size" value="2"/>
+            <param name="imu0_queue_size" value="10"/>
+
+        </node>
+
+        <node name="navsat_transform" pkg="robot_localization" type="navsat_transform_node" output="screen">
+                <param name="broadcast_utm_transform" value="true"/>
+        </node>
+    </group>
+</launch>

software/ros_packages/rover_odometry/src/odometry.py

@@ -4,28 +4,26 @@
 #####################################
 # Python native imports
 import rospy
-
+import serial
 from time import time, sleep
 import json
 
-import serial.rs485
-
 from nmea_msgs.msg import Sentence
-
+from sensor_msgs.msg import Imu
 
 #####################################
 # Global Variables
 #####################################
 NODE_NAME = "tower_odometry"
 
-# DEFAULT_PORT = "/dev/rover/ttyOdometry"
+DEFAULT_PORT = "/dev/rover/ttyOdometry"
-DEFAULT_PORT = "/dev/ttyUSB0"
+# DEFAULT_PORT = "/dev/ttyUSB0"
 DEFAULT_BAUD = 115200
 
 
 # DEFAULT_GPS_SENTENCE_TOPIC = "gps/sentence"
-DEFAULT_GPS_SENTENCE_TOPIC = "/nmea_sentence"
+DEFAULT_GPS_SENTENCE_TOPIC = "gps/sentence"
-DEFAULT_DRIVE_CONTROL_STATUS_TOPIC = "drive_status/rear"
+DEFAULT_IMU_TOPIC = "imu/data"
 
 DEFAULT_HERTZ = 100
 
@@ -41,6 +39,7 @@ class Odometry(object):
         self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
 
         self.gps_sentence_topic = rospy.get_param("~gps_sentence_topic", DEFAULT_GPS_SENTENCE_TOPIC)
+        self.imu_data_topic = rospy.get_param("~imu_data_topic", DEFAULT_IMU_TOPIC)
 
         self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
 
@@ -48,10 +47,10 @@ class Odometry(object):
         self.odom_serial.setRTS(0)
 
         self.sentence_publisher = rospy.Publisher(self.gps_sentence_topic, Sentence, queue_size=1)
+        self.imu_data_publisher = rospy.Publisher(self.imu_data_topic, Imu, queue_size=1)
 
         self.run()
 
-
     def run(self):
         while not rospy.is_shutdown():
             start_time = time()
@@ -62,37 +61,51 @@ class Odometry(object):
             except Exception, error:
                 print "Error occurred:", error
 
-
             time_diff = time() - start_time
 
             sleep(max(self.wait_time - time_diff, 0))
 
     def process_messages(self):
         if self.odom_serial.inWaiting():
-            line = self.odom_serial.readline()  # type:s
+            line = self.odom_serial.readline()
-            # print line
 
             temp = json.loads(line)
+
             gps = temp.get('gps', None)
+            imu = temp.get('imu', None)
 
             if gps:
                 self.broadcast_gps(gps)
 
-            imu = temp.get('imu', None)
-
             if imu:
-                print imu
+                self.broadcast_imu(imu)
-
-
-            # if not line.startswith("ox"):
-            #     print line,
-            # if line[0] == '$':
-            #     print line,
 
     def broadcast_gps(self, gps):
-        # print gps
+        message = Sentence()
-        self.sentence_publisher.publish(Sentence(sentence=gps))
+        message.header.frame_id = "gps"
+        message.header.stamp = rospy.get_rostime()
+        message.sentence = gps
+        self.sentence_publisher.publish(message)
 
+    def broadcast_imu(self, imu):
+        message = Imu()
+        message.header.frame_id = "imu"
+        message.header.stamp = rospy.get_rostime()
+
+        message.orientation.x = imu["ox"]
+        message.orientation.y = imu["oy"]
+        message.orientation.z = imu["oz"]
+        message.orientation.w = imu["ow"]
+
+        message.angular_velocity.x = imu["avx"]
+        message.angular_velocity.y = imu["avy"]
+        message.angular_velocity.z = imu["avz"]
+
+        message.linear_acceleration.x = imu["lax"]
+        message.linear_acceleration.y = imu["lay"]
+        message.linear_acceleration.z = imu["laz"]
+
+        self.imu_data_publisher.publish(message)
 
 if __name__ == "__main__":
     Odometry()