Merge pull request #182 from cwruRobotics/remove-switchable-video

Remove switchable video

src/surface/flight_control/flight_control/manual_control_node.py

@@ -1,10 +1,12 @@
 from collections.abc import MutableSequence
+from enum import IntEnum
 
 import rclpy
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default
 from sensor_msgs.msg import Joy
+from mavros_msgs.srv import CommandBool
 
 from rov_msgs.msg import (CameraControllerSwitch, Manip, PixhawkInstruction,
                           ValveManip)
@@ -38,11 +40,24 @@
 DPADHOR = 6
 DPADVERT = 7
 
+ARMING_SERVICE_TIMEOUT = 3.0
+ARM_MESSAGE = CommandBool.Request(value=True)
+DISARM_MESSAGE = CommandBool.Request(value=False)
+
+CONTROLLER_MODE_PARAM = "controller_mode"
+
+
+class ControllerMode(IntEnum):
+    ARM = 0
+    TOGGLE_CAMERAS = 1
+
 
 class ManualControlNode(Node):
     def __init__(self) -> None:
         super().__init__('manual_control_node')
 
+        mode_param = self.declare_parameter(CONTROLLER_MODE_PARAM, value=ControllerMode.ARM)
+
         self.rc_pub = self.create_publisher(
             PixhawkInstruction,
             'uninverted_pixhawk_control',
@@ -70,12 +85,20 @@ def __init__(self) -> None:
             qos_profile_system_default
         )
 
-        # Cameras
-        self.camera_toggle_publisher = self.create_publisher(
-            CameraControllerSwitch,
-            "camera_switch",
-            qos_profile_system_default
-        )
+        controller_mode = ControllerMode(mode_param.value)
+
+        if controller_mode is ControllerMode.TOGGLE_CAMERAS:
+            # Control camera switching
+            self.misc_controls_callback = self.toggle_cameras
+            self.camera_toggle_publisher = self.create_publisher(
+                CameraControllerSwitch,
+                "camera_switch",
+                qos_profile_system_default
+            )
+        else:
+            self.misc_controls_callback = self.set_arming
+            # Control arming
+            self.arm_client = self.create_client(CommandBool, "mavros/cmd/arming")
 
         self.manip_buttons: dict[int, ManipButton] = {
             X_BUTTON: ManipButton("left"),
@@ -90,7 +113,7 @@ def controller_callback(self, msg: Joy) -> None:
         self.joystick_to_pixhawk(msg)
         self.valve_manip_callback(msg)
         self.manip_callback(msg)
-        self.camera_toggle(msg)
+        self.misc_controls_callback(msg)
 
     def joystick_to_pixhawk(self, msg: Joy) -> None:
         axes: MutableSequence[float] = msg.axes
@@ -136,7 +159,7 @@ def valve_manip_callback(self, msg: Joy) -> None:
             self.valve_manip.publish(ValveManip(active=False))
             self.valve_manip_state = False
 
-    def camera_toggle(self, msg: Joy) -> None:
+    def toggle_cameras(self, msg: Joy) -> None:
         """Cycles through connected cameras on pilot GUI using menu and pairing buttons."""
         buttons: MutableSequence[int] = msg.buttons
 
@@ -151,6 +174,15 @@ def camera_toggle(self, msg: Joy) -> None:
             self.seen_left_cam = False
             self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=False))
 
+    def set_arming(self, msg: Joy) -> None:
+        """Set the arming state using the menu and pairing buttons."""
+        buttons: MutableSequence[int] = msg.buttons
+
+        if buttons[MENU] == PRESSED:
+            self.arm_client.call_async(ARM_MESSAGE)
+        elif buttons[PAIRING_BUTTON] == PRESSED:
+            self.arm_client.call_async(DISARM_MESSAGE)
+
 
 class ManipButton:
     def __init__(self, claw: str) -> None:

src/surface/flight_control/launch/flight_control_launch.py

@@ -1,13 +1,18 @@
 from launch.launch_description import LaunchDescription
 from launch_ros.actions import Node
