From eaaaf6f7f975ef4a0f6e0e6d60a116dec69154e9 Mon Sep 17 00:00:00 2001 From: Chris Pham Date: Thu, 15 Mar 2018 09:16:00 -0700 Subject: [PATCH 1/7] Generate current location marker changed and fit PIP8 --- .../src/Framework/MapSystems/RoverMap.py | 29 +++++++++---------- .../MapSystems/RoverMapCoordinator.py | 12 ++++++-- 2 files changed, 23 insertions(+), 18 deletions(-) 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 eca806f..d5b18c4 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -349,6 +349,7 @@ class OverlayImage(object): self.height = height self.big_image = None self.display_image = None + self.indicator = None self.helper = MapHelper.MapHelper() x, y = self._get_cartesian(latitude, longitude) @@ -370,6 +371,7 @@ class OverlayImage(object): True) self.display_image = self.helper.new_image(self.width, self.height, True) + self.generate_dot_and_hat() def _get_cartesian(self, lat, lon): """ @@ -409,23 +411,20 @@ class OverlayImage(object): return self.display_image + def generate_dot_and_hat(self): + self.indicator = self.helper.new_image(25, 50, True) + draw = PIL.ImageDraw.Draw(self.indicator) + draw.ellipse((0, 25, 25, 50), fill="red") + draw.line((0, 24, 13, 1), fill="red", width=5) + draw.line((13, 1, 24, 24), fill="red", width=5) + def _draw_rover(self, lat, lon, size, scaler): x, y = self._get_cartesian(lat, lon) - draw = PIL.ImageDraw.Draw(self.big_image) - draw.ellipse((x-size, y-size, x+size, y+size), fill="red") - point_1 = tuple((math.cos(x-2*size * scaler), math.sin(y * scaler))) - point_2 = tuple((math.cos(x * scaler), math.sin(y+2*size * scaler))) - point_3 = (math.cos(x+2*size * scaler), math.sin(y * scaler)) - draw.line( - (point_1, - point_2), - fill="red", - width=600) - draw.line( - (point_2, - point_3), - fill="red", - width=600) + # Center of the circle on the indicator is (12.5, 37.5) + x = x - 12 + y = y - 37 + self.display_image.paste(self.indicator, (x, y)) + self.display_image.save("Something.png") def update(self): 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 f6e52a2..19ac524 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -84,9 +84,15 @@ 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) - 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) + self.overlay_image_object.update_new_location(44.567161, + -123.278432, + .7) + 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 qim = ImageQt(self.map_image) self.map_pixmap = QtGui.QPixmap.fromImage(qim) From fdffd0d3e4c7d87a126a8bdfeab18b03a0814782 Mon Sep 17 00:00:00 2001 From: Chris Pham Date: Thu, 15 Mar 2018 09:44:49 -0700 Subject: [PATCH 2/7] Created square indicator for ez of rotation --- .../src/Framework/MapSystems/RoverMap.py | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) 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 d5b18c4..fcda2f8 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -406,26 +406,27 @@ class OverlayImage(object): return int(x), int(y) def update_new_location(self, latitude, longitude, compass): - self._draw_rover(latitude, longitude, 10, compass) + self._draw_rover(latitude, longitude, compass) self.update() return self.display_image def generate_dot_and_hat(self): - self.indicator = self.helper.new_image(25, 50, True) + self.indicator = self.helper.new_image(100, 100, True) draw = PIL.ImageDraw.Draw(self.indicator) - draw.ellipse((0, 25, 25, 50), fill="red") - draw.line((0, 24, 13, 1), fill="red", width=5) - draw.line((13, 1, 24, 24), fill="red", width=5) + 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_rover(self, lat, lon, size, scaler): + def _draw_rover(self, lat, lon, angle=0): x, y = self._get_cartesian(lat, lon) # Center of the circle on the indicator is (12.5, 37.5) - x = x - 12 - y = y - 37 + x = x - 50 + y = y - 50 + rotated = self.indicator.copy() + rotated = rotated.rotate(angle, expand=True) self.display_image.paste(self.indicator, (x, y)) - - self.display_image.save("Something.png") + # self.display_image.save("Something.png") def update(self): self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y)) From 1c7646d07dff79c2e5863da33f3ee4f1ae4b02b5 Mon Sep 17 00:00:00 2001 From: Ken Steinfeldt Date: Thu, 15 Mar 2018 10:06:43 -0700 Subject: [PATCH 3/7] change current outputs to signals/slots --- .../src/Framework/StatusSystems/StatusCore.py | 96 ++++++++++++++----- 1 file changed, 74 insertions(+), 22 deletions(-) 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 38e65ac..ff62b0b 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -17,9 +17,27 @@ MISC_TOPIC_NAME = "/rover_status/misc_status" class SensorCore(QtCore.QThread): + # ########## create signals for slots ########## jetson_cpu_update_ready__signal = QtCore.pyqtSignal(str) jetson_cpu_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + jetson_ram_update_ready__signal = QtCore.pyqtSignal(str) + jetson_ram_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + + bogie_connection_1_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + bogie_connection_2_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + bogie_connection_3_stylesheet_change_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) + camera_main_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + + gps_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + + frsky_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + + def __init__(self, shared_objects): super(SensorCore, self).__init__() @@ -32,6 +50,7 @@ class SensorCore(QtCore.QThread): self.cpu_read = self.screen_main_window.lineEdit # type: QtWidgets.QLabel self.ram_read = self.screen_main_window.lineEdit_2 # type: QtWidgets.QLabel + # ########## set vars to gui elements self.rover_conn = self.screen_main_window.rover # type: QtWidgets.QLabel self.frsky = self.screen_main_window.frsky # type: QtWidgets.QLabel self.nav_mouse = self.screen_main_window.nav_mouse # type: QtWidgets.QLabel @@ -48,7 +67,7 @@ class SensorCore(QtCore.QThread): self.cpu = self.screen_main_window.cpu # type: QtWidgets.QLabel self.ram = self.screen_main_window.ram # type: QtWidgets.QLabel - # Subscription examples on pulling data from system_statuses_node.py + # ########## subscriptions pulling data from system_statuses_node.py ########## self.camera_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback) self.bogie_status = rospy.Subscriber(BOGIE_TOPIC_NAME, BogieStatuses, self.__bogie_callback) self.frsky_status = rospy.Subscriber(FRSKY_TOPIC_NAME, FrSkyStatus, self.__frsky_callback) @@ -72,32 +91,40 @@ class SensorCore(QtCore.QThread): self.camera_msg.camera_main_navigation = data.camera_main_navigation if self.camera_msg.camera_zed is False: - self.zed.setStyleSheet("background-color: red;") + # self.zed.setStyleSheet("background-color: red;") + self.camera_zed_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.zed.setStyleSheet("background-color: darkgreen;") + # self.zed.setStyleSheet("background-color: darkgreen;") + self.camera_zed_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.camera_msg.camera_undercarriage is False: - self.under_cam.setStyleSheet("background-color: red;") + # self.under_cam.setStyleSheet("background-color: red;") + self.camera_under_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.under_cam.setStyleSheet("background-color: darkgreen;") + # self.under_cam.setStyleSheet("background-color: darkgreen;") + self.camera_under_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.camera_msg.camera_chassis is False: - self.chassis_cam.setStyleSheet("background-color: red;") + # self.chassis_cam.setStyleSheet("background-color: red;") + self.camera_chassis_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.chassis_cam.setStyleSheet("background-color: darkgreen;") + # self.chassis_cam.setStyleSheet("background-color: darkgreen;") + self.camera_chassis_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.camera_msg.camera_main_navigation is False: - self.main_cam.setStyleSheet("background-color: red;") + # self.main_cam.setStyleSheet("background-color: red;") + self.camera_main_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.main_cam.setStyleSheet("background-color: darkgreen;") + # self.main_cam.setStyleSheet("background-color: darkgreen;") + self.camera_main_stylesheet_change_ready__signal.emit("background-color: darkgreen;") def __frsky_callback(self, data): self.FrSky_msg.FrSky_controller_connection_status = data.FrSky_controller_connection_status if self.FrSky_msg.FrSky_controller_connection_status is False: - self.frsky.setStyleSheet("background-color: red;") + self.frsky_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.frsky.setStyleSheet("background-color: darkgreen;") + self.frsky_stylesheet_change_ready__signal.emit("background-color: darkgreen;") def __bogie_callback(self, data): self.bogie_msg.bogie_connection_1 = data.bogie_connection_1 @@ -105,19 +132,25 @@ class SensorCore(QtCore.QThread): self.bogie_msg.bogie_connection_3 = data.bogie_connection_3 if self.bogie_msg.bogie_connection_1 is False: - self.bogie_right.setStyleSheet("background-color: red;") + # self.bogie_right.setStyleSheet("background-color: red;") + self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.bogie_right.setStyleSheet("background-color: darkgreen;") + # self.bogie_right.setStyleSheet("background-color: darkgreen;") + self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.bogie_msg.bogie_connection_2 is False: - self.bogie_left.setStyleSheet("background-color: red;") + # self.bogie_left.setStyleSheet("background-color: red;") + self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.bogie_left.setStyleSheet("background-color: darkgreen;") + # self.bogie_left.setStyleSheet("background-color: darkgreen;") + self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.bogie_msg.bogie_connection_3 is False: - self.bogie_rear.setStyleSheet("background-color: red;") + # self.bogie_rear.setStyleSheet("background-color: red;") + self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.bogie_rear.setStyleSheet("background-color: darkgreen;") + # self.bogie_rear.setStyleSheet("background-color: darkgreen;") + self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: darkgreen;") def __jetson_callback(self, data): self.jetson_msg.jetson_CPU = data.jetson_CPU @@ -128,13 +161,19 @@ class SensorCore(QtCore.QThread): if self.jetson_msg.jetson_CPU > 79: self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: orange;") - # self.cpu.setStyleSheet("background-color: orange;") elif self.jetson_msg.jetson_CPU > 89: self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: red;") - # self.cpu.setStyleSheet("background-color: red;") else: self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: darkgreen;") - # self.cpu.setStyleSheet("background-color: darkgreen;") + + self.jetson_ram_update_ready__signal.emit(str(self.jetson_msg.jetson_RAM)) + + if self.jetson_msg.jetson_RAM > 79: + self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: orange;") + elif self.jetson_msg.jetson_RAM > 89: + self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: red;") + else: + self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: darkgreen;") # self.jetson_msg.jetson_RAM = data.jetson_RAM # self.ram.setText(str(self.jetson_msg.jetson_RAM)) @@ -154,9 +193,11 @@ class SensorCore(QtCore.QThread): self.GPS_msg.GPS_connection_status = data.GPS_connection_status if self.GPS_msg.GPS_connection_status is False: - self.gps.setStyleSheet("background-color: red") + # self.gps.setStyleSheet("background-color: red") + self.gps_stylesheet_change_ready__signal.emit("background-color: red;") else: - self.gps.setStyleSheet("background-color: darkgreen;") + # self.gps.setStyleSheet("background-color: darkgreen;") + self.gps_stylesheet_change_ready__signal.emit("background-color: darkgreen;") def __misc_callback(self, data): self.misc_msg.arm_connection_status = data.arm_connection_status @@ -177,6 +218,17 @@ class SensorCore(QtCore.QThread): def connect_signals_and_slots(self): self.jetson_cpu_update_ready__signal.connect(self.cpu.setText) self.jetson_cpu_stylesheet_change_ready__signal.connect(self.cpu.setStyleSheet) + self.jetson_ram_update_ready__signal.connect(self.ram.setText) + self.jetson_ram_stylesheet_change_ready__signal(self.ram.setStyleSheet) + self.bogie_connection_1_stylesheet_change_ready__signal.connect(self.bogie_right.setStyleSheet) + self.bogie_connection_2_stylesheet_change_ready__signal.connect(self.bogie_left.setStyleSheet) + self.bogie_connection_3_stylesheet_change_ready__signal.connect(self.bogie_rear.setStyleSheet) + self.camera_zed_stylesheet_change_ready__signal.connect(self.zed.setStyleSheet) + 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.frsky_stylesheet_change_ready__signal.connect(self.frsky.setStyleSheet) def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal): start_signal.connect(self.start) From d2e1b7e22495799e9df440016625a1f5afa54afb Mon Sep 17 00:00:00 2001 From: Ken Steinfeldt Date: Thu, 15 Mar 2018 10:38:48 -0700 Subject: [PATCH 4/7] add emmc and gpu temp to outputs --- .../src/Framework/StatusSystems/StatusCore.py | 40 ++++++++++++------ .../src/Resources/Ui/left_screen.ui | 41 +++++++++++++++---- 2 files changed, 61 insertions(+), 20 deletions(-) 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 ff62b0b..2c3de6a 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -20,7 +20,10 @@ class SensorCore(QtCore.QThread): # ########## create signals for slots ########## jetson_cpu_update_ready__signal = QtCore.pyqtSignal(str) jetson_cpu_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) - + jetson_emmc_update_ready__signal = QtCore.pyqtSignal(str) + jetson_emmc_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) + jetson_gpu_temp_update_ready__signal = QtCore.pyqtSignal(str) + jetson_gpu_temp_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) jetson_ram_update_ready__signal = QtCore.pyqtSignal(str) jetson_ram_stylesheet_change_ready__signal = QtCore.pyqtSignal(str) @@ -66,6 +69,8 @@ class SensorCore(QtCore.QThread): self.clock = self.screen_main_window.clock # type: QtWidgets.QLCDNumber self.cpu = self.screen_main_window.cpu # type: QtWidgets.QLabel self.ram = self.screen_main_window.ram # type: QtWidgets.QLabel + self.gpu_temp = self.screen_main_window.gpu_temp # type: QtWidgets.QLabel + self.emmc = self.screen_main_window.emmc # type: QtWidgets.QLabel # ########## subscriptions pulling data from system_statuses_node.py ########## self.camera_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback) @@ -175,18 +180,23 @@ class SensorCore(QtCore.QThread): else: self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: darkgreen;") - # self.jetson_msg.jetson_RAM = data.jetson_RAM - # self.ram.setText(str(self.jetson_msg.jetson_RAM)) - # if self.jetson_msg.jetson_RAM > 79: - # self.ram.setStyleSheet("background-color: orange;") - # elif self.jetson_msg.jetson_RAM > 89: - # self.ram.setStyleSheet("background-color: red;") - # else: - # self.ram.setStyleSheet("background-color: darkgreen;") - # - # self.jetson_msg.jetson_EMMC = data.jetson_EMMC - # self.jetson_msg.jetson_NVME_SSD = data.jetson_NVME_SSD - #rospy.loginfo(self.jetson_msg) + self.jetson_gpu_temp_update_ready__signal.emit(str(self.jetson_msg.jetson_GPU_temp)) + + if self.jetson_msg.jetson_GPU_temp > 64: + self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: orange;") + elif self.jetson_msg.jetson_GPU_temp > 79: + self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: red;") + else: + self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: darkgreen;") + + self.jetson_emmc_update_ready__signal.emit(str(self.jetson_msg.jetson_EMMC)) + + if self.jetson_msg.jetson_EMMC > 79: + self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: orange;") + elif self.jetson_msg.jetson_EMMC > 89: + self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: red;") + else: + self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: darkgreen") def __gps_callback(self, data): self.GPS_msg.UTC_GPS_time = data.UTC_GPS_time @@ -220,6 +230,10 @@ class SensorCore(QtCore.QThread): self.jetson_cpu_stylesheet_change_ready__signal.connect(self.cpu.setStyleSheet) self.jetson_ram_update_ready__signal.connect(self.ram.setText) self.jetson_ram_stylesheet_change_ready__signal(self.ram.setStyleSheet) + self.jetson_emmc_update_ready__signal.connect(self.emmc.setText) + self.jetson_emmc_stylesheet_change_ready__signal.connect(self.emmc.setStyleSheet) + self.jetson_gpu_temp_update_ready__signal.connect(self.gpu_temp.setText) + self.jetson_gpu_temp_stylesheet_change_ready__signal(self.gpu_temp.setStyleSheet) self.bogie_connection_1_stylesheet_change_ready__signal.connect(self.bogie_right.setStyleSheet) self.bogie_connection_2_stylesheet_change_ready__signal.connect(self.bogie_left.setStyleSheet) self.bogie_connection_3_stylesheet_change_ready__signal.connect(self.bogie_rear.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 2fa285b..1f212a0 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 @@ -39,7 +39,16 @@ 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -68,7 +77,16 @@ - + + 0 + + + 0 + + + 0 + + 0 @@ -80,22 +98,22 @@ 0 - + background-color: darkgreen; - placeholder + <html><head/><body><p align="center"><span style=" font-weight:600;">EMMC</span></p></body></html> - + background-color: darkgreen; - placeholder + <html><head/><body><p align="center"><span style=" font-weight:600;">GPU </span><span style=" font-weight:600;">Temp</span></p></body></html> @@ -798,7 +816,16 @@ Fix - + + 0 + + + 0 + + + 0 + + 0 From 102cdf0c77088d79c5cf537464700c2672dadfe3 Mon Sep 17 00:00:00 2001 From: Chris Pham Date: Thu, 15 Mar 2018 10:53:34 -0700 Subject: [PATCH 5/7] Updated new location, _get_table return less, change update_overlay --- .../src/Framework/MapSystems/RoverMap.py | 12 ++++++- .../MapSystems/RoverMapCoordinator.py | 33 +++++++++++++++++-- 2 files changed, 41 insertions(+), 4 deletions(-) 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 fcda2f8..d3b4975 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py @@ -405,7 +405,16 @@ class OverlayImage(object): return int(x), int(y) - def update_new_location(self, latitude, longitude, compass): + def update_new_location(self, latitude, longitude, + compass, navigation_list, landmark_list): + size = 5 + draw = PIL.ImageDraw.Draw(self.big_image) + for element in navigation_list: + x, y = self._get_cartesian(element[1], element[2]) + draw.ellipsis((x-size, y-size, x+size, y+size), fill="red") + for element in navigation_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(latitude, longitude, compass) self.update() @@ -430,3 +439,4 @@ class OverlayImage(object): def update(self): self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y)) + 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 19ac524..1ad197b 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -21,6 +21,7 @@ import RoverMap class RoverMapCoordinator(QtCore.QThread): pixmap_ready_signal = QtCore.pyqtSignal() + change_waypoint_signal = QtCore.pyqtSignal() def __init__(self, shared_objects): super(RoverMapCoordinator, self).__init__() @@ -28,6 +29,8 @@ 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.navigation_label = self.left_screen.tableWidget + self.landmark_label = self.left_screen.tableWidget_2 self.setings = QtCore.QSettings() @@ -84,9 +87,12 @@ 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) + # self.overlay_image_object.update_new_location(44.567161, + # -123.278432, + # .7, + # [], + # []) + self.update_overlay() self.map_image.paste(self.overlay_image_object.display_image, (0, 0), self.overlay_image_object.display_image) @@ -100,6 +106,7 @@ class RoverMapCoordinator(QtCore.QThread): def connect_signals_and_slots(self): self.pixmap_ready_signal.connect(self.pixmap_ready__slot) + self.change_waypoint_signal.connect(self.update_waypoints) def on_kill_threads_requested_slot(self): self.run_thread_flag = False @@ -112,3 +119,23 @@ class RoverMapCoordinator(QtCore.QThread): def pixmap_ready__slot(self): self.mapping_label.setPixmap(self.map_pixmap) + + def _get_table_elements(self, UI_element): + temp_list = [] + count = UI_element.rowCount() + for row in range(0, count): + num = UI_element.item(row, 0) + lat = UI_element.cellWidget(row, 1) + lng = UI_element.cellWidget(row, 2) + temp_tuple = (num, lat, lng) + temp_list.append(temp_tuple) + return temp_list + + def update_overlay(self): + navigation_list = self._get_table_elements(self.navigation_label) + landmark_list = self._get_table_elements(self.landmark_label) + self.overlay_image_object.update_new_location(self.latitude, + self.longitude, + 70, + navigation_list, + landmark_list) From 83c281917c8fa3873c9c03585826278a1eba96c8 Mon Sep 17 00:00:00 2001 From: Chris Pham Date: Thu, 15 Mar 2018 10:55:43 -0700 Subject: [PATCH 6/7] changed signal returncall --- .../src/Framework/MapSystems/RoverMapCoordinator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 1ad197b..54ea45d 100644 --- a/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py @@ -106,7 +106,7 @@ class RoverMapCoordinator(QtCore.QThread): def connect_signals_and_slots(self): self.pixmap_ready_signal.connect(self.pixmap_ready__slot) - self.change_waypoint_signal.connect(self.update_waypoints) + self.change_waypoint_signal.connect(self.update_overlay) def on_kill_threads_requested_slot(self): self.run_thread_flag = False From f33292a99220ecbeb3a42a2f57b4f7d096e77f06 Mon Sep 17 00:00:00 2001 From: Ken Steinfeldt Date: Thu, 15 Mar 2018 11:09:44 -0700 Subject: [PATCH 7/7] color change --- .../src/Framework/StatusSystems/StatusCore.py | 40 ++++++------ .../src/Resources/Ui/left_screen.ui | 63 ++++++------------- 2 files changed, 38 insertions(+), 65 deletions(-) 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 2c3de6a..579676f 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -97,28 +97,28 @@ class SensorCore(QtCore.QThread): if self.camera_msg.camera_zed is False: # self.zed.setStyleSheet("background-color: red;") - self.camera_zed_stylesheet_change_ready__signal.emit("background-color: red;") + self.camera_zed_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.zed.setStyleSheet("background-color: darkgreen;") self.camera_zed_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.camera_msg.camera_undercarriage is False: - # self.under_cam.setStyleSheet("background-color: red;") - self.camera_under_stylesheet_change_ready__signal.emit("background-color: red;") + # self.under_cam.setStyleSheet("background-color: darkred;") + self.camera_under_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.under_cam.setStyleSheet("background-color: darkgreen;") self.camera_under_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.camera_msg.camera_chassis is False: - # self.chassis_cam.setStyleSheet("background-color: red;") - self.camera_chassis_stylesheet_change_ready__signal.emit("background-color: red;") + # self.chassis_cam.setStyleSheet("background-color: darkred;") + self.camera_chassis_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.chassis_cam.setStyleSheet("background-color: darkgreen;") self.camera_chassis_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.camera_msg.camera_main_navigation is False: - # self.main_cam.setStyleSheet("background-color: red;") - self.camera_main_stylesheet_change_ready__signal.emit("background-color: red;") + # self.main_cam.setStyleSheet("background-color: darkred;") + self.camera_main_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.main_cam.setStyleSheet("background-color: darkgreen;") self.camera_main_stylesheet_change_ready__signal.emit("background-color: darkgreen;") @@ -127,7 +127,7 @@ class SensorCore(QtCore.QThread): self.FrSky_msg.FrSky_controller_connection_status = data.FrSky_controller_connection_status if self.FrSky_msg.FrSky_controller_connection_status is False: - self.frsky_stylesheet_change_ready__signal.emit("background-color: red;") + self.frsky_stylesheet_change_ready__signal.emit("background-color: darkred;") else: self.frsky_stylesheet_change_ready__signal.emit("background-color: darkgreen;") @@ -137,22 +137,22 @@ class SensorCore(QtCore.QThread): self.bogie_msg.bogie_connection_3 = data.bogie_connection_3 if self.bogie_msg.bogie_connection_1 is False: - # self.bogie_right.setStyleSheet("background-color: red;") - self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: red;") + # self.bogie_right.setStyleSheet("background-color: darkred;") + self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.bogie_right.setStyleSheet("background-color: darkgreen;") self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.bogie_msg.bogie_connection_2 is False: - # self.bogie_left.setStyleSheet("background-color: red;") - self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: red;") + # self.bogie_left.setStyleSheet("background-color: darkred;") + self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.bogie_left.setStyleSheet("background-color: darkgreen;") self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: darkgreen;") if self.bogie_msg.bogie_connection_3 is False: - # self.bogie_rear.setStyleSheet("background-color: red;") - self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: red;") + # self.bogie_rear.setStyleSheet("background-color: darkred;") + self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.bogie_rear.setStyleSheet("background-color: darkgreen;") self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: darkgreen;") @@ -167,7 +167,7 @@ class SensorCore(QtCore.QThread): if self.jetson_msg.jetson_CPU > 79: self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: orange;") elif self.jetson_msg.jetson_CPU > 89: - self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: red;") + self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: darkred;") else: self.jetson_cpu_stylesheet_change_ready__signal.emit("background-color: darkgreen;") @@ -176,7 +176,7 @@ class SensorCore(QtCore.QThread): if self.jetson_msg.jetson_RAM > 79: self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: orange;") elif self.jetson_msg.jetson_RAM > 89: - self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: red;") + self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: darkred;") else: self.jetson_ram_stylesheet_change_ready__signal.emit("background-color: darkgreen;") @@ -185,7 +185,7 @@ class SensorCore(QtCore.QThread): if self.jetson_msg.jetson_GPU_temp > 64: self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: orange;") elif self.jetson_msg.jetson_GPU_temp > 79: - self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: red;") + self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: darkred;") else: self.jetson_gpu_temp_stylesheet_change_ready__signal.emit("background-color: darkgreen;") @@ -194,7 +194,7 @@ class SensorCore(QtCore.QThread): if self.jetson_msg.jetson_EMMC > 79: self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: orange;") elif self.jetson_msg.jetson_EMMC > 89: - self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: red;") + self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: darkred;") else: self.jetson_emmc_stylesheet_change_ready__signal.emit("background-color: darkgreen") @@ -203,8 +203,8 @@ class SensorCore(QtCore.QThread): self.GPS_msg.GPS_connection_status = data.GPS_connection_status if self.GPS_msg.GPS_connection_status is False: - # self.gps.setStyleSheet("background-color: red") - self.gps_stylesheet_change_ready__signal.emit("background-color: red;") + # self.gps.setStyleSheet("background-color: darkred") + self.gps_stylesheet_change_ready__signal.emit("background-color: darkred;") else: # self.gps.setStyleSheet("background-color: darkgreen;") self.gps_stylesheet_change_ready__signal.emit("background-color: darkgreen;") 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 1f212a0..3221e1a 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 @@ -39,16 +39,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -77,16 +68,7 @@ - - 0 - - - 0 - - - 0 - - + 0 @@ -100,7 +82,7 @@ - background-color: darkgreen; + background-color: darkred; <html><head/><body><p align="center"><span style=" font-weight:600;">EMMC</span></p></body></html> @@ -110,7 +92,7 @@ - background-color: darkgreen; + background-color: darkred; <html><head/><body><p align="center"><span style=" font-weight:600;">GPU </span><span style=" font-weight:600;">Temp</span></p></body></html> @@ -145,7 +127,7 @@ - background-color:darkgreen; + background-color:darkred; QFrame::NoFrame @@ -193,7 +175,7 @@ Connected - background-color:darkgreen; + background-color:darkred; FrSky @@ -235,7 +217,7 @@ Connected false - background-color:darkgreen; + background-color:darkred; Right Bogie @@ -274,7 +256,7 @@ Connected - background-color:darkgreen; + background-color:darkred; QFrame::NoFrame @@ -316,7 +298,7 @@ Connected - background-color:darkgreen; + background-color:darkred; QFrame::NoFrame @@ -361,7 +343,7 @@ Connected false - background-color:darkgreen; + background-color:darkred; 3D Nav Mouse @@ -400,7 +382,7 @@ Connected - background-color:darkgreen; + background-color:darkred; QFrame::NoFrame @@ -448,7 +430,7 @@ Connected - background-color:darkgreen; + background-color:darkred; Chassis Camera @@ -493,7 +475,7 @@ Connected - background-color:darkgreen; + background-color:darkred; Undercarriage Camera @@ -510,7 +492,7 @@ Connected - background-color: darkgreen; + background-color: darkred; <html><head/><body><p align="center"><span style=" font-weight:600;">RAM</span></p></body></html> @@ -520,7 +502,7 @@ Connected - background-color: darkgreen; + background-color: darkred; <html><head/><body><p align="center"><span style=" font-weight:600;">CPU %</span></p></body></html> @@ -555,7 +537,7 @@ Connected - background-color:darkgreen; + background-color:darkred; QFrame::NoFrame @@ -600,7 +582,7 @@ Connected false - background-color:darkgreen; + background-color:darkred; GPS @@ -816,16 +798,7 @@ Fix - - 0 - - - 0 - - - 0 - - + 0