Simplified flight control

.vscode/easy_build.sh

@@ -1,6 +1,6 @@
 # Done to suppress setup.py install deprecated warnings
 # Can be removed once ROS redoes their python build system
-PYTHONWARNINGS=ignore:::setuptools.command.install,ignore:::setuptools.command.easy_install,ignore:::pkg_resources; export PYTHONWARNINGS
+PYTHONWARNINGS=ignore:::setuptools.command.install,ignore:::setuptools.command.easy_install,ignore:::setuptools.command.develop,ignore:::pkg_resources; export PYTHONWARNINGS
 
 # Stolen from colcon build command in VsCode
 colcon build --symlink-install

src/surface/flight_control/README.md

@@ -2,11 +2,11 @@
 
 ## Overview
 
-This package includes keyboard, PS5 controller, and automatic docking pilot nodes. It abstracts creating motor control messages with `PixhawkInstruction`s.
+This package includes keyboard and PS5 controller nodes.
 
 ## Usage
 
-The PS5 controller (`manual_control_node`) and the auto docking controller (`auto_docking_node`) are both run when the pilot is launched.
+The PS5 controller (`manual_control_node`) is run when the pilot is launched.
 You can run them on their own with:
 
 ```bash
@@ -22,11 +22,7 @@ ros2 launch flight_control keyboard_control_launch.py
 
 ## Launch files
 
-* **flight_control_launch.py:** launches the PS5 controller and readies the auto docking controller
-
-  * **`controller_mode`** Whether the controller arms or toggles cameras; options are 0 (arming), 1 (cameras). Default: 0.
-
-  * **`controller_profile`** Profile controls what buttons do what; value is the profile index. Default: 0.
+* **flight_control_launch.py:** launches the PS5 controller
 
 * **keyboard_control_launch.py:** launches the keyboard controller under the `/surface` namespace
 
@@ -42,40 +38,37 @@ Controls motors, manipulators, and camera switching (if applicable) from the PS5
 
     PS5 controller instructions.
 
-#### Published Topics
+* **`/surface/camera_switch`** ([rov_msgs/msg/CameraControllerSwitch])
 
-* **`/tether/mavros/rc/override`** ([mavros_msgs/msg/OverrideRcIn])
+    Instructions to change which camera should be active. TODO: Remove this if possible after upgrading to FLIR cams.
 
-    The movement instructions for the Pixhawk.
+#### Published Topics
 
 * **`/tether/manipulator_control`** ([rov_msgs/msg/Manip])
 
     Manipulator instructions for the Pi.
 
-* **`/surface/camera_switch`** ([rov_msgs/msg/CameraControllerSwitch])
+* **`/tether/mavros/manual_control/send`** ([mavros_msgs/msg/ManualControl])
 
-    Instructions to change which camera should be active. TODO: Remove this if possible after upgrading to FLIR cams.
-
-### keyboard_control_node
+    The movement instructions for the Pixhawk
 
-Controls motors (only) from the keyboard. Not run by general surface launch files. Run this separately with its launch file to use it.
-This node can publish concurrently with manual control/auto docking.
+#### Services
 
-#### Published Topics
+* **`/tether/mavros/cmd/command`** ([mavros_msgs/srv/CommandLong.srv])
 
-* **`/tether/mavros/rc/override`** ([mavros_msgs/msg/OverrideRcIn])
+    Instructions to set the Valve Manipulator servo
 
-    The movement instructions for the Pixhawk. This node can publish concurrently with manual control.
+* **`/tether/mavros/cmd/arming`** ([mavros_msgs/srv/CommandBool.srv])
 
-### auto_docking_node
+    Instruction to arm the robot
 
-Execute an automatic docking sequence. This node must be "activated" with its service before it will publish movement instructions.
-Once activated, it will publish concurrently with manual control/keyboard control nodes.
+### keyboard_control_node
 
-TODO: actually implement autodocking.
+Controls motors (only) from the keyboard. Not run by general surface launch files. Run this separately with its launch file to use it.
+This node can publish concurrently with manual control/auto docking.
 
-#### Services
+#### Published Topics
 
-* **`/surface/auto_docker_control`** ([rov_msgs/srv/AutonomousFlight.srv])
+* **`/tether/mavros/manual_control/send`** ([mavros_msgs/msg/ManualControl])
 
-    Toggles whether the auto docker is running (sending instructions) and returns the new state.
+    The movement instructions for the Pixhawk

src/surface/flight_control/flight_control/keyboard_control_node.py

@@ -1,11 +1,17 @@
 from typing import TYPE_CHECKING
 
 import rclpy.utilities
+from mavros_msgs.msg import ManualControl
 from pynput.keyboard import Key, KeyCode, Listener
 from rclpy.node import Node
-from rclpy.qos import qos_profile_system_default
+from rclpy.qos import QoSPresetProfiles
 
-from rov_msgs.msg import PixhawkInstruction
+from flight_control.manual_control_utils import (
+    Z_RANGE_SPEED,
+    Z_ZERO_SPEED,
+    apply_function,
+    joystick_map,
+)
 
 if TYPE_CHECKING:
     from rclpy.publisher import Publisher
@@ -57,8 +63,9 @@ class KeyboardListenerNode(Node):
     def __init__(self) -> None:
         super().__init__('keyboard_listener_node', parameter_overrides=[])
 
-        self.rc_pub: Publisher = self.create_publisher(
-            PixhawkInstruction, 'uninverted_pixhawk_control', qos_profile_system_default
+        # Manual Control
+        self.mc_pub: Publisher = self.create_publisher(
+            ManualControl, 'mavros/manual_control/send', QoSPresetProfiles.DEFAULT.value
         )
 
         self.get_logger().info(HELP_MSG)
@@ -112,17 +119,20 @@ def on_release(self, key: Key | KeyCode | None) -> None:
         self.pub_rov_control()
 
     def pub_rov_control(self) -> None:
-        instruction = PixhawkInstruction(
-            pitch=float(self.status[PITCH_UP] - self.status[PITCH_DOWN]),
-            roll=float(self.status[ROLL_LEFT] - self.status[ROLL_RIGHT]),
-            vertical=float(self.status[UP] - self.status[DOWN]),
-            forward=float(self.status[FORWARD] - self.status[BACKWARD]),
-            lateral=float(self.status[LEFT] - self.status[RIGHT]),
-            yaw=float(self.status[YAW_LEFT] - self.status[YAW_RIGHT]),
-            author=PixhawkInstruction.KEYBOARD_CONTROL,
+        instruction = ManualControl(
+            s=float(self.status[PITCH_UP] - self.status[PITCH_DOWN]),
+            t=float(self.status[ROLL_LEFT] - self.status[ROLL_RIGHT]),
+            z=float(self.status[UP] - self.status[DOWN]),
+            x=float(self.status[FORWARD] - self.status[BACKWARD]),
+            y=float(self.status[LEFT] - self.status[RIGHT]),
+            r=float(self.status[YAW_LEFT] - self.status[YAW_RIGHT]),
         )
 
-        self.rc_pub.publish(instruction)
+        # Convert to manual_control values
+        msg = apply_function(instruction, joystick_map)
+        msg.z = Z_RANGE_SPEED * instruction.z + Z_ZERO_SPEED
+
+        self.mc_pub.publish(msg)
 
     def spin(self) -> None:
         with Listener(on_press=self.on_press, on_release=self.on_release) as listener: