diff --git a/src/pi/pixhawk_communication/launch/mavros_launch.py b/src/pi/pixhawk_communication/launch/mavros_launch.py index 9bbf903d..f34a1c8e 100644 --- a/src/pi/pixhawk_communication/launch/mavros_launch.py +++ b/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'), ] ) diff --git a/src/rov_msgs/msg/VehicleState.msg b/src/rov_msgs/msg/VehicleState.msg index d23c3014..cab7d09f 100644 --- a/src/rov_msgs/msg/VehicleState.msg +++ b/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) \ No newline at end of file +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) \ No newline at end of file diff --git a/src/surface/gui/gui/operator_app.py b/src/surface/gui/gui/operator_app.py index f193f928..57594067 100644 --- a/src/surface/gui/gui/operator_app.py +++ b/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() diff --git a/src/surface/gui/gui/widgets/depth_hold.py b/src/surface/gui/gui/widgets/depth_hold.py new file mode 100644 index 00000000..c688be1c --- /dev/null +++ b/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() diff --git a/src/surface/gui/launch/operator_launch.py b/src/surface/gui/launch/operator_launch.py index 1f07f4a3..cab4506f 100644 --- a/src/surface/gui/launch/operator_launch.py +++ b/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, diff --git a/src/surface/vehicle_manager/vehicle_manager/connection_manager_node.py b/src/surface/vehicle_manager/vehicle_manager/connection_manager_node.py index a1b8ee92..a7cf0242 100644 --- a/src/surface/vehicle_manager/vehicle_manager/connection_manager_node.py +++ b/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: