Merge pull request #172 from cwruRobotics/manual-control-cleanup

cleanup

src/surface/flight_control/flight_control/manual_control_node.py

@@ -6,41 +6,42 @@
 from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default
 from sensor_msgs.msg import Joy
 
-from rov_msgs.msg import CameraControllerSwitch, Manip, ValveManip
+from rov_msgs.msg import (CameraControllerSwitch, Manip, PixhawkInstruction,
+                          ValveManip)
 
-from rov_msgs.msg import PixhawkInstruction
+UNPRESSED = 0
+PRESSED = 1
 
 # Button meanings for PS5 Control might be different for others
-X_BUTTON:        int = 0  # Manipulator 0
-O_BUTTON:        int = 1  # Manipulator 1
-TRI_BUTTON:      int = 2  # Manipulator 2
-SQUARE_BUTTON:   int = 3  # Manipulator 3
-L1:              int = 4
-R1:              int = 5
-L2:              int = 6
-R2:              int = 7
-PAIRING_BUTTON:  int = 8
-MENU:            int = 9
-PS_BUTTON:       int = 10
-LJOYPRESS:       int = 11
-RJOYPRESS:       int = 12
+X_BUTTON = 0  # Manipulator 0
+O_BUTTON = 1  # Manipulator 1
+TRI_BUTTON = 2  # Manipulator 2
+SQUARE_BUTTON = 3  # Manipulator 3
+L1 = 4
+R1 = 5
+L2 = 6
+R2 = 7
+PAIRING_BUTTON = 8
+MENU = 9
+PS_BUTTON = 10
+LJOYPRESS = 11
+RJOYPRESS = 12
 # Joystick Directions 1 is up/left -1 is down/right
 # X is forward/backward Y is left/right
 # L2 and R2 1 is not pressed and -1 is pressed
-LJOYX:           int = 0
-LJOYY:           int = 1
-L2PRESS_PERCENT: int = 2
-RJOYX:           int = 3
-RJOYY:           int = 4
-R2PRESS_PERCENT: int = 5
-DPADHOR:         int = 6
-DPADVERT:        int = 7
+LJOYX = 0
+LJOYY = 1
+L2PRESS_PERCENT = 2
+RJOYX = 3
+RJOYY = 4
+R2PRESS_PERCENT = 5
+DPADHOR = 6
+DPADVERT = 7
 
 
 class ManualControlNode(Node):
     def __init__(self) -> None:
-        super().__init__('manual_control_node',
-                         parameter_overrides=[])
+        super().__init__('manual_control_node')
 
         self.rc_pub = self.create_publisher(
             PixhawkInstruction,
@@ -83,6 +84,7 @@ def __init__(self) -> None:
 
         self.seen_left_cam = False
         self.seen_right_cam = False
+        self.valve_manip_state = False
 
     def controller_callback(self, msg: Joy) -> None:
         self.joystick_to_pixhawk(msg)
@@ -109,44 +111,42 @@ def manip_callback(self, msg: Joy) -> None:
         buttons: MutableSequence[int] = msg.buttons
 
         for button_id, manip_button in self.manip_buttons.items():
-            just_pressed: bool = False
-
-            if buttons[button_id] == 1:
-                just_pressed = True
+            just_pressed = buttons[button_id] == PRESSED
 
             if manip_button.last_button_state is False and just_pressed:
-                new_manip_state: bool = not manip_button.is_active
+                new_manip_state = not manip_button.is_active
                 manip_button.is_active = new_manip_state
 
-                log_msg: str = f"manip_id= {manip_button.claw}, manip_active= {new_manip_state}"
-                self.get_logger().info(log_msg)
-
-                manip_msg: Manip = Manip(manip_id=manip_button.claw,
-                                         activated=manip_button.is_active)
+                manip_msg = Manip(manip_id=manip_button.claw,
+                                  activated=manip_button.is_active)
                 self.manip_publisher.publish(manip_msg)
-
             manip_button.last_button_state = just_pressed
 
     def valve_manip_callback(self, msg: Joy) -> None:
-        if msg.buttons[TRI_BUTTON] == 1:
+        tri_pressed = msg.buttons[TRI_BUTTON] == PRESSED
+        square_pressed = msg.buttons[SQUARE_BUTTON] == PRESSED
+        if tri_pressed and not self.valve_manip_state:
             self.valve_manip.publish(ValveManip(active=True, pwm=ValveManip.MAX_PWM))
-        elif msg.buttons[SQUARE_BUTTON] == 1:
+            self.valve_manip_state = True
+        elif square_pressed and not self.valve_manip_state:
             self.valve_manip.publish(ValveManip(active=True, pwm=ValveManip.MIN_PWM))
-        else:
+            self.valve_manip_state = True
+        elif self.valve_manip_state and not tri_pressed and not square_pressed:
             self.valve_manip.publish(ValveManip(active=False))
+            self.valve_manip_state = False
 
     def camera_toggle(self, msg: Joy) -> None:
         """Cycles through connected cameras on pilot GUI using menu and pairing buttons."""
         buttons: MutableSequence[int] = msg.buttons
 
-        if buttons[MENU] == 1:
+        if buttons[MENU] == PRESSED:
             self.seen_right_cam = True
-        elif buttons[PAIRING_BUTTON] == 1:
+        elif buttons[PAIRING_BUTTON] == PRESSED:
             self.seen_left_cam = True
-        elif buttons[MENU] == 0 and self.seen_right_cam:
+        elif buttons[MENU] == UNPRESSED and self.seen_right_cam:
             self.seen_right_cam = False
             self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=True))
-        elif buttons[PAIRING_BUTTON] == 0 and self.seen_left_cam:
+        elif buttons[PAIRING_BUTTON] == UNPRESSED and self.seen_left_cam:
             self.seen_left_cam = False
             self.camera_toggle_publisher.publish(CameraControllerSwitch(toggle_right=False))
 

src/surface/rov_flir/launch/flir_launch.py

@@ -52,6 +52,7 @@ def generate_launch_description() -> LaunchDescription:
         package='spinnaker_camera_driver',
         executable='camera_driver_node',
         name='front_cam',
+        exec_name='front_cam',
         emulate_tty=True,
         output='screen',
         parameters=[Parameter('serial_number', '23473577'),
@@ -64,6 +65,7 @@ def generate_launch_description() -> LaunchDescription:
         package='spinnaker_camera_driver',
         executable='camera_driver_node',
         name='bottom_cam',
+        exec_name='bottom_cam',
         emulate_tty=True,
         output='screen',
         parameters=[Parameter('serial_number', '23473566'),