caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Started working on mapping waypoints and thinking about zoom/pan. Added watchdog for odometry. Fixed watchdog for IRIS. Changed voltage warning for rover, was too low. Added neat loading image for mapping on launch.

Browse Source
This commit is contained in:
Corwin Perren
2018-07-21 20:45:59 -07:00
parent 8c78d5729f
commit 71cff19b77
10 changed files with 111 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py
View File

@@ -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()

22
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
View File

@@ -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):

100
software/ros_packages/ground_station/src/Framework/NavigationSystems/WaypointsCoordinator.py
View File

@@ -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

6
software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py
View File

@@ -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)

20
software/ros_packages/ground_station/src/Resources/Settings/MappingLocations.py Normal file
View File

@@ -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]
}
}

0
software/ros_packages/ground_station/src/Resources/Settings/__init__.py Normal file
View File

16
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -2153,12 +2153,12 @@ QTableView QTableCornerButton::section{
</item>
<item row="0" column="1">
<property name="text">
<string>32.1665</string>
<string>44.5674</string>
</property>
</item>
<item row="0" column="2">
<property name="text">
<string>-112.1115</string>
<string>-123.2756</string>
</property>
</item>
<item row="0" column="3">
@@ -2173,12 +2173,12 @@ QTableView QTableCornerButton::section{
</item>
<item row="1" column="1">
<property name="text">
<string>33.1124</string>
<string>44.5677</string>
</property>
</item>
<item row="1" column="2">
<property name="text">
<string>-111.4334</string>
<string>-123.2748</string>
</property>
</item>
<item row="1" column="3">
@@ -2452,12 +2452,12 @@ QTableView QTableCornerButton::section{
</item>
<item row="0" column="1">
<property name="text">
<string>32.1665</string>
<string>44.56745</string>
</property>
</item>
<item row="0" column="2">
<property name="text">
<string>-112.1115</string>
<string>-123.2756</string>
</property>
</item>
<item row="0" column="3">
@@ -2472,12 +2472,12 @@ QTableView QTableCornerButton::section{
</item>
<item row="1" column="1">
<property name="text">
<string>33.1124</string>
<string>44.56775</string>
</property>
</item>
<item row="1" column="2">
<property name="text">
<string>-111.4334</string>
<string>-123.2748</string>
</property>
</item>
<item row="1" column="3">

0
software/ros_packages/ground_station/src/Resources/__init__.py Normal file
View File

3
software/ros_packages/rover_control/src/iris_controller/iris_controller.py
View File

@@ -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

11
software/ros_packages/rover_odometry/src/odometry.py
View File

@@ -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