Got farther with mapping, heading indication, and firmware with cal for IMU. Need to figure out which direction IMU is in for ROS to work right.

software/firmware/tower/tower.ino

@@ -2,6 +2,8 @@
 #include <ModbusRtu.h>
 #include <Adafruit_BNO055_t3.h>
 #include <ArduinoJson.h>
+#include <utility/imumaths.h>
+#include <EEPROM.h>
 
 /*
    Imu/data (Imu)
@@ -99,7 +101,7 @@ const char baud115200[] = "PUBX,41,1,3,3,115200,0";
 void setup() {
   // Debugging
   Serial.begin(9600);
-//  while (!Serial);
+ // while (!Serial);
 
   // Raw pin setup
   setup_hardware();
@@ -113,16 +115,7 @@ void setup() {
   GPS_IMU_STREAMING_PORT.begin(115200);
   GPS_IMU_STREAMING_PORT.transmitterEnable(HARDWARE::GPS_IMU_RS485_EN);
 
-  // IMU Setup
+  setup_imu();
-  Serial.println("Setting up IMU");
-  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!");
-    while (1);
-  }
-  Serial.println("IMU Online. Setting to external crystal.");
-  bno.setExtCrystalUse(true);
-  Serial.println("IMU Configured.");
 
   // GPS Setup
   GPS_SERIAL_PORT.begin(9600);
@@ -186,18 +179,73 @@ void send_imu_stream_line(JsonObject &root) {
   imu_object["avy"] = angular_vel.y();
   imu_object["avz"] = angular_vel.z();
 
-  //
+}
-  //  /* Display calibration status for each sensor. */
+
-  //  uint8_t system, gyro, accel, mag = 0;
+void setup_imu(){
-  //  bno.getCalibration(&system, &gyro, &accel, &mag);
+
-  //  Serial.print("CALIBRATION: Sys=");
+    // IMU Setup
-  //  Serial.print(system, DEC);
+    Serial.println("Setting up IMU");
-  //  Serial.print(" Gyro=");
+    if (!bno.begin()) {
-  //  Serial.print(gyro, DEC);
+      /* There was a problem detecting the BNO055 ... check your connections */
-  //  Serial.print(" Accel=");
+      Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
-  //  Serial.print(accel, DEC);
+      while (1);
-  //  Serial.print(" Mag=");
+    }
-  //  Serial.print(mag, DEC);
+    Serial.println("IMU Online. Setting to external crystal.");
+
+    int eeAddress = 0;
+    long bnoID;
+    bool foundCalib = false;
+
+    EEPROM.get(eeAddress, bnoID);
+
+    adafruit_bno055_offsets_t calibrationData;
+    sensor_t sensor;
+
+    /*
+    *  Look for the sensor's unique ID at the beginning oF EEPROM.
+    *  This isn't foolproof, but it's better than nothing.
+    */
+    bno.getSensor(&sensor);
+    eeAddress += sizeof(long);
+    EEPROM.get(eeAddress, calibrationData);
+    bno.setSensorOffsets(calibrationData);
+    bno.setAxisConfig(Adafruit_BNO055::REMAP_CONFIG_P6);
+    bno.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P6);
+    bno.setMode(Adafruit_BNO055::OPERATION_MODE_IMUPLUS);
+    delay(300);
+
+    bno.setExtCrystalUse(true);
+    Serial.println("IMU Configured");
+    // while(1){
+    //     display_cal_status();
+    // }
+}
+
+void display_cal_status(void)
+{
+  /* Get the four calibration values (0..3) */
+  /* Any sensor data reporting 0 should be ignored, */
+  /* 3 means 'fully calibrated" */
+  uint8_t system, gyro, accel, mag;
+  system = gyro = accel = mag = 0;
+  bno.getCalibration(&system, &gyro, &accel, &mag);
+
+  /* The data should be ignored until the system calibration is > 0 */
+  Serial.print("\t");
+  if (!system)
+  {
+    Serial.print("! ");
+  }
+
+  /* Display the individual values */
+  Serial.print("Sys:");
+  Serial.print(system, DEC);
+  Serial.print(" G:");
+  Serial.print(gyro, DEC);
+  Serial.print(" A:");
+  Serial.print(accel, DEC);
+  Serial.print(" M:");
+  Serial.println(mag, DEC);
 }
 
 void process_gps_and_send_if_ready(JsonObject &root) {

software/reference/design_reference/UI Design/rover_icon.svg

@@ -0,0 +1,73 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="1000"
+   height="1000"
+   viewBox="0 0 1000 1000"
+   id="svg2"
+   version="1.1"
+   inkscape:version="0.91 r13725"
+   sodipodi:docname="rover_icon.svg"
+   inkscape:export-filename="/home/caperren/Github/Rover_2017_2018/software/ros_packages/ground_station/src/Resources/Images/rover.png"
+   inkscape:export-xdpi="400"
+   inkscape:export-ydpi="400">
+  <defs
+     id="defs4" />
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#323232"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="0.35"
+     inkscape:cx="338.99045"
+     inkscape:cy="775.33545"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer1"
+     showgrid="false"
+     inkscape:window-width="2556"
+     inkscape:window-height="1401"
+     inkscape:window-x="1080"
+     inkscape:window-y="18"
+     inkscape:window-maximized="0"
+     units="px" />
+  <metadata
+     id="metadata7">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+        <dc:title />
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="Layer 1"
+     inkscape:groupmode="layer"
+     id="layer1"
+     transform="translate(0,-52.362161)">
+    <path
+       style="fill:#1974df;fill-opacity:1;stroke:#ffffff;stroke-width:26;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       d="M 499.63597,74.698266 319.16703,356.39733 c 37.73561,-64.04828 106.4946,-103.40171 180.8327,-103.49752 74.34377,0.10172 143.10405,39.46639 180.8327,103.52539 L 499.63487,74.698266 Z"
+       id="rect4196"
+       inkscape:connector-curvature="0"
+       sodipodi:nodetypes="cccccc" />
+    <circle
+       style="fill:#1974df;fill-opacity:1;stroke:#ffffff;stroke-width:30;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       id="path4157-6"
+       cx="500"
+       cy="552.36218"
+       r="229.86818" />
+  </g>
+</svg>

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py

@@ -453,7 +453,7 @@ class OverlayImage(object):
         return self.display_image
 
     def load_rover_icon(self):
-        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((50, 50))
+        self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((60, 60))
 
     def _draw_coordinate_text(self, latitude, longitude):
         location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
@@ -475,7 +475,7 @@ class OverlayImage(object):
         y -= 25
 
         rotated = self.indicator.copy()
-        rotated = rotated.rotate(angle, resample=PIL.Image.BICUBIC)
+        rotated = rotated.rotate(-angle, resample=PIL.Image.BICUBIC)
         # rotated.save("rotated.png")
         self.big_image.paste(rotated, (x, y), rotated)
         if self.write_once:
@@ -483,7 +483,8 @@ class OverlayImage(object):
             self.write_once = False
 
     def update(self, latitude, longitude):
-
+        # self.left_x -= 50
+        # self.upper_y -= 50
         self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
         self._draw_coordinate_text(latitude, longitude)
 

software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py

@@ -10,6 +10,10 @@ import numpy
 import logging
 
 import rospy
+from tf import transformations
+from scipy.interpolate import interp1d
+import math
+from sensor_msgs.msg import Imu
 
 # Custom Imports
 import RoverMap
@@ -21,6 +25,7 @@ from sensor_msgs.msg import NavSatFix
 # put some stuff here later so you can remember
 
 GPS_POSITION_TOPIC = "/rover_odometry/fix"
+IMU_DATA_TOPIC = "/rover_odometry/imu/data"
 
 
 class RoverMapCoordinator(QtCore.QThread):
@@ -58,6 +63,17 @@ class RoverMapCoordinator(QtCore.QThread):
         self.latitude = None
         self.last_heading = 0
 
+        self.imu_data = None
+        self.new_imu_data = False
+
+        self.yaw = None
+        self.pitch = None
+        self.roll = None
+
+        self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180])
+
+        self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
+
     def run(self):
         self.logger.debug("Starting Map Coordinator Thread")
         self.pixmap_ready_signal.emit()  # This gets us the loading map
@@ -66,6 +82,10 @@ class RoverMapCoordinator(QtCore.QThread):
                 self._map_setup()
                 self.setup_map_flag = False
             else:
+                if self.new_imu_data:
+                    self.calculate_euler_from_imu()
+                    self.new_imu_data = False
+
                 self._get_map_image()
             self.msleep(30)
 
@@ -158,3 +178,17 @@ class RoverMapCoordinator(QtCore.QThread):
     def gps_position_updated_callback(self, data):
         self.latitude = data.latitude
         self.longitude = data.longitude
+
+    def on_imu_data_received(self, data):
+        self.imu_data = data
+        self.new_imu_data = True
+
+    def calculate_euler_from_imu(self):
+        quat = (
+            self.imu_data.orientation.x,
+            self.imu_data.orientation.y,
+            self.imu_data.orientation.z,
+            self.imu_data.orientation.w,
+        )
+        self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
+        self.last_heading = self.euler_interpolator(self.yaw) % 360

software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py

@@ -9,6 +9,11 @@ from time import time
 import PIL.Image
 from PIL.ImageQt import ImageQt
 from random import random
+import rospy
+from tf import transformations
+from scipy.interpolate import interp1d
+import math
+from sensor_msgs.msg import Imu
 
 #####################################
 # Global Variables
