Merge pull request #119 from CWRUbotix/manual-control

Replace override_rc_in with manual_control

Author: Bunando

.github/workflows/industrial_ci_action.yml

@@ -27,6 +27,8 @@ jobs:
       CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci)
     steps:
       - uses: actions/checkout@v4 # clone target repository
+        with:
+          submodules: recursive
       - uses: actions/cache@v4 # fetch/store the directory used by ccache before/after the ci run
         with:
           path: ${{ env.CCACHE_DIR }}

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "src/surface/ros2_video_streamer"]
 	path = src/surface/ros2_video_streamer
 	url = [email protected]:CWRUbotix/ros2_video_streamer.git
+[submodule "src/pi/mavros"]
+	path = src/pi/mavros
+	url = https://github.com/mavlink/mavros.git

src/pi/mavros

@@ -26,12 +26,12 @@ def generate_launch_description() -> LaunchDescription:
             # receive a signal from a GCS.
             {'system_id': 255},
             # plugin_allowlist allows which mavros nodes get launched. The default is all of them.
-            {'plugin_allowlist': ['sys_status', 'rc_io', 'command']},
+            {'plugin_allowlist': ['sys_status', 'rc_io', 'command', 'manual_control']},
             {'fcu_url': '/dev/ttyPixhawk'},
         ],
         remappings=[
             ('/pi/mavros/state', '/tether/mavros/state'),
-            ('/pi/mavros/rc/override', '/tether/mavros/rc/override'),
+            ('/pi/mavros/manual_control/send', '/tether/mavros/manual_control/send'),
             ('/pi/mavros/cmd/arming', '/tether/mavros/cmd/arming'),
             ('/pi/mavros/cmd/command', '/tether/mavros/cmd/command'),
         ],

src/pi/pixhawk_communication/launch/mavros_launch.py

@@ -1,7 +1,8 @@
 from collections.abc import Callable
+from typing import Final
 
 import rclpy
-from mavros_msgs.msg import OverrideRCIn
+from mavros_msgs.msg import ManualControl
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.qos import QoSPresetProfiles
@@ -17,9 +18,14 @@
 
 # Range of values Pixhawk takes
 # In microseconds
-ZERO_SPEED = 1500
-MAX_RANGE_SPEED = 400
-RANGE_SPEED = MAX_RANGE_SPEED * SPEED_THROTTLE
+ZERO_SPEED: Final = 0
+Z_ZERO_SPEED: Final = 500
+MAX_RANGE_SPEED: Final = 2000
+Z_MAX_RANGE_SPEED: Final = 1000
+RANGE_SPEED: Final = MAX_RANGE_SPEED * SPEED_THROTTLE
+Z_RANGE_SPEED: Final = Z_MAX_RANGE_SPEED * SPEED_THROTTLE
+
+EXTENSIONS_CODE: Final = 0b00000011
 
 # Channels for RC command
 MAX_CHANNEL = 8
@@ -57,36 +63,39 @@ def __init__(self) -> None:
             QoSPresetProfiles.DEFAULT.value,
         )
 
