diff --git a/ground_station_git_reset.sh b/ground_station_git_reset.sh new file mode 100755 index 0000000..b4fe12e --- /dev/null +++ b/ground_station_git_reset.sh @@ -0,0 +1,8 @@ +#!/bin/bash +test -d ~/Maps_Backup || mkdir -p ~/Maps_Backup +cp -r software/ros_packages/ground_station/src/Resources/Maps/* ~/Maps_Backup +git reset --hard +git clean -fdx +git pull +test -d ~/software/ros_packages/ground_station/src/Resources/Maps || mkdir -p software/ros_packages/ground_station/src/Resources/Maps +cp -r ~/Maps_Backup/* software/ros_packages/ground_station/src/Resources/Maps/ diff --git a/rover_git_reset.sh b/rover_git_reset.sh new file mode 100755 index 0000000..bd3bf66 --- /dev/null +++ b/rover_git_reset.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +git reset --hard +git clean -fdx +git pull diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index 4ef8dc8..1f4410d 100644 --- a/software/firmware/tower/tower.ino +++ b/software/firmware/tower/tower.ino @@ -2,6 +2,8 @@ #include #include #include +#include +#include /* 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 - 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."); + setup_imu(); // GPS Setup GPS_SERIAL_PORT.begin(9600); @@ -170,7 +163,7 @@ void send_imu_stream_line(JsonObject &root) { JsonObject& imu_object = root.createNestedObject("imu"); quat = bno.getQuat(); - linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + linear_accel = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER); angular_vel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); imu_object["ox"] = quat.x(); @@ -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; - // bno.getCalibration(&system, &gyro, &accel, &mag); - // Serial.print("CALIBRATION: Sys="); - // Serial.print(system, DEC); - // Serial.print(" Gyro="); - // Serial.print(gyro, DEC); - // Serial.print(" Accel="); - // Serial.print(accel, DEC); - // Serial.print(" Mag="); - // Serial.print(mag, DEC); +} + +void setup_imu(){ + + // IMU Setup + 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."); + + 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) { diff --git a/software/reference/design_reference/UI Design/maps_loading.svg b/software/reference/design_reference/UI Design/maps_loading.svg new file mode 100644 index 0000000..6d792c6 --- /dev/null +++ b/software/reference/design_reference/UI Design/maps_loading.svg @@ -0,0 +1,4382 @@ + + + + + + + + + + image/svg+xml + + + + + + + + Maps loading... + + diff --git a/software/reference/design_reference/UI Design/rover_icon.svg b/software/reference/design_reference/UI Design/rover_icon.svg new file mode 100644 index 0000000..a77dc59 --- /dev/null +++ b/software/reference/design_reference/UI Design/rover_icon.svg @@ -0,0 +1,73 @@ + + + + + + + + + + image/svg+xml + + + + + + + + + + diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py index d33a124..8cee877 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -26,8 +26,12 @@ from io import StringIO, BytesIO import os import time import PIL.ImageDraw +import PIL.Image +import PIL.ImageFont import signing import RoverMapHelper as MapHelper +import cv2 +import numpy as np ##################################### # Constants @@ -36,7 +40,8 @@ _KEYS = [] # Number of pixels in half the earth's circumference at zoom = 21 _EARTHPIX = 268435456 # Number of decimal places for rounding coordinates -_DEGREE_PRECISION = 4 +_DEGREE_PRECISION = 6 +_PRECISION_FORMAT = '%.' + str(_DEGREE_PRECISION) + 'f' # Larget tile we can grab without paying _TILESIZE = 640 # Fastest rate at which we can download tiles without paying @@ -113,14 +118,14 @@ class GMapsStitcher(object): # Make the url string for polling # GET request header gets appended to the string urlbase = 'https://maps.googleapis.com/maps/api/staticmap?' - urlbase += 'center=%.4f,%.4f&zoom=%d&maptype=%s' + urlbase += 'center=' + _PRECISION_FORMAT + ',' + _PRECISION_FORMAT + '&zoom=%d&maptype=%s' urlbase += '&size=%dx%d&format=png&key=%s' # Fill the formatting specs = (self.helper.fast_round(latitude, _DEGREE_PRECISION), self.helper.fast_round(longitude, _DEGREE_PRECISION), self.zoom, self.maptype, _TILESIZE, _TILESIZE, _KEYS[0]) - filename = 'Resources/Maps/' + ('%.4f_%.4f_%d_%s_%d_%d_%s' % specs) + filename = 'Resources/Maps/' + ((_PRECISION_FORMAT + '_' + _PRECISION_FORMAT + '_%d_%s_%d_%d_%s') % specs) filename += '.png' # Tile Image object @@ -134,7 +139,13 @@ class GMapsStitcher(object): # make the url url = urlbase % specs url = signing.sign_url(url, _KEYS[1]) - result = urllib2.urlopen(urllib2.Request(url)).read() + try: + result = urllib2.urlopen(urllib2.Request(url)).read() + except urllib2.HTTPError, e: + print "Error accessing url for reason:", e + print url + return + tile_object = PIL.Image.open(BytesIO(result)) if not os.path.exists('Resources/Maps'): os.mkdir('Resources/Maps') @@ -165,10 +176,10 @@ class GMapsStitcher(object): """ # Magic Lines return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels + - self.helper.pixels_to_degrees( - (iterator - self.num_tiles / - 2) * _TILESIZE, self.zoom)) - - _EARTHPIX) / _PIXRAD))) + self.helper.pixels_to_degrees( + (iterator - self.num_tiles / + 2) * _TILESIZE, self.zoom)) - + _EARTHPIX) / _PIXRAD))) def fetch_tiles(self): """ @@ -188,14 +199,14 @@ class GMapsStitcher(object): # latitude to desired radius in meters if self.radius_meters is not None: self.num_tiles = (int( - round(2 * self.helper.pixels_to_meters( - self.latitude, self.zoom) / - (_TILESIZE / 2. / self.radius_meters)))) + round(2 * self.helper.pixels_to_meters( + self.latitude, self.zoom) / + (_TILESIZE / 2. / self.radius_meters)))) lon_pixels = _EARTHPIX + self.longitude * math.radians(_PIXRAD) sin_lat = math.sin(math.radians(self.latitude)) - lat_pixels = _EARTHPIX - _PIXRAD * math.log((1+sin_lat)/(1-sin_lat))/2 + lat_pixels = _EARTHPIX - _PIXRAD * math.log((1 + sin_lat) / (1 - sin_lat)) / 2 self.big_size = self.num_tiles * _TILESIZE big_image = self.helper.new_image(self.big_size, self.big_size) @@ -231,7 +242,7 @@ class GMapsStitcher(object): new_value = self.left_x - diff if ((not new_value > 0) and - (new_value < self.big_image.size[0] - self.width)): + (new_value < self.big_image.size[0] - self.width)): return self.left_x else: return new_value @@ -243,7 +254,7 @@ class GMapsStitcher(object): new_value = self.upper_y - diff if ((not new_value > 0) and - (new_value < self.big_image.size[1] - self.height)): + (new_value < self.big_image.size[1] - self.height)): return self.upper_y else: return new_value @@ -266,8 +277,8 @@ class GMapsStitcher(object): Function to move the object/rover """ x, y = self._get_cartesian(lat, lon) - self._constrain_x(self.center_x-x) - self._constrain_y(self.center_y-y) + self._constrain_x(self.center_x - x) + self._constrain_y(self.center_y - y) self.update() def _get_cartesian(self, lat, lon): @@ -309,9 +320,9 @@ class GMapsStitcher(object): x, y = self._get_cartesian(lat, lon) draw = PIL.ImageDraw.Draw(self.big_image) if shape is "ellipsis": - draw.ellipsis((x-size, y-size, x+size, y+size), fill) + draw.ellipsis((x - size, y - size, x + size, y + size), fill) else: - draw.rectangle([x-size, y-size, x+size, y+size], fill) + draw.rectangle([x - size, y - size, x + size, y + size], fill) self.update() def center_display(self, lat, lon): @@ -322,8 +333,8 @@ class GMapsStitcher(object): self.center_x = x self.center_y = y - self.left_x = (self.center_x - (self.width/2)) - self.upper_y = (self.center_y - (self.height/2)) + self.left_x = (self.center_x - (self.width / 2)) + self.upper_y = (self.center_y - (self.height / 2)) self.update() # def update_rover_map_location(self, lat, lon): @@ -348,7 +359,9 @@ class OverlayImage(object): self.width = width self.height = height self.big_image = None + self.big_image_copy = None self.display_image = None + self.display_image_copy = None self.indicator = None self.helper = MapHelper.MapHelper() @@ -356,12 +369,19 @@ class OverlayImage(object): self.center_x = x self.center_y = y - self.left_x = (self.center_x - (self.width/2)) - self.upper_y = (self.center_y - (self.height/2)) + self.left_x = (self.center_x - (self.width / 2)) + self.upper_y = (self.center_y - (self.height / 2)) self.generate_image_files() self.write_once = True + # Text Drawing Variables + self.font = cv2.FONT_HERSHEY_TRIPLEX + self.font_thickness = 1 + self.font_baseline = 0 + + self.nav_coordinates_text_image = None + def generate_image_files(self): """ Creates big_image and display image sizes @@ -370,9 +390,15 @@ class OverlayImage(object): """ self.big_image = self.helper.new_image(self.big_width, self.big_height, True) + + self.big_image_copy = self.big_image.copy() + self.display_image = self.helper.new_image(self.width, self.height, True) - self.generate_dot_and_hat() + + self.display_image_copy = self.display_image.copy() + + self.load_rover_icon() self.indicator.save("location.png") def _get_cartesian(self, lat, lon): @@ -404,45 +430,51 @@ class OverlayImage(object): return int(x), int(y) - def update_new_location(self, latitude, longitude, + def update_new_location(self, latitude, longitude, compass, navigation_list, landmark_list): + self.big_image = self.big_image_copy.copy() + self.display_image = self.display_image_copy.copy() + size = 5 draw = PIL.ImageDraw.Draw(self.big_image) + for element in navigation_list: - x, y = self._get_cartesian(float(element[2]), float(element[1])) - draw.ellipse((x-size, y-size, x+size, y+size), fill="red") - # for element in landmark_list: - # x, y = self._get_cartesian(element[1], element[2]) - # draw.ellipsis((x-size, y-size, x+size, y+size), fill="blue") - self._draw_rover(longitude, latitude, compass) - self.update() + x, y = self._get_cartesian(float(element[1]), float(element[2])) + draw.text((x + 10, y - 5), str(element[0])) + draw.ellipse((x - size, y - size, x + size, y + size), fill=(element[3].red(), element[3].green(), element[3].blue())) + + for element in landmark_list: + x, y = self._get_cartesian(element[1], element[2]) + draw.text((x + 10, y - 5), str(element[0])) + draw.ellipse((x - size, y - size, x + size, y + size), fill=(element[3].red(), element[3].green(), element[3].blue())) + + self._draw_rover(latitude, longitude, compass) + self.update(latitude, longitude) return self.display_image - def generate_dot_and_hat(self): - self.indicator = self.helper.new_image(100, 100, True) - draw = PIL.ImageDraw.Draw(self.indicator) - draw.ellipse((50-12, 50-12, 50+12, 50+12), fill="red") - draw.line((25, 40, 50, 12), fill="red", width=7) - draw.line((50, 12, 75, 40), fill="red", width=7) + def load_rover_icon(self): + self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((40, 40)) def _draw_rover(self, lat, lon, angle=0): x, y = self._get_cartesian(lat, lon) - # print x,y - # Center of the circle on the indicator is (12.5, 37.5) - x = x - 50 - y = y - 50 + + x -= 25 # Half the height of the icon + y -= 25 + rotated = self.indicator.copy() - rotated = rotated.rotate(angle, expand=True) - rotated.save("rotated.png") + rotated = rotated.rotate(-angle, resample=PIL.Image.BICUBIC) + # rotated.save("rotated.png") self.big_image.paste(rotated, (x, y), rotated) if self.write_once: - self.display_image.save("Something.png") + # self.display_image.save("Something.png") self.write_once = False - def update(self): + 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) def connect_signals_and_slots(self): pass - diff --git a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py index bd45b33..d9959b1 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -5,14 +5,22 @@ from PyQt5 import QtCore, QtWidgets, QtGui from PIL.ImageQt import ImageQt from PIL import Image +import PIL.ImageDraw +import PIL.Image +import PIL.ImageFont 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 +from Resources.Settings import Mapping as MappingSettings from sensor_msgs.msg import NavSatFix ##################################### @@ -21,6 +29,12 @@ 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" + + +MAP_WIDGET_WIDTH = float(1280) +MAP_WIDGET_HEIGHT = float(720) +MAP_WIDGET_RATIO = MAP_WIDGET_WIDTH / MAP_WIDGET_HEIGHT class RoverMapCoordinator(QtCore.QThread): @@ -32,7 +46,7 @@ class RoverMapCoordinator(QtCore.QThread): self.shared_objects = shared_objects self.left_screen = self.shared_objects["screens"]["left_screen"] - self.mapping_label = self.left_screen.mapping_label + self.mapping_label = self.left_screen.mapping_label # type:QtWidgets.QLabel self.navigation_label = self.left_screen.navigation_waypoints_table_widget self.landmark_label = self.left_screen.landmark_waypoints_table_widget @@ -47,43 +61,65 @@ class RoverMapCoordinator(QtCore.QThread): self.google_maps_object = None self.map_image = None + self.map_image_copy = None self.overlay_image = None self.overlay_image_object = None - self.map_pixmap = None + self.map_pixmap = QtGui.QPixmap.fromImage(ImageQt(Image.open("Resources/Images/maps_loading.png").resize((1280, 720), Image.BICUBIC))) self.last_map_pixmap_cache_key = None - self.longitude = None - self.latitude = None + self.longitude = 0 + self.latitude = 0 + 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) + + self.zoom_pan_x_location = 0 + self.zoom_pan_y_location = 0 + + self.zoom_subtraction = 0 + + self.x_drag = 0 + self.y_drag = 0 + + self.x_drag_start = -1 + self.y_drag_start = -1 + + self.x_drag_end = -1 + self.y_drag_end = -1 def run(self): self.logger.debug("Starting Map Coordinator Thread") - + self.pixmap_ready_signal.emit() # This gets us the loading map while self.run_thread_flag: if self.setup_map_flag: 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) + self.msleep(10) self.logger.debug("Stopping Map Coordinator Thread") - # def _setup_map_threads(self): - # self.google_maps_object = RoverMap.GMapsStitcher(1280, - # 720, - # 44.567161, - # -123.278432, - # 18, - # 'satellite', - # None, 20) - def _map_setup(self): self.google_maps_object = RoverMap.GMapsStitcher(1280, 720, 44.5675721667, -123.2750535, - 20, # FIXME: Used to be 18 + 18, # FIXME: Used to be 18 'satellite', None, 20) self.overlay_image_object = ( @@ -97,30 +133,65 @@ class RoverMapCoordinator(QtCore.QThread): def _get_map_image(self): while self.map_image is None: self.map_image = self.google_maps_object.display_image - # self.overlay_image_object.update_new_location(44.567161, - # -123.278432, - # .7, - # [], - # []) + + if self.map_image: + self.map_image_copy = self.map_image.copy() + self.update_overlay() + + self.map_image = self.map_image_copy.copy() self.map_image.paste(self.overlay_image_object.display_image, (0, 0), self.overlay_image_object.display_image) - # self.map_image = Image.alpha_composite( - # self.google_maps_object.display_image, - # self.overlay_image_object.display_image) - # get overlay here + + # ##### Zoom testing! ##### + if self.zoom_subtraction: + self.zoom_subtraction = self.constrain(self.zoom_subtraction, 0, 520) + + crop_x_start = ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2) - self.x_drag - self.x_drag_end + crop_y_start = (self.zoom_subtraction / 2) - self.y_drag - self.y_drag_end + crop_x_end = (MAP_WIDGET_WIDTH - ((self.zoom_subtraction * MAP_WIDGET_RATIO) / 2)) - self.x_drag - self.x_drag_end + crop_y_end = (MAP_WIDGET_HEIGHT - (self.zoom_subtraction / 2)) - self.y_drag - self.y_drag_end + crop_box = (int(crop_x_start), int(crop_y_start), int(crop_x_end), int(crop_y_end)) + + self.map_image = self.map_image.crop(crop_box) + self.map_image = self.map_image.resize((1280, 720), resample=PIL.Image.BICUBIC) + + # ##### Draw coordinates ##### + self._draw_coordinate_text(self.latitude, self.longitude) + qim = ImageQt(self.map_image) self.map_pixmap = QtGui.QPixmap.fromImage(qim) - if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key: - self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey() - self.pixmap_ready_signal.emit() + # if self.map_pixmap.cacheKey() != self.last_map_pixmap_cache_key: + # self.last_map_pixmap_cache_key = self.map_pixmap.cacheKey() + self.pixmap_ready_signal.emit() + + def _draw_coordinate_text(self, latitude, longitude): + + if latitude == 0 and longitude == 0: + location_text = "LAT: NO FIX\nLON: NO FIX" + else: + location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude) + + font = PIL.ImageFont.truetype("UbuntuMono-R", size=20) + + new_image = PIL.Image.new('RGBA', (200, 45), "black") + + draw = PIL.ImageDraw.Draw(new_image) + + draw.multiline_text((5, 0), location_text, font=font) + + self.map_image.paste(new_image, (0, 0)) def connect_signals_and_slots(self): self.pixmap_ready_signal.connect(self.pixmap_ready__slot) self.change_waypoint_signal.connect(self.update_overlay) + self.mapping_label.mouseReleaseEvent = self.__mouse_released_event + self.mapping_label.wheelEvent = self.__mouse_wheel_event + self.mapping_label.mouseMoveEvent = self.__mouse_move_event + def on_kill_threads_requested_slot(self): self.run_thread_flag = False @@ -137,31 +208,79 @@ class RoverMapCoordinator(QtCore.QThread): temp_list = [] count = UI_element.rowCount() for row in range(0, count): - num = UI_element.item(row, 0).text() + name = UI_element.item(row, 0).text() lat = float(UI_element.item(row, 1).text()) lng = float(UI_element.item(row, 2).text()) - temp_tuple = (num, lat, lng) + color = UI_element.item(row, 3).background().color() + temp_tuple = (name, lat, lng, color) temp_list.append(temp_tuple) return temp_list def update_overlay(self): - if self.latitude and self.longitude: - if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude): + if not numpy.isnan(self.latitude) and not numpy.isnan(self.longitude): + latitude = float(self.latitude) + longitude = float(self.longitude) - longitude = self.latitude - latitude = self.longitude - - navigation_list = self._get_table_elements(self.navigation_label) - # landmark_list = self._get_table_elements(self.landmark_label) - landmark_list = [] - self.overlay_image = self.overlay_image_object.update_new_location( - latitude, - longitude, - 70, - navigation_list, - landmark_list) - # self.overlay_image.save("something.png") + navigation_list = self._get_table_elements(self.navigation_label) + landmark_list = self._get_table_elements(self.landmark_label) + self.overlay_image = self.overlay_image_object.update_new_location( + latitude, + longitude, + self.last_heading, + navigation_list, + landmark_list) + # self.last_heading = (self.last_heading + 5) % 360 + # self.overlay_image.save("something.png") 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) + MappingSettings.DECLINATION_OFFSET) % 360 + + def __mouse_released_event(self, _): + self.x_drag_start = -1 + self.y_drag_start = -1 + + self.x_drag_end += self.x_drag + self.y_drag_end += self.y_drag + + self.x_drag = 0 + self.x_drag = 0 + + + def __mouse_wheel_event(self, event): + # print "wheel:", event.angleDelta() + self.zoom_subtraction += event.angleDelta().y() / 12 + + def __mouse_move_event(self, event): + if self.x_drag_start != -1 and self.x_drag_start != -1: + buttons = event.buttons() + + if buttons == QtCore.Qt.LeftButton: + x_pos = event.pos().x() + y_pos = event.pos().y() + dx = x_pos - self.x_drag_start + dy = y_pos - self.y_drag_start + + self.x_drag = dx + self.y_drag = dy + else: + self.x_drag_start = event.pos().x() + self.y_drag_start = event.pos().y() + + @staticmethod + def constrain(val, min_val, max_val): + return min(max_val, max(min_val, val)) \ No newline at end of file diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py new file mode 100644 index 0000000..952f715 --- /dev/null +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py @@ -0,0 +1,47 @@ +# coding=utf-8 +##################################### +# Imports +##################################### +# Python native imports +from PyQt5 import QtCore, QtWidgets +import logging +from time import time +import paramiko + +##################################### +# Global Variables +##################################### +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" +ACCESS_POINT_PASSWORD = "rover4lyfe^" # We don't care about this password, don't freak out. Wifi is open anyways... + + +##################################### +# UbiquitiRadioSettings Class Definition +##################################### +class SSHConsole(QtCore.QThread): + def __init__(self, shared_objects): + super(SSHConsole, self).__init__() + + # ########## Reference to class init variables ########## + self.shared_objects = shared_objects + self.left_screen = self.shared_objects["screens"]["left_screen"] + + self.ubiquiti_channel_spin_box = self.left_screen.ssh_console_widget # type: QtWidgets.QSpinBox + self.ubiquiti_channel_apply_button = self.left_screen.ubiquiti_channel_apply_button # type: QtWidgets.QPushButton + + # ########## Get the settings instance ########## + self.settings = QtCore.QSettings() + + # ########## Get the Pick And Plate instance of the logger ########## + self.logger = logging.getLogger("groundstation") + + # ########## Thread Flags ########## + self.run_thread_flag = True + + # ########## Class Variables ########## + + self.connect_signals_and_slots() + + def connect_signals_and_slots(self): + pass \ No newline at end of file diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/__init__.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py index 53fb305..6a94bdc 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py @@ -9,6 +9,12 @@ 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 +from Resources.Settings import Mapping as MappingSettings ##################################### # Global Variables @@ -17,6 +23,8 @@ THREAD_HERTZ = 20 ROTATION_SPEED_MODIFIER = 2.5 +IMU_DATA_TOPIC = "/rover_odometry/imu/data" + ##################################### # Controller Class Definition @@ -58,15 +66,30 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.shown_heading = (self.current_heading + (1.5 * ROTATION_SPEED_MODIFIER)) % 360 self.current_heading_shown_rotation_angle = 0 - self.last_current_heading_shown = 0 + self.last_current_heading_shown = -1000 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 +100,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) + MappingSettings.DECLINATION_OFFSET) % 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.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 + self.current_heading_shown_rotation_angle = int(self.current_heading) - if should_update: - self.current_heading_shown_rotation_angle = int(self.shown_heading) + if self.current_heading_shown_rotation_angle != self.last_current_heading_shown: + 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: - 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 - - self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image)) - self.show_compass_image__signal.emit() + self.compass_pixmap = QtGui.QPixmap.fromImage(ImageQt(new_compass_image)) + self.show_compass_image__signal.emit() + self.heading_text_update_ready__signal.emit(str(self.current_heading_shown_rotation_angle) + "°") def update_heading_movement(self): current_minus_shown = (self.current_heading - self.shown_heading) % 360 @@ -126,6 +149,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) diff --git a/software/ros_packages/ground_station/src/Framework/NavigationSystems/WaypointsCoordinator.py b/software/ros_packages/ground_station/src/Framework/NavigationSystems/WaypointsCoordinator.py index 06f2fd9..0c1c459 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/WaypointsCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/WaypointsCoordinator.py @@ -2,6 +2,10 @@ from PyQt5 import QtCore, QtWidgets, QtGui import logging import rospy +from sensor_msgs.msg import NavSatFix + +GPS_POSITION_TOPIC = "/rover_odometry/fix" + class WaypointsCoordinator(QtCore.QThread): new_manual_waypoint_entry = QtCore.pyqtSignal(str, float, float, int) @@ -20,13 +24,10 @@ class WaypointsCoordinator(QtCore.QThread): navigation_waypoints_table_widget) self.landmark_label = self.left_screen.landmark_waypoints_table_widget - self.name_edit_label = (self.left_screen. - manual_waypoint_landmark_name_line_edit) - self.latitude_label = (self.left_screen. - manual_waypoint_decimal_lattitude_spin_box) - self.longitude_label = (self.left_screen. - manual_waypoint_decimal_longitude_spin_box) - + self.name_edit_label = self.left_screen.manual_waypoint_landmark_name_line_edit + self.latitude_label = self.left_screen.manual_waypoint_decimal_lattitude_spin_box + self.longitude_label = self.left_screen.manual_waypoint_decimal_longitude_spin_box + self.latitude_degree_label = self.left_screen.manual_waypoint_degrees_lattitude_spin_box self.longitude_degree_label = self.left_screen.manual_waypoint_degrees_longitude_spin_box @@ -43,11 +44,18 @@ class WaypointsCoordinator(QtCore.QThread): self.longitude_card_label = self.left_screen.manual_waypoint_cardinal_longitude_combo_box + # Color Labels and Buttons + self.nav_color_label = self.left_screen.manual_waypoint_navigation_color_label + self.nav_color_set_button = self.left_screen.manual_waypoint_navigation_color_set_button + + self.landmark_color_label = self.left_screen.manual_waypoint_landmark_color_label + self.landmark_color_set_button = self.left_screen.manual_waypoint_landmark_color_set_button + # Nav Table Buttons self.nav_set_button_label = (self.left_screen. navigation_waypoints_set_button) self.nav_add_manual_button_label = ( - self.left_screen.navigation_waypoints_add_manual_button) + self.left_screen.navigation_waypoints_add_manual_button) self.nav_add_gps_button_label = (self.left_screen. navigation_waypoints_add_gps_button) self.nav_delete_button_label = (self.left_screen. @@ -58,7 +66,7 @@ class WaypointsCoordinator(QtCore.QThread): landmark_waypoints_set_button) self.land_add_manual_button_label = ( - self.left_screen.landmark_waypoints_add_manual_button) + self.left_screen.landmark_waypoints_add_manual_button) self.land_add_gps_button_label = (self.left_screen. landmark_waypoints_add_gps_button) @@ -70,6 +78,23 @@ class WaypointsCoordinator(QtCore.QThread): self.logger = logging.getLogger("groundstation") + self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix, + self.gps_position_updated_callback) + + self.longitude = None + self.latitude = None + + self.nav_color = QtCore.Qt.yellow + self.landmark_color = QtCore.Qt.blue + + self.nav_color_dialog = QtWidgets.QColorDialog() + self.nav_color_dialog.setWindowFlags( + QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint) + + self.landmark_color_dialog = QtWidgets.QColorDialog() + self.landmark_color_dialog.setWindowFlags( + QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint) + def run(self): while self.run_thread_flag: self.msleep(3) @@ -93,13 +118,22 @@ class WaypointsCoordinator(QtCore.QThread): self.navigation_label.cellClicked.connect(self._on_nav_clicked) self.landmark_label.cellClicked.connect(self._on_land_clicked) - def _add_to_table(self, name, lat, lng, dist, table): + self.nav_color_set_button.clicked.connect(self.nav_color_dialog.show) + self.landmark_color_set_button.clicked.connect(self.landmark_color_dialog.show) + + self.nav_color_dialog.currentColorChanged.connect(self.__on_new_nav_color_selected) + self.landmark_color_dialog.currentColorChanged.connect(self.__on_new_landmark_color_selected) + + def _add_to_table(self, name, lat, lng, color, table): + color_table_item = QtWidgets.QTableWidgetItem() + color_table_item.setBackground(color) + count = table.rowCount() table.insertRow(count) table.setItem(count, 0, QtWidgets.QTableWidgetItem(name)) table.setItem(count, 1, QtWidgets.QTableWidgetItem(lat)) table.setItem(count, 2, QtWidgets.QTableWidgetItem(lng)) - table.setItem(count, 3, QtWidgets.QTableWidgetItem(dist)) + table.setItem(count, 3, color_table_item) def _clear_inputs(self): self.name_edit_label.clear() @@ -107,50 +141,54 @@ class WaypointsCoordinator(QtCore.QThread): self.longitude_label.clear() def _is_empty_inputs(self): - if self.name_edit_label.text().isEmpty(): + if not self.name_edit_label.text(): return True - if self.latitude_label.text().isEmpty(): + if not self.latitude_label.text(): return True - if self.longitude_label.text().isEmpty(): + if not self.longitude_label.text(): return True return False def _nav_add_gps(self): - # request GPS data - name = self.navigation_label.rowCount() - lat = 44.567200 - lng = -123.27860 - distance = 200 - self._add_to_table(str(name+1), str(lat), - str(lng), str(distance), - self.navigation_label) - self._clear_inputs() + if self.longitude and self.latitude: + name = self.navigation_label.rowCount() + self._add_to_table(str(name + 1), str(self.latitude), + str(self.longitude), self.nav_color, + self.navigation_label) + self._clear_inputs() def _nav_save(self): if not self._is_empty_inputs(): - lat = self.latitude_label.getText() - lng = self.longitude_label.getText() + lat = self.latitude_label.text() + lng = self.longitude_label.text() self.navigation_label.setItem( self.navigation_table_cur_click, 1, QtWidgets.QTableWidgetItem(lat)) self.navigation_label.setItem( - self.navigation_label, + self.navigation_table_cur_click, 2, QtWidgets.QTableWidgetItem(lng)) + + color_table_item = QtWidgets.QTableWidgetItem() + color_table_item.setBackground(self.nav_color) + + self.navigation_label.setItem( + self.navigation_table_cur_click, + 3, + color_table_item) self._clear_inputs() def _nav_add_manual(self): # request GPS data if not self._is_empty_inputs(): name = self.navigation_label.rowCount() - lat = self.latitude_label.getText() - lng = self.longitude_label.getText() - distance = 200 - self._add_to_table(str(name+1), lat, - lng, str(distance), + lat = self.latitude_label.text() + lng = self.longitude_label.text() + self._add_to_table(str(name + 1), lat, + lng, self.nav_color, self.navigation_label) - self._clear_inputs + self._clear_inputs() def _nav_del(self): if self.navigation_table_cur_click is not None: @@ -160,27 +198,22 @@ class WaypointsCoordinator(QtCore.QThread): self.navigation_label.setItem(x, 0, QtWidgets. - QTableWidgetItem(str(x+1))) + QTableWidgetItem(str(x + 1))) self._clear_inputs() def _land_add_gps(self): - name = self.name_edit_label.getText() - lat = 44.19223 - lng = -123.12394 - distance = 200 - self._add_to_table(name, str(lat), - str(lng), str(distance), - self.landmark_label) - self._clear_inputs() + if self.longitude and self.latitude: + name = self.name_edit_label.text() + self._add_to_table(name, str(self.latitude), str(self.longitude), self.landmark_color, self.landmark_label) + self._clear_inputs() def _land_add_manual(self): if not self._is_empty_inputs(): - name = self.name_edit_label.getText() - lat = self.latitude_label.getText() - lng = self.longitude_label.getText() - distance = 200 + name = self.name_edit_label.text() + lat = self.latitude_label.text() + lng = self.longitude_label.text() self._add_to_table(name, lat, - lng, str(distance), + lng, self.landmark_color, self.landmark_label) self._clear_inputs() @@ -192,14 +225,14 @@ class WaypointsCoordinator(QtCore.QThread): self.navigation_label.setItem(x, 0, QtWidgets. - QTableWidgetItem(str(x+1))) + QTableWidgetItem(str(x + 1))) self._clear_inputs() def _land_save(self): if not self._is_empty_inputs(): - name = self.name_edit_label.getText() - lat = self.latitude_label.getText() - lng = self.longitude_label.getText() + name = self.name_edit_label.text() + lat = self.latitude_label.text() + lng = self.longitude_label.text() self.landmark_label.setItem(self.landmark_table_cur_click, 0, QtWidgets.QTableWidgetItem(name)) @@ -209,6 +242,12 @@ class WaypointsCoordinator(QtCore.QThread): self.landmark_label.setItem(self.landmark_table_cur_click, 2, QtWidgets.QTableWidgetItem(lng)) + color_table_item = QtWidgets.QTableWidgetItem() + color_table_item.setBackground(self.landmark_color) + + self.landmark_label.setItem(self.landmark_table_cur_click, 3, + color_table_item) + self._clear_inputs() def setup_signals(self, start_signal, @@ -228,7 +267,7 @@ class WaypointsCoordinator(QtCore.QThread): lat_d = float(abs(int(lat))) lat_m = float(int((abs(lat) - lat_d) * 60)) - lat_s = ((abs(lat) - lat_d - (lat_m/60.0)) * 3600.) + lat_s = ((abs(lat) - lat_d - (lat_m / 60.0)) * 3600.) if lat > 0.: self.latitude_card_label.setCurrentText("N") else: @@ -239,7 +278,7 @@ class WaypointsCoordinator(QtCore.QThread): lng_d = float(abs(int(lng))) lng_m = float(int((abs(lng) - lng_d) * 60)) - lng_s = ((abs(lng) - lng_d - (lng_m/60.0)) * 3600.) + lng_s = ((abs(lng) - lng_d - (lng_m / 60.0)) * 3600.) if lng > 0.: self.longitude_card_label.setCurrentText("E") else: @@ -267,3 +306,19 @@ class WaypointsCoordinator(QtCore.QThread): float(self.landmark_label.item(row, 2).text()), 1 ) + + def __on_new_nav_color_selected(self, color): + self.nav_color_label.setStyleSheet( + "background-color: rgb(%s, %s, %s)" % (color.red(), color.green(), color.blue())) + + self.nav_color = color + + def __on_new_landmark_color_selected(self, color): + self.landmark_color_label.setStyleSheet( + "background-color: rgb(%s, %s, %s)" % (color.red(), color.green(), color.blue())) + + self.landmark_color = color + + def gps_position_updated_callback(self, data): + self.latitude = data.latitude + self.longitude = data.longitude diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py index d4af21b..e572948 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -7,13 +7,14 @@ from PyQt5 import QtWidgets, QtCore, QtGui, uic from std_msgs.msg import Empty import PIL.Image from PIL.ImageQt import ImageQt +import time from std_msgs.msg import UInt16 + # import Timer REQUEST_UPDATE_TOPIC = "/rover_status/update_requested" - CAMERA_TOPIC_NAME = "/rover_status/camera_status" BOGIE_TOPIC_NAME = "/rover_status/bogie_status" FRSKY_TOPIC_NAME = "/rover_status/frsky_status" @@ -25,8 +26,14 @@ CO2_TOPIC_NAME = "/rover_control/tower/status/co2" COLOR_GREEN = "background-color: darkgreen; border: 1px solid black;" COLOR_ORANGE = "background-color: orange; border: 1px solid black;" +COLOR_YELLOW = "background-color: rgb(204,204,0); border: 1px solid black; color: black;" COLOR_RED = "background-color: darkred; border: 1px solid black;" +GPS_BEST_CASE_ACCURACY = 3 + +LOW_BATTERY_DIALOG_TIMEOUT = 120 +CRITICAL_BATTERY_DIALOG_TIMEOUT = 30 + class SensorCore(QtCore.QThread): # ########## create signals for slots ########## @@ -62,6 +69,9 @@ class SensorCore(QtCore.QThread): battery_voltage_update_ready__signal = QtCore.pyqtSignal(str) battery_voltage_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + show_battery_low__signal = QtCore.pyqtSignal() + show_battery_critical__signal = QtCore.pyqtSignal() + def __init__(self, shared_objects): super(SensorCore, self).__init__() @@ -106,7 +116,7 @@ class SensorCore(QtCore.QThread): self.co2_status = rospy.Subscriber(CO2_TOPIC_NAME, UInt16, self.__co2_callback) self.camera_msg = CameraStatuses() - self.bogie_msg = None # BogieStatuses() + self.bogie_msg = None # BogieStatuses() self.FrSky_msg = FrSkyStatus() self.GPS_msg = GPSInfo() self.jetson_msg = JetsonInfo() @@ -120,6 +130,28 @@ class SensorCore(QtCore.QThread): self.osurc_logo_pixmap = QtGui.QPixmap.fromImage(ImageQt(self.osurc_logo_pil)) self.osurc_logo_label.setPixmap(self.osurc_logo_pixmap) # Init should be in main thread, should be fine + self.low_battery_warning_dialog = QtWidgets.QMessageBox() + self.low_battery_warning_dialog.setIcon(QtWidgets.QMessageBox.Warning) + self.low_battery_warning_dialog.setText("\n\n\n\nRover battery low!\nReturn and charge soon to avoid battery damage!\n\n\n\n") + self.low_battery_warning_dialog.setWindowTitle("Low Battery") + self.low_battery_warning_dialog.setStandardButtons(QtWidgets.QMessageBox.Ok) + self.low_battery_warning_dialog.setWindowFlags( + QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint) + self.low_battery_warning_dialog.setStyleSheet(COLOR_YELLOW) + + self.critical_battery_warning_dialog = QtWidgets.QMessageBox() + self.critical_battery_warning_dialog.setIcon(QtWidgets.QMessageBox.Critical) + self.critical_battery_warning_dialog.setText( + "\n\n\n\nRover battery critical!\nPower down immediately or battery damage will occur!\n\n\n\n") + self.critical_battery_warning_dialog.setWindowTitle("Critical Battery") + self.critical_battery_warning_dialog.setStandardButtons(QtWidgets.QMessageBox.Ok) + self.critical_battery_warning_dialog.setWindowFlags( + QtCore.Qt.WindowStaysOnTopHint | QtCore.Qt.X11BypassWindowManagerHint | QtCore.Qt.WindowTitleHint | QtCore.Qt.WindowCloseButtonHint) + self.critical_battery_warning_dialog.setStyleSheet(COLOR_RED) + + self.low_battery_warning_last_shown = 0 + self.critical_battery_warning_last_shown = 0 + def __camera_callback(self, data): self.camera_msg.camera_zed = data.camera_zed self.camera_msg.camera_undercarriage = data.camera_undercarriage @@ -215,7 +247,8 @@ class SensorCore(QtCore.QThread): self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nFalse") self.gps_num_satellites_update_ready__signal.emit("GPS Satellites\n%s" % data.num_satellites) - self.gps_accuracy_update_ready__signal.emit("GPS Accuracy\n%2.2f m" % data.horizontal_dilution) + self.gps_accuracy_update_ready__signal.emit( + "GPS Accuracy\n%2.2f m" % (data.horizontal_dilution * GPS_BEST_CASE_ACCURACY)) def __misc_callback(self, data): self.misc_msg.arm_connection_status = data.arm_connection_status @@ -227,11 +260,22 @@ class SensorCore(QtCore.QThread): def __battery_callback(self, data): voltage = data.battery_voltage / 1000.0 - if voltage >= 20: + if voltage >= 21: self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_GREEN) + elif voltage >= 19: + self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_YELLOW) + + if (time.time() - self.low_battery_warning_last_shown) > LOW_BATTERY_DIALOG_TIMEOUT: + self.show_battery_low__signal.emit() + self.low_battery_warning_last_shown = time.time() + else: self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_RED) + if (time.time() - self.critical_battery_warning_last_shown) > CRITICAL_BATTERY_DIALOG_TIMEOUT: + self.show_battery_critical__signal.emit() + self.critical_battery_warning_last_shown = time.time() + self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + " V") def __co2_callback(self, data): @@ -247,7 +291,7 @@ class SensorCore(QtCore.QThread): def run(self): while self.run_thread_flag: - self.update_requester.publish(Empty()) + # self.update_requester.publish(Empty()) self.__display_time() self.msleep(1000) @@ -277,6 +321,9 @@ class SensorCore(QtCore.QThread): self.battery_voltage_update_ready__signal.connect(self.battery.setText) self.battery_voltage_stylesheet_change_ready__signal.connect(self.battery.setStyleSheet) + self.show_battery_low__signal.connect(self.low_battery_warning_dialog.show) + self.show_battery_critical__signal.connect(self.critical_battery_warning_dialog.show) + def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): start_signal.connect(self.start) signals_and_slots_signal.connect(self.connect_signals_and_slots) diff --git a/software/ros_packages/ground_station/src/Resources/Images/maps_loading.png b/software/ros_packages/ground_station/src/Resources/Images/maps_loading.png new file mode 100644 index 0000000..3e8ae52 Binary files /dev/null and b/software/ros_packages/ground_station/src/Resources/Images/maps_loading.png differ diff --git a/software/ros_packages/ground_station/src/Resources/Images/rover.png b/software/ros_packages/ground_station/src/Resources/Images/rover.png new file mode 100644 index 0000000..72121ed Binary files /dev/null and b/software/ros_packages/ground_station/src/Resources/Images/rover.png differ diff --git a/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py b/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py new file mode 100644 index 0000000..6fe8115 --- /dev/null +++ b/software/ros_packages/ground_station/src/Resources/Settings/Mapping.py @@ -0,0 +1,23 @@ +# Note that the lat and lon positions below correspond to the center point of the maps you want to download +# Proper zoom level selection determines total viewable area + +MAPPING_LOCATIONS = { + "Graf Hall": { + "latitude": 44.5675721667, + "longitude": -123.2750535, + "default_zoom": 18, + "valid_zoom_options": [15, 16, 17, 18, 19, 20], + "pre_cache_map_zoom_levels": [18, 19, 20] + }, + + "Crystal Lake": { + "latitude": 44.547155, + "longitude": -123.251438, + "default_zoom": 18, + "valid_zoom_options": [15, 16, 17, 18, 19, 20], + "pre_cache_map_zoom_levels": [18, 19, 20] + } +} + +# ##### This is the offset from magnetic north to true north +DECLINATION_OFFSET = 15 diff --git a/software/ros_packages/ground_station/src/Resources/Settings/__init__.py b/software/ros_packages/ground_station/src/Resources/Settings/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 7fb73dc..6360c49 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -1392,224 +1392,13 @@ N/A 0 - + - Science Readouts + SSH Console - - - - - - - - 12 - 75 - true - - - - Soil Probe - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - Soil Temperature: - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - N/A - - - - - - - Soil PH: - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - N/A - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Read Soil Probe - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - - Qt::Vertical - - - - - - - - - - 12 - 75 - true - - - - Dust Sensor - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - - Small Dust Present: - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - Large Dust Present: - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - False - - - - - - - False - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - + + + @@ -2108,6 +1897,125 @@ N/A + + + + 6 + + + + + + 75 + true + + + + Color + + + + + + + Qt::Vertical + + + + + + + Navigation + + + + + + + + 25 + 25 + + + + + 25 + 25 + + + + background-color:yellow; + + + + + + + + + + Set + + + + + + + Qt::Vertical + + + + + + + Landmark + + + + + + + + 25 + 25 + + + + + 25 + 25 + + + + background-color:blue; + + + + + + + + + + Set + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + @@ -2279,22 +2187,26 @@ N/A + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 9999999 + + - QTableView{ - selection-background-color: #DE8D47; -} - -QTableView QHeaderView{ - background-color: #201F1D; -} - -QTableView QHeaderView::section{ - background-color: #201F1D; -} - -QTableView QTableCornerButton::section{ - background-color: #201F1D; -} + Qt::ScrollBarAlwaysOff @@ -2332,16 +2244,6 @@ QTableView QTableCornerButton::section{ 30 - - - 1 - - - - - 2 - - # @@ -2359,49 +2261,9 @@ QTableView QTableCornerButton::section{ - Dist + Color - - - 1 - - - - - 32.1665 - - - - - -112.1115 - - - - - 2.4 KM - - - - - 2 - - - - - 33.1124 - - - - - -111.4334 - - - - - 5.7 KM - - @@ -2409,6 +2271,9 @@ QTableView QTableCornerButton::section{ 2 + + 4 + @@ -2423,11 +2288,13 @@ QTableView QTableCornerButton::section{ 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Set @@ -2448,11 +2315,13 @@ QTableView QTableCornerButton::section{ 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Add @@ -2474,11 +2343,13 @@ Manual 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Add @@ -2500,11 +2371,13 @@ GPS 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Delete @@ -2575,22 +2448,26 @@ GPS + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 9999999 + + - QTableView{ - selection-background-color: #DE8D47; -} - -QTableView QHeaderView{ - background-color: #201F1D; -} - -QTableView QHeaderView::section{ - background-color: #201F1D; -} - -QTableView QTableCornerButton::section{ - background-color: #201F1D; -} + Qt::ScrollBarAlwaysOff @@ -2631,16 +2508,6 @@ QTableView QTableCornerButton::section{ 30 - - - 1 - - - - - 2 - - Name @@ -2658,49 +2525,9 @@ QTableView QTableCornerButton::section{ - Dist + Color - - - Astronaut - - - - - 32.1665 - - - - - -112.1115 - - - - - 2.4 KM - - - - - Water - - - - - 33.1124 - - - - - -111.4334 - - - - - 5.7 KM - - @@ -2708,6 +2535,9 @@ QTableView QTableCornerButton::section{ 2 + + 4 + @@ -2722,11 +2552,13 @@ QTableView QTableCornerButton::section{ 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Set @@ -2747,11 +2579,13 @@ QTableView QTableCornerButton::section{ 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Add @@ -2773,11 +2607,13 @@ Manual 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Add @@ -2799,11 +2635,13 @@ GPS 35 + + + 8 + + - QPushButton{ - color: #201F1D; - background-color: #868685; -} + Delete diff --git a/software/ros_packages/ground_station/src/Resources/__init__.py b/software/ros_packages/ground_station/src/Resources/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/rover_camera/src/rover_camera.cpp b/software/ros_packages/rover_camera/src/rover_camera.cpp index 625c4c4..baca27e 100644 --- a/software/ros_packages/rover_camera/src/rover_camera.cpp +++ b/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; - bool medium_ready_to_broadcast = false; - bool small_ready_to_broadcast = false; + if(upside_down){ + cv::flip(image_large, image_large, -1); + } if(broadcast_large_image){ large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg(); - large_ready_to_broadcast = true; - } - - if(broadcast_medium_image){ + large_image_publisher.publish(large_image_message); + }else 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; - } - - if(broadcast_small_image){ + medium_image_publisher.publish(medium_image_message); + }else 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; diff --git a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index c0e92f0..15df4cb 100755 --- a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py +++ b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py @@ -83,7 +83,7 @@ class DriveCoordinator(object): try: self.process_drive_commands() except Exception, error: - print "Error occurred:", error + print "COORDINATOR: Error occurred:", error time_diff = time() - start_time diff --git a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py index 3266206..5d0bfe7 100755 --- a/software/ros_packages/rover_control/src/iris_controller/iris_controller.py +++ b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py @@ -118,11 +118,9 @@ 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 + print "IRIS: Error occurred:", error if (time() - self.iris_last_seen_time) > IRIS_LAST_SEEN_TIMEOUT: print "Iris not seen for", IRIS_LAST_SEEN_TIMEOUT, "seconds. Exiting." @@ -135,6 +133,7 @@ class IrisController(object): def read_registers(self): try: self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS)) + self.iris_last_seen_time = time() except Exception, error: self.iris_connected = False diff --git a/software/ros_packages/rover_main/launch/rover/cameras.launch b/software/ros_packages/rover_main/launch/rover/cameras.launch index a5f99ad..52aae29 100644 --- a/software/ros_packages/rover_main/launch/rover/cameras.launch +++ b/software/ros_packages/rover_main/launch/rover/cameras.launch @@ -3,6 +3,7 @@ + @@ -17,7 +18,7 @@ - + diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index 1a4ca14..74761a2 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -9,10 +9,8 @@ - - - - + + diff --git a/software/ros_packages/rover_main/launch/rover/odometry2.launch b/software/ros_packages/rover_main/launch/rover/odometry2.launch index 024e8f2..dbe51b3 100644 --- a/software/ros_packages/rover_main/launch/rover/odometry2.launch +++ b/software/ros_packages/rover_main/launch/rover/odometry2.launch @@ -32,7 +32,7 @@ - + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 511bd96..bba4c13 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -153,7 +153,8 @@ [ {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}, ] diff --git a/software/ros_packages/rover_odometry/msg/odom.msg b/software/ros_packages/rover_odometry/msg/odom.msg new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/rover_odometry/src/odometry.py b/software/ros_packages/rover_odometry/src/odometry.py index 4b35573..bb3efbf 100755 --- a/software/ros_packages/rover_odometry/src/odometry.py +++ b/software/ros_packages/rover_odometry/src/odometry.py @@ -28,6 +28,8 @@ DEFAULT_IMU_TOPIC = "imu/data" DEFAULT_HERTZ = 100 +ODOM_LAST_SEEN_TIMEOUT = 1 # seconds + ##################################### # DriveControl Class Definition @@ -50,6 +52,8 @@ class Odometry(object): 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.odom_last_seen_time = time() + self.run() def run(self): @@ -58,9 +62,14 @@ class Odometry(object): try: self.process_messages() + self.odom_last_seen_time = time() except Exception, error: - print "Error occurred:", error + pass + + if (time() - self.odom_last_seen_time) > ODOM_LAST_SEEN_TIMEOUT: + print "Odometry not seen for", ODOM_LAST_SEEN_TIMEOUT, "seconds. Exiting." + return # Exit so respawn can take over time_diff = time() - start_time