@@ -17,6 +22,8 @@ THREAD_HERTZ = 20
 
 ROTATION_SPEED_MODIFIER = 2.5
 
+IMU_DATA_TOPIC = "/rover_odometry/imu/data"
+
 
 #####################################
 # Controller Class Definition
@@ -61,12 +68,27 @@ class SpeedAndHeadingIndication(QtCore.QThread):
         self.last_current_heading_shown = 0
         self.rotation_direction = 1
 
+        self.imu_data = None
+        self.new_imu_data = False
+
+        self.yaw = None
+        self.pitch = None
+        self.roll = None
+
+        self.euler_interpolator = interp1d([math.pi, -math.pi], [-180, 180])
+
+        self.imu_data_subscriber = rospy.Subscriber(IMU_DATA_TOPIC, Imu, self.on_imu_data_received)
+
     def run(self):
         self.on_heading_changed__slot(self.current_heading)
 
         while self.run_thread_flag:
             start_time = time()
 
+            if self.new_imu_data:
+                self.calculate_euler_from_imu()
+                self.new_imu_data = False
+
             if self.current_heading_changed:
                 self.update_heading_movement()
                 self.current_heading_changed = False
@@ -77,27 +99,27 @@ class SpeedAndHeadingIndication(QtCore.QThread):
 
             self.msleep(max(int(self.wait_time - time_diff), 0))
 
+    def calculate_euler_from_imu(self):
+        quat = (
+            self.imu_data.orientation.x,
+            self.imu_data.orientation.y,
+            self.imu_data.orientation.z,
+            self.imu_data.orientation.w,
+        )
+        self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat)
+        self.current_heading = self.euler_interpolator(self.yaw) % 360
+
     def rotate_compass_if_needed(self):
-        heading_difference = abs(int(self.shown_heading) - self.current_heading)
-        should_update = False
 
-        if heading_difference > ROTATION_SPEED_MODIFIER:
+        self.current_heading_shown_rotation_angle = int(self.current_heading)
-            self.shown_heading += self.rotation_direction * ROTATION_SPEED_MODIFIER
-            self.shown_heading %= 360
-            should_update = True
-        elif heading_difference != 0:
-            self.shown_heading = self.current_heading
-            should_update = True
 
-        if should_update:
+        if self.current_heading_shown_rotation_angle != self.last_current_heading_shown:
-            self.current_heading_shown_rotation_angle = int(self.shown_heading)
+            new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC)
+            self.last_current_heading_shown = self.current_heading_shown_rotation_angle
 
-            if self.current_heading_shown_rotation_angle != self.last_current_heading_shown:
+            self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image))
-                new_compass_image = self.main_compass_image.rotate(self.current_heading_shown_rotation_angle, resample=PIL.Image.BICUBIC)
+            self.show_compass_image__signal.emit()
-                self.last_current_heading_shown = self.current_heading_shown_rotation_angle
+            self.heading_text_update_ready__signal.emit(str(self.current_heading_shown_rotation_angle) + "°")
-
-                self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image))
-                self.show_compass_image__signal.emit()
 
     def update_heading_movement(self):
         current_minus_shown = (self.current_heading - self.shown_heading) % 360
@@ -126,6 +148,10 @@ class SpeedAndHeadingIndication(QtCore.QThread):
     def on_new_compass_image_ready__slot(self):
         self.heading_compass_label.setPixmap(self.compass_pixmap)
 
+    def on_imu_data_received(self, data):
+        self.imu_data = data
+        self.new_imu_data = True
+
     def connect_signals_and_slots(self):
         self.show_compass_image__signal.connect(self.on_new_compass_image_ready__slot)
         self.heading_text_update_ready__signal.connect(self.heading_text_label.setText)

software/ros_packages/rover_camera/src/rover_camera.cpp

@@ -26,6 +26,8 @@ public:
         node_handle->param("device_path", capture_device_path, std::string("/dev/video0"));
         node_handle->param("fps", fps, 30);
 
+        node_handle->param("upside_down", upside_down, false);
+
         node_handle->param("large_image_width", large_image_width, 1280);
         node_handle->param("large_image_height", large_image_height, 720);
         node_handle->param("medium_image_width", medium_image_width, 640);
@@ -97,30 +99,22 @@ public:
             }
 
             if(!image_large.empty()){
-                bool large_ready_to_broadcast = false;
+                if(upside_down){
-                bool medium_ready_to_broadcast = false;
+                    cv::flip(image_large, image_large, -1);
-                bool small_ready_to_broadcast = false;
+                }
 
                 if(broadcast_large_image){
                     large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
-                    large_ready_to_broadcast = true;
+                    large_image_publisher.publish(large_image_message);
-                }
+                }else if(broadcast_medium_image){
-
-                if(broadcast_medium_image){
                     cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
                     medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
-                    medium_ready_to_broadcast = true;
+                    medium_image_publisher.publish(medium_image_message);
-                }
+                }else if(broadcast_small_image){
-
-                if(broadcast_small_image){
                     cv::resize(image_large, image_small, cv::Size(small_image_width, small_image_height));
                     small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
-                    small_ready_to_broadcast = true;
+                    small_image_publisher.publish(small_image_message);
                 }
-
-                if(large_ready_to_broadcast){ large_image_publisher.publish(large_image_message); };
-                if(medium_ready_to_broadcast){ medium_image_publisher.publish(medium_image_message); };
-                if(small_ready_to_broadcast){ small_image_publisher.publish(small_image_message); };
             }
 
             ros::spinOnce();
@@ -152,6 +146,8 @@ private:
 
     ros::Rate *loop_rate;
 
+    bool upside_down;
+
     int large_image_width;
     int large_image_height;
     int medium_image_width;

software/ros_packages/rover_main/launch/rover/cameras.launch

@@ -3,6 +3,7 @@
         <!-- Start Undercarriage Camera -->
         <node name="undercarriage" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 1" respawn="true" output="screen">
             <param name="device_path" value="/dev/rover/camera_undercarriage"/>
+            <param name="upside_down" value="true"/>
         </node>
 
         <!--&lt;!&ndash; Start Main Navigation Camera &ndash;&gt;-->
@@ -17,7 +18,7 @@
 
         <!--&lt;!&ndash; Start End Effector Camera &ndash;&gt;-->
         <node name="end_effector" pkg="rover_camera" type="rover_camera" launch-prefix="taskset -c 4" respawn="true" output="screen" >
-            <param name="is_rtsp_camera" value="True" />
+            <param name="is_rtsp_camera" value="true" />
             <param name="device_path" value="rtsp://192.168.1.11" />
         </node>
     </group>

software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch

@@ -153,7 +153,8 @@
             <rosparam param="topics">
                 [
                 {name: "/rover_status/battery_status", compress: false, rate: 1.0},
-                {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0}
+                {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
+                {name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
                 ]
             </rosparam>
         </node>