Added display for wildlife

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

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

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

NaviGator/utils/navigator_gui/plugin.xml

@@ -42,4 +42,18 @@
       <statustip>It's STC time bitchess</statustip>
     </qtgui>
   </class>
+  <class name="WildlifeDisplay" type="navigator_gui.WildlifeDisplay" base_class_type="rqt_gui_py::Plugin">
+    <description>
+      Shows the location of found wildlife
+    </description>
+    <qtgui>
+      <group>
+        <icon type="file">resource/mil_logo.png</icon>
+        <label>MIL</label>
+      </group>
+      <icon type="file">resource/shooter.png</icon>
+      <label>WIldlife Display</label>
+      <statustip>It's Wildlife time respectable members of this community</statustip>
+    </qtgui>
+  </class>
 </library>

NaviGator/utils/navigator_gui/resource/wildlife.ui

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+    <class>Form</class>
+    <widget class="QWidget" name="Form">
+        <property name="geometry">
+            <rect>
+                <x>0</x>
+                <y>0</y>
+                <width>800</width>
+                <height>600</height>
+            </rect>
+        </property>
+        <widget class="QWidget" name="graph_area">
+            <property name="geometry">
+                <rect>
+                    <x>10</x>
+                    <y>10</y>
+                    <width>400</width>
+                    <height>300</height>
+                </rect>
+            </property>
+        </widget>
+    </widget>
+</ui>

NaviGator/utils/navigator_msgs/CMakeLists.txt

@@ -29,6 +29,7 @@ add_message_files(
   Hosts.msg
   ColoramaDebug.msg
   ScanTheCode.msg
+  Wildlife.msg
 )
 
 add_service_files(

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