Add depth hold

src/pi/pixhawk_communication/launch/mavros_launch.py

@@ -33,6 +33,7 @@ def generate_launch_description() -> LaunchDescription:
             ('/pi/mavros/rc/override', '/tether/mavros/rc/override'),
             ('/pi/mavros/cmd/arming', '/tether/mavros/cmd/arming'),
             ('/pi/mavros/cmd/command', '/tether/mavros/cmd/command'),
+            ('/pi/mavros/set_mode', '/tether/mavros/set_mode'),
         ]
     )
 

src/rov_msgs/msg/VehicleState.msg

@@ -1,3 +1,4 @@
 bool pi_connected  # Whether ROS and mavros are running on the pi
 bool pixhawk_connected  # Whether mavros indicates connection to the pixhawk
-bool armed  # Whether the flight controller is armed (ready for flight)
+bool armed  # Whether the flight controller is armed (ready for flight)
+bool depth_hold_enabled  # Whether the flight controller is in depth hold mode (does not require it to be armed)

src/surface/gui/gui/operator_app.py

@@ -3,11 +3,13 @@
 from gui.widgets.tabs.general_debug_tab import GeneralDebugTab
 from gui.widgets.float_comm import FloatComm
 from gui.widgets.timer import InteractiveTimer
-from gui.widgets.task_selector import TaskSelector
+# from gui.widgets.task_selector import TaskSelector
+from gui.widgets.arm import Arm
 from gui.widgets.flood_warning import FloodWarning
 from gui.widgets.temperature import TemperatureSensor
 from gui.widgets.heartbeat import HeartbeatWidget
 from gui.widgets.ip_widget import IPWidget
+from gui.widgets.depth_hold import DepthHold
 from PyQt6.QtWidgets import QTabWidget, QWidget, QVBoxLayout, QHBoxLayout
 
 
@@ -40,7 +42,9 @@ def __init__(self) -> None:
         right_pane.addWidget(TemperatureSensor())
         right_pane.addWidget(IPWidget())
         right_pane.addStretch()
-        right_pane.addWidget(TaskSelector())
+        right_pane.addWidget(DepthHold())
+        right_pane.addWidget(Arm())
+        # right_pane.addWidget(TaskSelector())
 
         # Add tabs to root
         root_layout = QVBoxLayout()

src/surface/gui/gui/widgets/depth_hold.py

