From 9aa89fad7dc50b3f7fa8c90afbb95f5852edb957 Mon Sep 17 00:00:00 2001 From: NoahMollerstuen Date: Wed, 19 Jun 2024 16:19:56 -0400 Subject: [PATCH 1/4] Add depth hold --- .../launch/mavros_launch.py | 1 + src/rov_msgs/msg/VehicleState.msg | 3 +- src/surface/gui/gui/operator_app.py | 2 + src/surface/gui/gui/widgets/depth_hold.py | 95 +++++++++++++++++++ src/surface/gui/launch/operator_launch.py | 1 + .../connection_manager_node.py | 12 ++- 6 files changed, 111 insertions(+), 3 deletions(-) create mode 100644 src/surface/gui/gui/widgets/depth_hold.py 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..f590fafc 100644 --- a/src/surface/gui/gui/operator_app.py +++ b/src/surface/gui/gui/operator_app.py @@ -8,6 +8,7 @@ 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,6 +41,7 @@ def __init__(self) -> None: right_pane.addWidget(TemperatureSensor()) right_pane.addWidget(IPWidget()) right_pane.addStretch() + right_pane.addWidget(DepthHold()) right_pane.addWidget(TaskSelector()) # Add tabs to root 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..0d57f28a --- /dev/null +++ b/src/surface/gui/gui/widgets/depth_hold.py @@ -0,0 +1,95 @@ +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 + + +class DepthHold(QWidget): + """Widget for setting the pixhawk's stabilization mode.""" + + 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; }' + + 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(self.BUTTON_WIDTH) + self.disable_button.setMinimumWidth(self.BUTTON_WIDTH) + + self.enable_button.setMinimumHeight(self.BUTTON_HEIGHT) + self.disable_button.setMinimumHeight(self.BUTTON_HEIGHT) + + self.enable_button.setStyleSheet(self.BUTTON_STYLESHEET) + self.disable_button.setStyleSheet(self.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.enable_button) + h_layout.addWidget(self.disable_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: + self.set_mode_callback.send_request_async(self.ENABLE_REQUEST) + + def disable_clicked(self) -> None: + self.set_mode_callback.send_request_async(self.DISABLE_REQUEST) + + @pyqtSlot(SetMode.Response) + def response_callback(self, res: SetMode.Response) -> None: + 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: + 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: From d82f91414db1b7acf77d40c20c6721164a4e358e Mon Sep 17 00:00:00 2001 From: NoahMollerstuen Date: Wed, 19 Jun 2024 16:34:04 -0400 Subject: [PATCH 2/4] Update operator gui --- src/surface/gui/gui/operator_app.py | 6 ++++-- src/surface/gui/gui/widgets/depth_hold.py | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/surface/gui/gui/operator_app.py b/src/surface/gui/gui/operator_app.py index f590fafc..57594067 100644 --- a/src/surface/gui/gui/operator_app.py +++ b/src/surface/gui/gui/operator_app.py @@ -3,7 +3,8 @@ 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 @@ -42,7 +43,8 @@ def __init__(self) -> None: right_pane.addWidget(IPWidget()) right_pane.addStretch() right_pane.addWidget(DepthHold()) - right_pane.addWidget(TaskSelector()) + 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 index 0d57f28a..c89a8c7d 100644 --- a/src/surface/gui/gui/widgets/depth_hold.py +++ b/src/surface/gui/gui/widgets/depth_hold.py @@ -51,8 +51,8 @@ def __init__(self) -> None: self.enable_button.clicked.connect(self.enable_clicked) self.disable_button.clicked.connect(self.disable_clicked) - h_layout.addWidget(self.enable_button) h_layout.addWidget(self.disable_button) + h_layout.addWidget(self.enable_button) layout.addLayout(h_layout) From b41ed0cc082e19f91c83b4a03bab808c1de848ec Mon Sep 17 00:00:00 2001 From: NoahMollerstuen Date: Wed, 19 Jun 2024 16:34:23 -0400 Subject: [PATCH 3/4] Move constants to globals --- src/surface/gui/gui/widgets/depth_hold.py | 29 ++++++++++++----------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/surface/gui/gui/widgets/depth_hold.py b/src/surface/gui/gui/widgets/depth_hold.py index c89a8c7d..a4df0d43 100644 --- a/src/surface/gui/gui/widgets/depth_hold.py +++ b/src/surface/gui/gui/widgets/depth_hold.py @@ -8,15 +8,16 @@ 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.""" - 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; }' - command_response_signal = pyqtSignal(SetMode.Response) vehicle_state_signal = pyqtSignal(VehicleState) @@ -37,14 +38,14 @@ def __init__(self) -> None: self.enable_button.setText("Enable") self.disable_button.setText("Disable") - self.enable_button.setMinimumWidth(self.BUTTON_WIDTH) - self.disable_button.setMinimumWidth(self.BUTTON_WIDTH) + self.enable_button.setMinimumWidth(BUTTON_WIDTH) + self.disable_button.setMinimumWidth(BUTTON_WIDTH) - self.enable_button.setMinimumHeight(self.BUTTON_HEIGHT) - self.disable_button.setMinimumHeight(self.BUTTON_HEIGHT) + self.enable_button.setMinimumHeight(BUTTON_HEIGHT) + self.disable_button.setMinimumHeight(BUTTON_HEIGHT) - self.enable_button.setStyleSheet(self.BUTTON_STYLESHEET) - self.disable_button.setStyleSheet(self.BUTTON_STYLESHEET) + self.enable_button.setStyleSheet(BUTTON_STYLESHEET) + self.disable_button.setStyleSheet(BUTTON_STYLESHEET) self.enable_button.set_inactive() self.disable_button.set_inactive() @@ -71,10 +72,10 @@ def __init__(self) -> None: self.vehicle_state_signal.connect(self.vehicle_state_callback) def enable_clicked(self) -> None: - self.set_mode_callback.send_request_async(self.ENABLE_REQUEST) + self.set_mode_callback.send_request_async(ENABLE_REQUEST) def disable_clicked(self) -> None: - self.set_mode_callback.send_request_async(self.DISABLE_REQUEST) + self.set_mode_callback.send_request_async(DISABLE_REQUEST) @pyqtSlot(SetMode.Response) def response_callback(self, res: SetMode.Response) -> None: From 31f8da28439c3ed27e27bab3977a104ecf63991b Mon Sep 17 00:00:00 2001 From: NoahMollerstuen Date: Wed, 19 Jun 2024 16:44:10 -0400 Subject: [PATCH 4/4] Add docstrings --- src/surface/gui/gui/widgets/depth_hold.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/surface/gui/gui/widgets/depth_hold.py b/src/surface/gui/gui/widgets/depth_hold.py index a4df0d43..c688be1c 100644 --- a/src/surface/gui/gui/widgets/depth_hold.py +++ b/src/surface/gui/gui/widgets/depth_hold.py @@ -72,18 +72,36 @@ def __init__(self) -> None: 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()