Merge pull request #169 from CWRUbotix/luxonis-stoppable-stream

Luxonis stoppable stream

Author: benjaminwp18

.gitignore

@@ -1,3 +1,6 @@
+# Luxonis cam
+.cache/
+
 # Python
 __pycache__/
 *.py[cod]

src/rov_msgs/CMakeLists.txt

@@ -27,7 +27,7 @@ find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Manip.msg"
-  "msg/CameraControllerSwitch.msg"
+  "msg/VideoWidgetSwitch.msg"
   "msg/Flooding.msg"
   "msg/Temperature.msg"
   "msg/IPAddress.msg"

src/rov_msgs/README.md

@@ -6,7 +6,7 @@ This is the package for all our msg, srv, and action definitions.
 
 ## Msg files
 
-[CameraControllerSwitch.msg](msg/CameraControllerSwitch.msg)
+[VideoWidgetSwitch.msg](msg/VideoWidgetSwitch.msg)
 
 Used for toggling through camera feeds in the pilot gui.
 

src/rov_msgs/msg/CameraControllerSwitch.msg

@@ -0,0 +1,4 @@
+# True interprets index as a relative change in cam index from the current index
+# False treats it as the absolute index of the new stream
+bool relative
+int8 index

src/rov_msgs/msg/VideoWidgetSwitch.msg

@@ -1,6 +1,13 @@
-uint8 FRONT = 0
-uint8 DOWN = 1
-uint8 DUAL = 2
+uint8 FLIR_FRONT = 0
+uint8 FLIR_DOWN = 1
+
+uint8 LUX_LEFT = 10
+uint8 LUX_RIGHT = 11
+
+uint8 LUX_LEFT_RECT = 20
+uint8 LUX_RIGHT_RECT = 21
+uint8 LUX_DISPARITY = 22
+uint8 LUX_DEPTH = 23
 
 bool on
 uint8 cam

src/rov_msgs/srv/CameraManage.srv

@@ -9,7 +9,7 @@
 from rclpy.node import Node
 from rclpy.qos import qos_profile_system_default
 
-from rov_msgs.msg import Heartbeat, Manip, VehicleState
+from rov_msgs.msg import Heartbeat, Manip, VehicleState, VideoWidgetSwitch
 from rov_msgs.srv import VehicleArming
 
 os.environ['MAVLINK20'] = '1'  # Force mavlink 2.0 for pymavlink
@@ -159,6 +159,10 @@ def __init__(self) -> None:
             VehicleArming, 'arming', callback=self.arming_service_callback
         )
 
+        self.right_stream_switch_publisher = self.create_publisher(
+            VideoWidgetSwitch, 'switch_right_stream', qos_profile_system_default
+        )
+
         self.invert_controls = False
 
         # Unix timestamp of the last mavlink heartbeat from the pi
@@ -178,6 +182,7 @@ def controller_callback(self, joy_state: JoystickState) -> None:
         joy_state : JoystickState
             The current state of the joystick buttons and axes
         """
+        # MARK: CTRLR CALLBACK
         self.process_arming_buttons(joy_state)
         self.send_mavlink_control(joy_state)
         self.manip_callback(joy_state)
@@ -197,6 +202,7 @@ def joystick_map(raw: float) -> float:
         float
             The output of the mapping, between -1 and 1
         """
+        # MARK: JOY MAP
         return math.copysign(math.fabs(raw) ** JOY_MAP_STRENGTH, raw) * GLOBAL_THROTTLE
 
     def send_mavlink_control(self, joy_state: JoystickState) -> None:
@@ -207,6 +213,7 @@ def send_mavlink_control(self, joy_state: JoystickState) -> None:
         joy_state : JoystickState
             The state of the joystick buttons and axes
         """
+        # MARK: SEND MAV CTRL
         axes = joy_state.axes
         buttons = joy_state.buttons
 
@@ -242,6 +249,7 @@ def manip_callback(self, joy_state: JoystickState) -> None:
         joy_state : JoystickState
             The state of the joystick buttons and axes
         """
+        # MARK: MANIP CALLBACK
         buttons = joy_state.buttons
         for button_id, manip_button in self.manip_buttons.items():
             just_pressed = buttons[button_id] == PRESSED
@@ -261,6 +269,7 @@ def set_armed(self, *, arm: bool) -> None:
         arm : bool
             Whether the vehicle should be armed (True) or disarmed (False)
         """
+        # MARK: SET ARMED
         self.mavlink.mav.command_long_send(
             self.mavlink.target_system,
             self.mavlink.target_component,
@@ -292,6 +301,7 @@ def arming_service_callback(
         VehicleArming.Response
             The filled ROS service response
         """
+        # MARK: ARM CALLBACK
         self.set_armed(arm=request.arm)
 
         response.message_sent = True
@@ -305,6 +315,7 @@ def process_arming_buttons(self, joy_state: JoystickState) -> None:
         joy_state : JoystickState
             The state of the joystick buttons and axes
         """
+        # MARK: PROC ARM BUTTONS
         buttons = joy_state.buttons
 
         if buttons[self.profile.disarm_button] == PRESSED:
@@ -322,19 +333,21 @@ def process_camera_buttons(self, joy_state: JoystickState) -> None:
         joy_state : JoystickState
             The state of the joystick buttons and axess
         """
+        # MARK: PROC CAM BUTTONS
         # Camera switching uses the DPAD, currently not remapable with the controller profile system
         # because DPADs are presented as axes not buttons and using any other axis is non-sensible
         if joy_state.buttons[self.profile.cam_front_button]:
-            # TODO: Message camera manager and gui to swap cameras
+            self.right_stream_switch_publisher.publish(VideoWidgetSwitch(relative=False, index=0))
             self.invert_controls = False
         elif joy_state.buttons[self.profile.cam_back_button]:
-            # TODO: Message camera manager and gui to swap cameras
+            self.right_stream_switch_publisher.publish(VideoWidgetSwitch(relative=False, index=1))
             self.invert_controls = True
 
     def poll_mavlink(self) -> None:
         """Check for incoming mavlink messages from the vehicle and send state updates if
         the vehicle state has changed.
         """
+        # MARK: POLL MAVLINK
         new_state = self.poll_mavlink_for_new_state()
 
         # Check pi timeeout
@@ -364,6 +377,7 @@ def poll_mavlink_for_new_state(self) -> VehicleState:
         VehicleState
             The updated state of the vehicle
         """
+        # MARK: POLL MAV NEW STATE
         new_state = VehicleState(
             pi_connected=self.vehicle_state.pi_connected,
             ardusub_connected=self.vehicle_state.ardusub_connected,
@@ -404,6 +418,7 @@ def pi_heartbeat_callback(self, _: Heartbeat) -> None:
         _ : Heartbeat
             The heartbeat message
         """
+        # MARK: PI HEARTBEAT
         self.last_pi_heartbeat = time.time()
 
         if not self.vehicle_state.pi_connected:
@@ -415,6 +430,7 @@ def poll_subscribers(self) -> None:
         """Check for ros subsribers to our vehicle state topic, and send a state update whenever
         a new one subscribes.
         """
+        # MARK: POLL SUBSCRIBERS
         # Whenever a node subscribes to vehicle state updates, send the current state
         subscriber_count = self.state_publisher.get_subscription_count()
         if subscriber_count > self.last_state_subscriber_count:
@@ -424,6 +440,7 @@ def poll_subscribers(self) -> None:
 
     def handle_pygame_events(self) -> None:
         """Poll pygame for joystick connection and disconnection events."""
+        # MARK: HANDLE PYGAME
         for event in pygame.event.get():
             if event.type == pygame.JOYDEVICEADDED:
                 new_joy = joystick.Joystick(event.device_index)
@@ -445,6 +462,7 @@ def handle_pygame_events(self) -> None:
 
     def poll_joystick(self) -> None:
         """Read the current state of the joystick and send a mavlink message."""
+        # MARK: POLL JOYSTICK
         self.handle_pygame_events()
 
         if self.current_joystick_id is not None:

src/surface/flight_control/flight_control/mavlink_control_node.py

@@ -121,7 +121,7 @@ def __connect_to_service(self, client: Client, timeout: float) -> None:
             )
 
     def send_request_multithreaded(
-        self, client: Client, request: SrvTypeRequest, signal: pyqtBoundSignal
+        self, client: Client, request: SrvTypeRequest, signal: pyqtBoundSignal | None = None
     ) -> None:
         """Send a request from the given client on a separate thread.
         Emit the result to the given signal.
@@ -137,7 +137,9 @@ def send_request_multithreaded(
         """
 
         def wrapper(request: SrvTypeRequest) -> None:
-            signal.emit(client.call(request))
+            response: SrvTypeRequest | None = client.call(request)
+            if signal is not None:
+                signal.emit(response)
 
         Thread(
             target=wrapper,

src/surface/gui/gui/gui_node.py

@@ -7,7 +7,6 @@
 from gui.widgets.ip_widget import IPWidget
 from gui.widgets.logger import Logger
 from gui.widgets.tabs.general_debug_tab import GeneralDebugTab
-from gui.widgets.task_selector import TaskSelector
 from gui.widgets.temperature import TemperatureSensor
 from gui.widgets.timer import InteractiveTimer
 
@@ -41,7 +40,6 @@ def __init__(self) -> None:
         right_pane.addWidget(TemperatureSensor())
         right_pane.addWidget(IPWidget())
         right_pane.addStretch()
-        right_pane.addWidget(TaskSelector())
 
         # Add tabs to root
         root_layout = QVBoxLayout()