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 2321b34..89f9736 100644
--- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py
+++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py
@@ -392,7 +392,7 @@ class OverlayImage(object):
self.display_image_copy = self.display_image.copy()
- self.generate_dot_and_hat()
+ self.load_rover_icon()
self.indicator.save("location.png")
def _get_cartesian(self, lat, lon):
@@ -431,29 +431,26 @@ class OverlayImage(object):
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]))
+ x, y = self._get_cartesian(float(element[1]), float(element[2]))
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")
+
+ for element in landmark_list:
+ x, y = self._get_cartesian(element[1], element[2])
+ draw.ellipse((x - size, y - size, x + size, y + size), fill="blue")
+
self._draw_rover(latitude, longitude, compass)
self._draw_coordinate_text(latitude, longitude)
self.update(latitude, longitude)
return self.display_image
- def generate_dot_and_hat(self):
+ def load_rover_icon(self):
self.indicator = PIL.Image.open("Resources/Images/rover.png").resize((50, 50))
- # 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 _draw_coordinate_text(self, latitude, longitude):
location_text = "LAT: %+014.9f\nLON: %+014.9f" % (latitude, longitude)
- # location_text = "LAT: " + str(latitude) + "\nLON: " + str(longitude)
font = PIL.ImageFont.truetype("UbuntuMono-R", size=20)
@@ -468,7 +465,7 @@ class OverlayImage(object):
def _draw_rover(self, lat, lon, angle=0):
x, y = self._get_cartesian(lat, lon)
- x -= 25
+ x -= 25 # Half the height of the icon
y -= 25
rotated = self.indicator.copy()
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 5a86027..dc1ebd4 100644
--- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
+++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
@@ -71,21 +71,12 @@ class RoverMapCoordinator(QtCore.QThread):
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,
- 18, # FIXME: Used to be 18
+ 19, # FIXME: Used to be 18
'satellite',
None, 20)
self.overlay_image_object = (
@@ -102,11 +93,7 @@ class RoverMapCoordinator(QtCore.QThread):
if self.map_image:
self.map_image_copy = self.map_image.copy()
- # self.overlay_image_object.update_new_location(44.567161,
- # -123.278432,
- # .7,
- # [],
- # [])
+
self.update_overlay()
self.map_image = self.map_image_copy.copy()
@@ -158,15 +145,14 @@ class RoverMapCoordinator(QtCore.QThread):
longitude = float(self.longitude)
navigation_list = self._get_table_elements(self.navigation_label)
- # landmark_list = self._get_table_elements(self.landmark_label)
- landmark_list = []
+ 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.last_heading = (self.last_heading + 5) % 360
# self.overlay_image.save("something.png")
def gps_position_updated_callback(self, data):
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..19b250c 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
@@ -47,7 +48,7 @@ class WaypointsCoordinator(QtCore.QThread):
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 +59,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 +71,12 @@ 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
+
def run(self):
while self.run_thread_flag:
self.msleep(3)
@@ -107,35 +114,33 @@ 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()
+ distance = 0 # FIXME: this should be calculated from current to enterred position
+ self._add_to_table(str(name + 1), str(self.latitude),
+ str(self.longitude), str(distance),
+ 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))
self._clear_inputs()
@@ -144,13 +149,13 @@ class WaypointsCoordinator(QtCore.QThread):
# request GPS data
if not self._is_empty_inputs():
name = self.navigation_label.rowCount()
- lat = self.latitude_label.getText()
- lng = self.longitude_label.getText()
+ lat = self.latitude_label.text()
+ lng = self.longitude_label.text()
distance = 200
- self._add_to_table(str(name+1), lat,
+ self._add_to_table(str(name + 1), lat,
lng, str(distance),
self.navigation_label)
- self._clear_inputs
+ self._clear_inputs()
def _nav_del(self):
if self.navigation_table_cur_click is not None:
@@ -160,24 +165,23 @@ 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()
+ distance = 200 # FIXME: this should be calculated from current to enterred position
+ self._add_to_table(name, str(self.latitude),
+ str(self.longitude), str(distance),
+ 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()
+ name = self.name_edit_label.text()
+ lat = self.latitude_label.text()
+ lng = self.longitude_label.text()
distance = 200
self._add_to_table(name, lat,
lng, str(distance),
@@ -192,14 +196,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))
@@ -228,7 +232,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 +243,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 +271,7 @@ class WaypointsCoordinator(QtCore.QThread):
float(self.landmark_label.item(row, 2).text()),
1
)
+
+ 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..8f2a4a1 100644
--- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
+++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
@@ -27,6 +27,8 @@ COLOR_GREEN = "background-color: darkgreen; border: 1px solid black;"
COLOR_ORANGE = "background-color: orange; border: 1px solid black;"
COLOR_RED = "background-color: darkred; border: 1px solid black;"
+GPS_BEST_CASE_ACCURACY = 3
+
class SensorCore(QtCore.QThread):
# ########## create signals for slots ##########
@@ -215,7 +217,7 @@ 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,7 +229,7 @@ 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)
else:
self.battery_voltage_stylesheet_change_ready__signal.emit(COLOR_RED)
diff --git a/software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py b/software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py
new file mode 100644
index 0000000..8d52243
--- /dev/null
+++ b/software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py
@@ -0,0 +1,20 @@
+# 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]
+ }
+}
\ No newline at end of file
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 e55878d..2211f53 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
@@ -2153,12 +2153,12 @@ QTableView QTableCornerButton::section{
-
- 32.1665
+ 44.5674
-
- -112.1115
+ -123.2756
-
@@ -2173,12 +2173,12 @@ QTableView QTableCornerButton::section{
-
- 33.1124
+ 44.5677
-
- -111.4334
+ -123.2748
-
@@ -2452,12 +2452,12 @@ QTableView QTableCornerButton::section{
-
- 32.1665
+ 44.56745
-
- -112.1115
+ -123.2756
-
@@ -2472,12 +2472,12 @@ QTableView QTableCornerButton::section{
-
- 33.1124
+ 44.56775
-
- -111.4334
+ -123.2748
-
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_control/src/iris_controller/iris_controller.py b/software/ros_packages/rover_control/src/iris_controller/iris_controller.py
index 1b0849c..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 "IRIS: 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."
@@ -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_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