+from launch.substitutions.launch_configuration import LaunchConfiguration
 
 
 def generate_launch_description() -> LaunchDescription:
     manual_control_node = Node(
         package='flight_control',
         executable='manual_control_node',
+        parameters=[
+            {"controller_mode": LaunchConfiguration('controller_mode', default=0)}
+        ],
         remappings=[('/surface/manipulator_control', '/tether/manipulator_control'),
-                    ('/surface/valve_manipulator', '/tether/valve_manipulator')],
+                    ('/surface/valve_manipulator', '/tether/valve_manipulator'),
+                    ('/surface/mavros/cmd/arming', '/tether/mavros/cmd/arming')],
         emulate_tty=True,
         output='screen'
     )

src/surface/gui/gui/pilot_app.py

@@ -3,7 +3,7 @@
 from gui.widgets.timer import TimerDisplay
 from gui.widgets.flood_warning import FloodWarning
 from gui.widgets.video_widget import (CameraDescription, CameraType,
-                                      SwitchableVideoWidget, VideoWidget)
+                                      VideoWidget)
 from gui.widgets.livestream_header import LivestreamHeader
 
 from PyQt6.QtCore import Qt
@@ -13,7 +13,6 @@
 
 FRONT_CAM_TOPIC = 'front_cam/image_raw'
 BOTTOM_CAM_TOPIC = 'bottom_cam/image_raw'
-DEPTH_CAM_TOPIC = 'depth_cam/image_raw'
 
 
 def make_bottom_bar() -> QHBoxLayout:
@@ -46,11 +45,9 @@ def __init__(self) -> None:
         if simulation_param.value:
             front_cam_type = CameraType.SIMULATION
             bottom_cam_type = CameraType.SIMULATION
-            depth_cam_type = CameraType.SIMULATION
         else:
             front_cam_type = CameraType.ETHERNET
             bottom_cam_type = CameraType.ETHERNET
-            depth_cam_type = CameraType.DEPTH
 
         if gui_param.value == 'pilot':
             self.setWindowTitle('Pilot GUI - CWRUbotix ROV 2024')
@@ -67,17 +64,11 @@ def __init__(self) -> None:
                 "Bottom Camera",
                 1280, 720
             )
-            depth_cam_description = CameraDescription(
-                depth_cam_type,
-                DEPTH_CAM_TOPIC,
-                "Depth Cam",
-                640, 360
-            )
 
             main_layout.addWidget(VideoWidget(front_cam_description),
                                   alignment=Qt.AlignmentFlag.AlignHCenter)
             main_layout.addWidget(
-                SwitchableVideoWidget([bottom_cam_description, depth_cam_description]),
+                VideoWidget(bottom_cam_description),
                 alignment=Qt.AlignmentFlag.AlignHCenter
             )
 
@@ -137,19 +128,13 @@ def __init__(self) -> None:
                 "Bottom Camera",
                 721, 541
             )
-            depth_cam_description = CameraDescription(
-                depth_cam_type,
-                DEPTH_CAM_TOPIC,
-                "Depth Cam",
-                640, 360
-            )
 
             video_layout = QHBoxLayout()
 
             video_layout.addWidget(VideoWidget(front_cam_description),
                                    alignment=Qt.AlignmentFlag.AlignHCenter)
             video_layout.addWidget(
-                SwitchableVideoWidget([bottom_cam_description, depth_cam_description]),
+                VideoWidget(bottom_cam_description),
                 alignment=Qt.AlignmentFlag.AlignHCenter
             )
 

src/surface/gui/gui/widgets/arm.py

@@ -47,8 +47,8 @@ def __init__(self) -> None:
         self.arm_button.clicked.connect(self.arm_clicked)
         self.disarm_button.clicked.connect(self.disarm_clicked)
 
-        layout.addWidget(self.arm_button)
         layout.addWidget(self.disarm_button)
+        layout.addWidget(self.arm_button)
 
         self.command_response_signal.connect(self.arm_status)