@@ -0,0 +1,114 @@
+from gui.gui_nodes.event_nodes.client import GUIEventClient
+from gui.gui_nodes.event_nodes.subscriber import GUIEventSubscriber
+from gui.styles.custom_styles import ButtonIndicator
+from PyQt6.QtCore import pyqtSignal, pyqtSlot
+from PyQt6.QtWidgets import QVBoxLayout, QHBoxLayout, QWidget, QLabel
+
+from rov_msgs.msg import VehicleState
+from mavros_msgs.srv import SetMode
+
+
+ENABLE_REQUEST = SetMode.Request(base_mode=0, custom_mode="ALT_HOLD")
+DISABLE_REQUEST = SetMode.Request(base_mode=0, custom_mode="MANUAL")
+BUTTON_WIDTH = 120
+BUTTON_HEIGHT = 60
+BUTTON_STYLESHEET = 'QPushButton { font-size: 20px; }'
+
+
+class DepthHold(QWidget):
+    """Widget for setting the pixhawk's stabilization mode."""
+
+    command_response_signal = pyqtSignal(SetMode.Response)
+    vehicle_state_signal = pyqtSignal(VehicleState)
+
+    def __init__(self) -> None:
+
+        super().__init__()
+
+        layout = QVBoxLayout()
+        layout.addWidget(QLabel("Depth Hold"))
+
+        self.setLayout(layout)
+
+        h_layout = QHBoxLayout()
+
+        self.enable_button = ButtonIndicator()
+        self.disable_button = ButtonIndicator()
+
+        self.enable_button.setText("Enable")
+        self.disable_button.setText("Disable")
+
+        self.enable_button.setMinimumWidth(BUTTON_WIDTH)
+        self.disable_button.setMinimumWidth(BUTTON_WIDTH)
+
+        self.enable_button.setMinimumHeight(BUTTON_HEIGHT)
+        self.disable_button.setMinimumHeight(BUTTON_HEIGHT)
+
+        self.enable_button.setStyleSheet(BUTTON_STYLESHEET)
+        self.disable_button.setStyleSheet(BUTTON_STYLESHEET)
+        self.enable_button.set_inactive()
+        self.disable_button.set_inactive()
+
+        self.enable_button.clicked.connect(self.enable_clicked)
+        self.disable_button.clicked.connect(self.disable_clicked)
+
+        h_layout.addWidget(self.disable_button)
+        h_layout.addWidget(self.enable_button)
+
+        layout.addLayout(h_layout)
+
+        self.command_response_signal.connect(self.response_callback)
+
+        self.set_mode_callback = GUIEventClient(SetMode, "mavros/set_mode",
+                                                self.command_response_signal,
+                                                expected_namespace='/tether')
+
+        self.mavros_subscription = GUIEventSubscriber(
+            VehicleState,
+            'vehicle_state_event',
+            self.vehicle_state_signal,
+        )
+
+        self.vehicle_state_signal.connect(self.vehicle_state_callback)
+
+    def enable_clicked(self) -> None:
+        """Send a mavros service request to enable depth hold
+        """
+        self.set_mode_callback.send_request_async(ENABLE_REQUEST)
+
+    def disable_clicked(self) -> None:
+        """Send a mavros service request to disable depth hold
+        """
+        self.set_mode_callback.send_request_async(DISABLE_REQUEST)
+
+    @pyqtSlot(SetMode.Response)
+    def response_callback(self, res: SetMode.Response) -> None:
+        """If the service call failed, send a ros warning
+
+        Parameters
+        ----------
+        res : SetMode.Response
+            The mavros service response
+        """
+        if not res.mode_sent:
+            self.set_mode_callback.get_logger().warn("Failed to set pixhawk mode.")
+
+    @pyqtSlot(VehicleState)
+    def vehicle_state_callback(self, msg: VehicleState) -> None:
+        """Update the button styles to show the status of the pixhawk
+
+        Parameters
+        ----------
+        msg : VehicleState
+            The mavros vehicle state message
+        """
+        if msg.pixhawk_connected:
+            if msg.depth_hold_enabled:
+                self.enable_button.set_on()
+                self.disable_button.remove_state()
+            else:
+                self.enable_button.remove_state()
+                self.disable_button.set_off()
+        else:
+            self.enable_button.set_inactive()
+            self.disable_button.set_inactive()

src/surface/gui/launch/operator_launch.py

@@ -18,6 +18,7 @@ def generate_launch_description() -> LaunchDescription:
                     ("/surface/gui/temperature", "/tether/temperature"),
                     ("/surface/gui/vehicle_state_event", "/surface/vehicle_state_event"),
                     ("/surface/gui/mavros/cmd/arming", "/tether/mavros/cmd/arming"),
+                    ("/surface/gui/mavros/set_mode", "/tether/mavros/set_mode"),
                     ("/surface/gui/ip_address", "/tether/ip_address"),
                     ("/surface/gui/flooding", "/tether/flooding")],
         emulate_tty=True,

src/surface/vehicle_manager/vehicle_manager/connection_manager_node.py

@@ -18,6 +18,7 @@ class VehicleState():
     pi_connected: bool = False
     pixhawk_connected: bool = False
     armed: bool = False
+    depth_hold_enabled: bool = False
 
 
 class VehicleManagerNode(Node):
@@ -55,14 +56,16 @@ def publish_state(self, state: VehicleState) -> None:
             VehicleStateMsg(
                 pi_connected=state.pi_connected,
                 pixhawk_connected=state.pixhawk_connected,
-                armed=state.armed
+                armed=state.armed,
+                depth_hold_enabled=state.depth_hold_enabled,
             )
         )
 
     def mavros_callback(self, msg: State) -> None:
         new_state = VehicleState(pi_connected=True,
                                  pixhawk_connected=msg.connected,
-                                 armed=msg.armed)
+                                 armed=msg.armed,
+                                 depth_hold_enabled=msg.mode == "ALT_HOLD")
 
         if new_state != self.vehicle_state:
             self.publish_state(new_state)
@@ -80,6 +83,11 @@ def mavros_callback(self, msg: State) -> None:
             elif not self.vehicle_state.armed and new_state.armed:
                 self.get_logger().info("Pixhawk armed")
 
+            if self.vehicle_state.depth_hold_enabled and not new_state.depth_hold_enabled:
+                self.get_logger().info("Depth Hold Disabled")
+            elif not self.vehicle_state.depth_hold_enabled and new_state.depth_hold_enabled:
+                self.get_logger().info("Depth Hold Enabled")
+
             self.vehicle_state = new_state
 
     def heartbeat_callback(self, _: Heartbeat) -> None: