diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py index 03c08f701..16ca7f330 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py @@ -4,9 +4,11 @@ import numpy as np import rospy +from geometry_msgs.msg import Point from mil_msgs.msg import ObjectsInImage from mil_msgs.srv import CameraToLidarTransform from mil_tools import rosmsg_to_numpy +from navigator_msgs.msg import Wildlife from .navigator import NaviGatorMission @@ -20,9 +22,9 @@ class MoveState(Enum): class Wildlife2024(NaviGatorMission): animals_observed = { - "blue_manatee_buoy": False, # Manatee => Counter clockwise - "green_iguana_buoy": False, # Iguana => Clockwise (by choice) - "red_python_buoy": False, # Python => Clockwise + "blue_manatee_buoy": None, # Manatee => Counter clockwise + "green_iguana_buoy": None, # Iguana => Clockwise (by choice) + "red_python_buoy": None, # Python => Clockwise } @classmethod @@ -38,6 +40,7 @@ async def setup(cls): @classmethod async def shutdown(cls): await cls.camsub.shutdown() + await cls.report_findings.shutdown() def get_indices_of_type(self, objects, classifications): if isinstance(classifications, str): @@ -256,12 +259,19 @@ def is_done(objects, positions): filter_and_sort, ) - # Go to each object and circle them accordingly for animal in animals: + position = animal[1] object = animal[0] label = object.labeled_classification # Update explore dict - self.animals_observed[label] = True + self.animals_observed[label] = position + + await self.report_findings() + + # Go to each object and circle them accordingly + for animal in animals: + object = animal[0] + label = object.labeled_classification if label == self.chosen_animal: await self.circle_animal(animal) if self.chosen_animal == "red_python_buoy": @@ -280,10 +290,36 @@ def is_done(objects, positions): else: print("ALL WILDLIFE OBSERVED!") + async def report_findings(self): + self.send_feedback("Sending message to display...") + self.send_feedback(self.animals_observed["blue_manatee_buoy"]) + msg = Wildlife() + msg.has_blue_manatee = self.animals_observed["blue_manatee_buoy"] is not None + if isinstance(self.animals_observed["blue_manatee_buoy"], list): + msg.blue_manatee = Point() + msg.blue_manatee.x = self.animals_observed["blue_manatee_buoy"][0] + msg.blue_manatee.y = self.animals_observed["blue_manatee_buoy"][1] + + msg.has_green_iguana = self.animals_observed["green_iguana_buoy"] is not None + if isinstance(self.animals_observed["green_iguana_buoy"], list): + msg.green_iguana = Point() + msg.green_iguana.x = self.animals_observed["green_iguana_buoy"][0] + msg.green_iguana.y = self.animals_observed["green_iguana_buoy"][1] + + msg.has_red_python = self.animals_observed["red_python_buoy"] is not None + if isinstance(self.animals_observed["red_python_buoy"], list): + msg.red_python = Point() + msg.red_python.x = self.animals_observed["red_python_buoy"][0] + msg.red_python.y = self.animals_observed["red_python_buoy"][1] + + await self.exploration_report.publish(msg) + async def run(self, args): # Check nearest objects self.objects_passed = set() self.chosen_animal = rospy.get_param("chosen_animal", "red_python_buoy") + self.exploration_report = self.nh.advertise("/wildlife_report", Wildlife) + await self.exploration_report.setup() await self.change_wrench("autonomous") # Wait a bit for PCDAR to get setup # await self.set_classifier_enabled.wait_for_service() diff --git a/NaviGator/utils/navigator_gui/navigator_gui/__init__.py b/NaviGator/utils/navigator_gui/navigator_gui/__init__.py index 8683694b0..135db930f 100755 --- a/NaviGator/utils/navigator_gui/navigator_gui/__init__.py +++ b/NaviGator/utils/navigator_gui/navigator_gui/__init__.py @@ -2,3 +2,4 @@ from .dashboard import Dashboard from .shooter import Shooter from .stc_display import StcDisplay +from .wildlife_display import WildlifeDisplay diff --git a/NaviGator/utils/navigator_gui/navigator_gui/wildlife_display.py b/NaviGator/utils/navigator_gui/navigator_gui/wildlife_display.py new file mode 100644 index 000000000..cf79c105b --- /dev/null +++ b/NaviGator/utils/navigator_gui/navigator_gui/wildlife_display.py @@ -0,0 +1,166 @@ +import os + +import matplotlib.pyplot as plt +import rospkg +import rospy +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from navigator_msgs.msg import Wildlife +from python_qt_binding import QtWidgets, loadUi +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from qt_gui.plugin import Plugin + + +class WildlifeDisplay(Plugin): + UPDATE_MILLISECONDS = 1000 + WILDLIFE_TOPIC = "/wildlife_report" + colors = { + "red": "QWidget {background-color:#FF432E;}", + "green": "QWidget {background-color:#B1EB00;}", + "blue": "QWidget {background-color:#4AA8DB;}", + } + + def __init__(self, context): + super().__init__(context) + # Give QObjects reasonable names + self.setObjectName("Wildlife Display") + + # Process standalone plugin command-line arguments + from argparse import ArgumentParser + + parser = ArgumentParser() + # Add argument(s) to the parser. + parser.add_argument( + "-q", + "--quiet", + action="store_true", + dest="quiet", + help="Put plugin in silent mode", + ) + args, unknowns = parser.parse_known_args(context.argv()) + if not args.quiet: + print("arguments: ", args) + print("unknowns: ", unknowns) + + # Create QWidget + self._widget = QWidget() + # Get path to UI file which should be in the "resource" folder of this package + ui_file = os.path.join( + rospkg.RosPack().get_path("navigator_gui"), + "resource", + "wildlife.ui", + ) + # Extend the widget with all attributes and children from UI file + loadUi(ui_file, self._widget) + # Give QObjects reasonable names + self._widget.setObjectName("MyPluginUi") + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle( + self._widget.windowTitle() + (" (%d)" % context.serial_number()), + ) + # Add widget to the user interface + context.add_widget(self._widget) + self.connect_ui() + self.wildlife_pub = rospy.Subscriber( + self.WILDLIFE_TOPIC, + Wildlife, + self.update_gui, + ) + + def connect_ui(self) -> None: + # Existing UI connections + self.color1_rect = self._widget.findChild(QtWidgets.QColumnView, "color1_rect") + self.color2_rect = self._widget.findChild(QtWidgets.QColumnView, "color2_rect") + self.color3_rect = self._widget.findChild(QtWidgets.QColumnView, "color3_rect") + self.label1 = self._widget.findChild(QtWidgets.QLabel, "label") + self.label2 = self._widget.findChild(QtWidgets.QLabel, "label_2") + self.label3 = self._widget.findChild(QtWidgets.QLabel, "label_3") + + # Add a matplotlib canvas to display the graph + self.figure = plt.figure() + self.canvas = FigureCanvas(self.figure) + layout = QVBoxLayout() + layout.addWidget(self.canvas) + + self._widget.findChild(QtWidgets.QWidget, "graph_area").setLayout(layout) + + # Test display TODO: Remove + # msg = Wildlife() + # msg.has_red_python = True + # msg.red_python = Point(2.0, 3.0, 0.0) + + # msg.has_green_iguana = False + # msg.green_iguana = Point() + + # msg.has_blue_manatee = True + # msg.blue_manatee = Point(-4.0, 5.0, 0.0) + + # self.plot_points(msg) + + def plot_points(self, msg: Wildlife): + """ + Plot points on the graph with their names and corresponding colors based on the message. + + :param msg: WildlifeDisplay message containing points and availability flags + """ + self.figure.clear() + ax = self.figure.add_subplot(111) + + # Define the points and colors for the animals + animals = [ + (msg.has_red_python, msg.red_python, "Red Python", "red"), + (msg.has_green_iguana, msg.green_iguana, "Green Iguana", "green"), + (msg.has_blue_manatee, msg.blue_manatee, "Blue Manatee", "blue"), + ] + + # Plot only the points that are present + for has_animal, point, name, color in animals: + if has_animal: # Plot only if the animal is present + x, y = point.x, point.y + ax.plot(x, y, "o", label=name, color=color) # 'o' for circular marker + ax.text(x, y, name, fontsize=10, ha="right") + + # Set graph limits and titles + ax.set_xlabel("X Axis") + ax.set_ylabel("Y Axis") + ax.set_title("Wildlife Display Points") + ax.legend() + + # Refresh the canvas + self.canvas.draw() + + def update_gui(self, msg: Wildlife): + """ + Update the GUI with the received message. + + :param msg: WildlifeDisplay message containing the data + """ + print("updating gui...") + + # Plot the points based on the message + self.plot_points(msg) + + print("finished updating!") + + def shutdown_plugin(self): + # TODO unregister all publishers here + pass + + def save_settings(self, plugin_settings, instance_settings): + # TODO save intrinsic configuration, usually using: + # instance_settings.set_value(k, v) + pass + + def restore_settings(self, plugin_settings, instance_settings): + # TODO restore intrinsic configuration, usually using: + # v = instance_settings.value(k) + pass + + # def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget title bar + # Usually used to open a modal configuration dialog diff --git a/NaviGator/utils/navigator_gui/plugin.xml b/NaviGator/utils/navigator_gui/plugin.xml index dc6afffa5..f3e801e82 100644 --- a/NaviGator/utils/navigator_gui/plugin.xml +++ b/NaviGator/utils/navigator_gui/plugin.xml @@ -42,4 +42,18 @@ It's STC time bitchess + + + Shows the location of found wildlife + + + + resource/mil_logo.png + + + resource/shooter.png + + It's Wildlife time respectable members of this community + + diff --git a/NaviGator/utils/navigator_gui/resource/wildlife.ui b/NaviGator/utils/navigator_gui/resource/wildlife.ui new file mode 100644 index 000000000..8a04edf99 --- /dev/null +++ b/NaviGator/utils/navigator_gui/resource/wildlife.ui @@ -0,0 +1,24 @@ + + + Form + + + + 0 + 0 + 800 + 600 + + + + + + 10 + 10 + 400 + 300 + + + + + diff --git a/NaviGator/utils/navigator_msgs/CMakeLists.txt b/NaviGator/utils/navigator_msgs/CMakeLists.txt index dffa1e0f2..ba85fdbb2 100644 --- a/NaviGator/utils/navigator_msgs/CMakeLists.txt +++ b/NaviGator/utils/navigator_msgs/CMakeLists.txt @@ -29,6 +29,7 @@ add_message_files( Hosts.msg ColoramaDebug.msg ScanTheCode.msg + Wildlife.msg ) add_service_files( diff --git a/NaviGator/utils/navigator_msgs/msg/Wildlife.msg b/NaviGator/utils/navigator_msgs/msg/Wildlife.msg new file mode 100644 index 000000000..e5ca2344e --- /dev/null +++ b/NaviGator/utils/navigator_msgs/msg/Wildlife.msg @@ -0,0 +1,8 @@ +bool has_red_python +geometry_msgs/Point red_python + +bool has_green_iguana +geometry_msgs/Point green_iguana + +bool has_blue_manatee +geometry_msgs/Point blue_manatee