-        self.rc_pub = self.create_publisher(
-            OverrideRCIn, 'mavros/rc/override', QoSPresetProfiles.DEFAULT.value
+        self.mc_pub = self.create_publisher(
+            ManualControl, 'mavros/manual_control/send', QoSPresetProfiles.DEFAULT.value
         )
 
     @staticmethod
     def apply(msg: PixhawkInstruction, function_to_apply: Callable[[float], float]) -> None:
         """Apply a function to each dimension of this PixhawkInstruction."""
         msg.forward = function_to_apply(msg.forward)
-        msg.vertical = function_to_apply(msg.vertical)
+        msg.vertical = msg.vertical
         msg.lateral = function_to_apply(msg.lateral)
         msg.pitch = function_to_apply(msg.pitch)
         msg.yaw = function_to_apply(msg.yaw)
         msg.roll = function_to_apply(msg.roll)
 
     @staticmethod
-    def to_override_rc_in(msg: PixhawkInstruction) -> OverrideRCIn:
+    def to_manual_control(msg: PixhawkInstruction) -> ManualControl:
         """Convert this PixhawkInstruction to an rc_msg with the appropriate channels array."""
-        rc_msg = OverrideRCIn()
+        mc_msg = ManualControl()
 
         # Maps to PWM
-        MultiplexerNode.apply(msg, lambda value: int(RANGE_SPEED * value) + ZERO_SPEED)
+        MultiplexerNode.apply(msg, lambda value: (RANGE_SPEED * value) + ZERO_SPEED)
 
-        rc_msg.channels[FORWARD_CHANNEL] = msg.forward
-        rc_msg.channels[THROTTLE_CHANNEL] = msg.vertical
-        rc_msg.channels[LATERAL_CHANNEL] = msg.lateral
-        rc_msg.channels[PITCH_CHANNEL] = msg.pitch
-        rc_msg.channels[YAW_CHANNEL] = msg.yaw
-        rc_msg.channels[ROLL_CHANNEL] = msg.roll
+        mc_msg.x = msg.forward
+        mc_msg.z = (
+            Z_RANGE_SPEED * msg.vertical
+        ) + Z_ZERO_SPEED  # To account for different z limits
+        mc_msg.y = msg.lateral
+        mc_msg.r = msg.yaw
+        mc_msg.enabled_extensions = EXTENSIONS_CODE
+        mc_msg.s = msg.pitch
+        mc_msg.t = msg.roll
 
-        return rc_msg
+        return mc_msg
 
     def state_control(
         self, req: AutonomousFlight.Request, res: AutonomousFlight.Response
@@ -114,7 +123,7 @@ def control_callback(self, msg: PixhawkInstruction) -> None:
         else:
             return
 
-        self.rc_pub.publish(msg=self.to_override_rc_in(msg))
+        self.mc_pub.publish(self.to_manual_control(msg))
 
 
 def main() -> None:

src/surface/flight_control/flight_control/multiplexer.py

@@ -35,7 +35,7 @@ def generate_launch_description() -> LaunchDescription:
     multiplexer_node = Node(
         package='flight_control',
         executable='multiplexer_node',
-        remappings=[('/surface/mavros/rc/override', '/tether/mavros/rc/override')],
+        remappings=[('/surface/mavros/manual_control/send', '/tether/mavros/manual_control/send')],
         emulate_tty=True,
         output='screen',
     )

src/surface/flight_control/launch/flight_control_launch.py

@@ -1,13 +1,9 @@
 import rclpy
 from flight_control.manual_control_node import ManualControlNode
 from flight_control.multiplexer import (
-    FORWARD_CHANNEL,
-    LATERAL_CHANNEL,
-    PITCH_CHANNEL,
     RANGE_SPEED,
-    ROLL_CHANNEL,
-    THROTTLE_CHANNEL,
-    YAW_CHANNEL,
+    Z_RANGE_SPEED,
+    Z_ZERO_SPEED,
     ZERO_SPEED,
     MultiplexerNode,
 )
@@ -35,14 +31,14 @@ def test_joystick_profiles() -> None:
         roll=0.92,
     )
 
-    msg = MultiplexerNode.to_override_rc_in(instruction)
+    msg = MultiplexerNode.to_manual_control(instruction)
 
-    assert msg.channels[FORWARD_CHANNEL] == ZERO_SPEED
-    assert msg.channels[THROTTLE_CHANNEL] == (ZERO_SPEED + RANGE_SPEED)
-    assert msg.channels[LATERAL_CHANNEL] == (ZERO_SPEED - RANGE_SPEED)
+    assert msg.x == ZERO_SPEED
+    assert msg.z == (Z_ZERO_SPEED + Z_RANGE_SPEED)
+    assert msg.y == (ZERO_SPEED - RANGE_SPEED)
 
     # 1539 1378
 
-    assert msg.channels[PITCH_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * 0.34)
-    assert msg.channels[YAW_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * -0.6)
-    assert msg.channels[ROLL_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * 0.92)
+    assert msg.s == ZERO_SPEED + int(RANGE_SPEED * 0.34)
+    assert msg.r == ZERO_SPEED + int(RANGE_SPEED * -0.6)
+    assert msg.t == ZERO_SPEED + int(RANGE_SPEED * 0.92)