diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py index c8ec883..fe4ae43 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py @@ -18,7 +18,7 @@ GAME_CONTROLLER_NAME = "Logitech Logitech Extreme 3D Pro" DEFAULT_DRIVE_COMMAND_TOPIC = "/rover_control/command_control/ground_station_drive" DEFAULT_PAN_TILT_COMMAND_TOPIC = "/rover_control/pan_tilt/control" -DRIVE_COMMAND_HERTZ = 15 +DRIVE_COMMAND_HERTZ = 20 Y_AXIS_DEADBAND = 0.05 X_AXIS_DEADBAND = 0.05 @@ -32,7 +32,7 @@ GUI_ELEMENT_CHANGE_TIME = 0.2 CAMERA_TOGGLE_CHANGE_TIME = 0.35 PAN_TILT_X_AXIS_SCALAR = 3 -PAN_TILT_Y_AXIS_SCALAR = 10 +PAN_TILT_Y_AXIS_SCALAR = 20 ##################################### @@ -169,6 +169,9 @@ class JoystickControlSender(QtCore.QThread): self.drive_command_publisher = rospy.Publisher(DEFAULT_DRIVE_COMMAND_TOPIC, DriveCommandMessage, queue_size=1) self.tower_pan_tilt_command_publisher = rospy.Publisher(DEFAULT_PAN_TILT_COMMAND_TOPIC, TowerPanTiltControlMessage, queue_size=1) + self.last_hat_x_was_movement = False + self.last_hat_y_was_movement = False + self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ self.drive_paused = True 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 9ba54dc..7c3573c 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -12,12 +12,15 @@ import rospy # Custom Imports import RoverMap +from sensor_msgs.msg import NavSatFix ##################################### # Global Variables ##################################### # put some stuff here later so you can remember +GPS_POSITION_TOPIC = "/rover_odometry/fix" + class RoverMapCoordinator(QtCore.QThread): pixmap_ready_signal = QtCore.pyqtSignal() @@ -36,6 +39,8 @@ class RoverMapCoordinator(QtCore.QThread): self.logger = logging.getLogger("groundstation") + self.gps_position_subscriber = rospy.Subscriber(GPS_POSITION_TOPIC, NavSatFix, self.gps_position_updated_callback) + self.run_thread_flag = True self.setup_map_flag = True @@ -47,6 +52,9 @@ class RoverMapCoordinator(QtCore.QThread): self.map_pixmap = None self.last_map_pixmap_cache_key = None + self.longitude = None + self.latitude = None + def run(self): self.logger.debug("Starting Map Coordinator Thread") @@ -72,13 +80,13 @@ class RoverMapCoordinator(QtCore.QThread): def _map_setup(self): self.google_maps_object = RoverMap.GMapsStitcher(1280, 720, - 44.567161, - -123.278432, - 18, + 44.5675721667, + -123.2750535, + 20, # FIXME: Used to be 18 'satellite', None, 20) self.overlay_image_object = ( - RoverMap.OverlayImage(44.567161, -123.278432, + RoverMap.OverlayImage(44.5675721667, -123.2750535, self.google_maps_object.northwest, self.google_maps_object.southeast, self.google_maps_object.big_image.size[0], @@ -136,15 +144,22 @@ class RoverMapCoordinator(QtCore.QThread): return temp_list def update_overlay(self): - longitude = 44.567161 - latitude = -123.278432 - 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") + if self.latitude and self.longitude: + longitude = self.latitude + latitude = self.longitude + + print self.longitude, " : ", self.latitude + 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") + + 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 1270442..d4af21b 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -7,6 +7,8 @@ from PyQt5 import QtWidgets, QtCore, QtGui, uic from std_msgs.msg import Empty import PIL.Image from PIL.ImageQt import ImageQt + +from std_msgs.msg import UInt16 # import Timer REQUEST_UPDATE_TOPIC = "/rover_status/update_requested" @@ -19,6 +21,7 @@ GPS_TOPIC_NAME = "/rover_status/gps_status" JETSON_TOPIC_NAME = "/rover_status/jetson_status" MISC_TOPIC_NAME = "/rover_status/misc_status" BATTERY_TOPIC_NAME = "/rover_status/battery_status" +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;" @@ -40,6 +43,13 @@ class SensorCore(QtCore.QThread): bogie_connection_2_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) bogie_connection_3_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + gps_fix_update_ready__signal = QtCore.pyqtSignal(str) + gps_heading_valid_update_ready__signal = QtCore.pyqtSignal(str) + gps_num_satellites_update_ready__signal = QtCore.pyqtSignal(str) + gps_accuracy_update_ready__signal = QtCore.pyqtSignal(str) + + co2_levels_update_ready__signal = QtCore.pyqtSignal(str) + camera_zed_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) camera_under_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) camera_chassis_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) @@ -70,7 +80,10 @@ class SensorCore(QtCore.QThread): self.frsky = self.screen_main_window.frsky # type: QtWidgets.QLabel self.nav_mouse = self.screen_main_window.nav_mouse # type: QtWidgets.QLabel self.joystick = self.screen_main_window.joystick # type: QtWidgets.QLabel - self.gps = self.screen_main_window.gps # type: QtWidgets.QLabel + self.gps_fix_label = self.screen_main_window.gps_fix_label # type: QtWidgets.QLabel + self.gps_heading_valid_label = self.screen_main_window.gps_heading_valid_label # type: QtWidgets.QLabel + self.gps_num_satellites_label = self.screen_main_window.gps_num_satellites_label # type: QtWidgets.QLabel + self.gps_accuracy_label = self.screen_main_window.gps_accuracy_label # type: QtWidgets.QLabel self.zed = self.screen_main_window.zed # type: QtWidgets.QLabel self.main_cam = self.screen_main_window.main_cam # type: QtWidgets.QLabel self.chassis_cam = self.screen_main_window.chassis_cam # type: QtWidgets.QLabel @@ -81,6 +94,7 @@ class SensorCore(QtCore.QThread): self.gpu_temp = self.screen_main_window.gpu_temp # type: QtWidgets.QLabel self.emmc = self.screen_main_window.emmc # type: QtWidgets.QLabel self.battery = self.screen_main_window.battery_voltage_status_label # type: QtWidgets.QLabel + self.co2_levels_label = self.screen_main_window.co2_levels_label # type: QtWidgets.QLabel # ########## subscriptions pulling data from system_statuses_node.py ########## self.camera_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback) @@ -89,6 +103,7 @@ class SensorCore(QtCore.QThread): self.jetson_status = rospy.Subscriber(JETSON_TOPIC_NAME, JetsonInfo, self.__jetson_callback) self.misc_status = rospy.Subscriber(MISC_TOPIC_NAME, MiscStatuses, self.__misc_callback) self.battery_status = rospy.Subscriber(BATTERY_TOPIC_NAME, BatteryStatusMessage, self.__battery_callback) + self.co2_status = rospy.Subscriber(CO2_TOPIC_NAME, UInt16, self.__co2_callback) self.camera_msg = CameraStatuses() self.bogie_msg = None # BogieStatuses() @@ -185,13 +200,23 @@ class SensorCore(QtCore.QThread): self.jetson_emmc_stylesheet_change_ready__signal.emit(COLOR_GREEN) def __gps_callback(self, data): - self.GPS_msg.UTC_GPS_time = data.UTC_GPS_time - if not data.GPS_connection_status: - self.gps_stylesheet_change_ready__signal.emit(COLOR_RED) - else: + if data.gps_connected: + self.gps_fix_update_ready__signal.emit("GPS Fix\nTrue") self.gps_stylesheet_change_ready__signal.emit(COLOR_GREEN) + else: + self.gps_stylesheet_change_ready__signal.emit(COLOR_RED) + self.gps_fix_update_ready__signal.emit("GPS Fix\nFalse") + + if data.gps_heading != -1: + self.gps_heading_valid_update_ready__signal.emit("GPS Heading Valid\nTrue") + else: + 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) + def __misc_callback(self, data): self.misc_msg.arm_connection_status = data.arm_connection_status self.misc_msg.arm_end_effector_connection_statuses = data.arm_end_effector_connection_statuses @@ -209,6 +234,12 @@ class SensorCore(QtCore.QThread): self.battery_voltage_update_ready__signal.emit("Battery Voltage\n" + str(voltage) + " V") + def __co2_callback(self, data): + if data.data != 9999: + self.co2_levels_update_ready__signal.emit("CO2 Levels\n%d ppm" % data.data) + else: + self.co2_levels_update_ready__signal.emit("CO2 Levels\n--- ppm") + def __display_time(self): time = QtCore.QTime.currentTime() temp = time.toString('hh:mm') @@ -233,9 +264,16 @@ class SensorCore(QtCore.QThread): self.camera_under_stylesheet_change_ready__signal.connect(self.under_cam.setStyleSheet) self.camera_chassis_stylesheet_change_ready__signal.connect(self.chassis_cam.setStyleSheet) self.camera_main_stylesheet_change_ready__signal.connect(self.main_cam.setStyleSheet) - self.gps_stylesheet_change_ready__signal.connect(self.gps.setStyleSheet) + self.gps_stylesheet_change_ready__signal.connect(self.gps_fix_label.setStyleSheet) self.frsky_stylesheet_change_ready__signal.connect(self.frsky.setStyleSheet) + self.gps_fix_update_ready__signal.connect(self.gps_fix_label.setText) + self.gps_heading_valid_update_ready__signal.connect(self.gps_heading_valid_label.setText) + self.gps_num_satellites_update_ready__signal.connect(self.gps_num_satellites_label.setText) + self.gps_accuracy_update_ready__signal.connect(self.gps_accuracy_label.setText) + + self.co2_levels_update_ready__signal.connect(self.co2_levels_label.setText) + self.battery_voltage_update_ready__signal.connect(self.battery.setText) self.battery_voltage_stylesheet_change_ready__signal.connect(self.battery.setStyleSheet) 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 32a95bb..7fb73dc 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 @@ -251,7 +251,7 @@ 0 - + @@ -275,7 +275,7 @@ - + @@ -320,7 +320,7 @@ Connected - + @@ -452,7 +452,7 @@ Connected - + @@ -494,7 +494,7 @@ Connected - + @@ -578,49 +578,7 @@ Connected - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 9999999 - 9999999 - - - - - 10 - 75 - true - - - - false - - - background-color:darkgreen; border: 0.5px solid black; - - - GPS -Fix - - - Qt::AlignCenter - - - - + @@ -662,7 +620,7 @@ Connected - + @@ -812,7 +770,7 @@ Disconnected - + @@ -878,49 +836,7 @@ Connected - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 9999999 - 9999999 - - - - - 10 - 75 - true - - - - background-color:darkgreen; border: 0.5px solid black; - - - QFrame::NoFrame - - - Battery Voltage -N/A - - - Qt::AlignCenter - - - - + @@ -986,7 +902,7 @@ Connected - + @@ -1070,7 +986,7 @@ Connected - + @@ -1154,7 +1070,7 @@ TX Rate: --- Mbps - + @@ -1196,6 +1112,258 @@ RX Latency: ----- ms + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + false + + + background-color:darkgreen; border: 0.5px solid black; + + + GPS Accuracy + -- m + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + false + + + background-color:darkgreen; border: 0.5px solid black; + + + GPS Satellites +0 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + false + + + background-color:darkgreen; border: 0.5px solid black; + + + GPS Heading Valid +False + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + false + + + background-color:darkgreen; border: 0.5px solid black; + + + GPS Fix +False + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkgreen; border: 0.5px solid black; + + + QFrame::NoFrame + + + Battery Voltage +N/A + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkgreen; border: 0.5px solid black; + + + QFrame::NoFrame + + + CO2 Levels +--- ppm + + + Qt::AlignCenter + + + diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index ec55c1c..1f78383 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -7,7 +7,7 @@ [{name: "/rover_control/command_control/ground_station_drive", compress: true, rate: 15.0}, - {name: "/rover_control/pan_tilt/control", compress: true, rate: 15.0}] + {name: "/rover_control/pan_tilt/control", compress: true, rate: 20.0}] 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 7954592..511bd96 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 @@ -2,7 +2,7 @@ - + @@ -10,7 +10,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -26,7 +26,7 @@ - + @@ -34,7 +34,7 @@ - + @@ -42,7 +42,7 @@ - + @@ -50,7 +50,7 @@ - + @@ -58,7 +58,7 @@ - + @@ -66,7 +66,7 @@ - + @@ -74,7 +74,7 @@ - + @@ -82,7 +82,7 @@ - + @@ -90,7 +90,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -106,7 +106,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -122,15 +122,16 @@ - + - [{name: "/rover_status/gps_status", compress: false, rate: 1.0}] + [{name: "/rover_status/gps_status", compress: false, rate: 1.0}, + {name: "/rover_odometry/fix", compress: false, rate: 5.0}] - + @@ -138,7 +139,7 @@ - + @@ -146,13 +147,13 @@ - + [ - {name: "/rover_status/gps_status", compress: true, rate: 5.0}, - {name: "/rover_status/battery_status", compress: false, rate: 1.0} + {name: "/rover_status/battery_status", compress: false, rate: 1.0}, + {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0} ] diff --git a/software/ros_packages/rover_odometry/src/odometry.py b/software/ros_packages/rover_odometry/src/odometry.py index a33978c..4b35573 100755 --- a/software/ros_packages/rover_odometry/src/odometry.py +++ b/software/ros_packages/rover_odometry/src/odometry.py @@ -7,6 +7,7 @@ import rospy import serial from time import time, sleep import json +import re from nmea_msgs.msg import Sentence from sensor_msgs.msg import Imu @@ -75,13 +76,42 @@ class Odometry(object): imu = temp.get('imu', None) if gps: - print gps + # ###### THIS IS HERE TO DEAL WITH UBLOX GPS ##### + if "GNGGA" in gps: + gps = gps.replace("GNGGA", "GPGGA") + gps = gps[:-2] + str(self.chksum_nmea(gps))[2:] + # print gps + # ##### + self.broadcast_gps(gps) if imu: # print imu self.broadcast_imu(imu) + @staticmethod + def chksum_nmea(sentence): + # String slicing: Grabs all the characters + # between '$' and '*' and nukes any lingering + # newline or CRLF + chksumdata = re.sub("(\n|\r\n)", "", sentence[sentence.find("$") + 1:sentence.find("*")]) + + # Initializing our first XOR value + csum = 0 + + # For each char in chksumdata, XOR against the previous + # XOR'd char. The final XOR of the last char will be our + # checksum to verify against the checksum we sliced off + # the NMEA sentence + + for c in chksumdata: + # XOR'ing value of csum against the next char in line + # and storing the new XOR value in csum + csum ^= ord(c) + + # Do we have a validated sentence? + return hex(csum) + def broadcast_gps(self, gps): message = Sentence() message.header.frame_id = "gps" diff --git a/software/ros_packages/rover_status/src/system_statuses_node.py b/software/ros_packages/rover_status/src/system_statuses_node.py index d73fe53..a562c57 100755 --- a/software/ros_packages/rover_status/src/system_statuses_node.py +++ b/software/ros_packages/rover_status/src/system_statuses_node.py @@ -36,7 +36,7 @@ DEFAULT_IRIS_STATUS_TOPIC_NAME = "/rover_control/iris_status" DEFAULT_BOGIE_LEFT_TOPIC_NAME = '/rover_control/drive_status/left' DEFAULT_BOGIE_RIGHT_TOPIC_NAME = '/rover_control/drive_status/right' DEFAULT_BOGIE_REAR_TOPIC_NAME = '/rover_control/drive_status/rear' -DEFAULT_GPS_NMEA_TOPIC_NAME = '/odometry/gps/sentence' +DEFAULT_GPS_NMEA_TOPIC_NAME = '/rover_odometry/gps/sentence' ##################################### @@ -130,7 +130,11 @@ class SystemStatuses: # Pulls the UTC GPS Information (WIP v2.0) def __set_gps_info(self, data): self.GPS_msg.gps_connected = True - self.Nmea_Message = pynmea2.parse(data.sentence) + try: + self.Nmea_Message = pynmea2.parse(data.sentence) + except: + return + if self.Nmea_Message.sentence_type == 'GGA': if int(self.Nmea_Message.gps_qual) != 0: self.GPS_msg.gps_fix = True