Add debug vector send capability

examples/send_debug.py

@@ -0,0 +1,105 @@
+# pymavswarm is an interface for swarm control and interaction
+# Copyright (C) 2022  Grant Phillips
+
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+from __future__ import annotations
+
+import time
+from argparse import ArgumentParser
+from concurrent.futures import Future
+from typing import Any
+
+from pymavswarm import MavSwarm
+
+
+def parse_args() -> Any:
+    """
+    Parse the script arguments.
+
+    :return: argument namespace
+    :rtype: Any
+    """
+    parser = ArgumentParser()
+    parser.add_argument(
+        "port", type=str, help="port to establish a MAVLink connection over"
+    )
+    parser.add_argument("baud", type=int, help="baudrate to establish a connection at")
+    parser.add_argument("name",type=str, help="debug value name")
+    parser.add_argument("data1", type=float, help="debug vector payload1")
+    parser.add_argument("data2", type=float, help="debug vector payload2")
+    parser.add_argument("data3", type=float, help="debug vector payload3")
+    return parser.parse_args()
+
+
+# Define a callback to attach to a future
+def print_message_response_cb(future: Future) -> None:
+    """
+    Print the result of the future.
+
+    :param future: message execution future
+    :type future: Future
+    """
+    responses = future.result()
+
+    if isinstance(responses, list):
+        for response in responses:
+            print(
+                f"Result of {response.message_type} message sent to "
+                f"({response.target_agent_id}): {response.code}"
+            )
+    else:
+        print(
+            f"Result of {responses.message_type} message sent to "
+            f"({responses.target_agent_id}): {responses.code}"
+        )
+
+    return
+
+
+def main() -> None:
+    """
+    Demonstrate how to send a debug vector.
+    """
+    # Parse the script arguments
+    args = parse_args()
+
+    # Create a new MavSwarm instance
+    mavswarm = MavSwarm()
+
+    # Attempt to create a new MAVLink connection
+    if not mavswarm.connect(args.port, args.baud):
+        return
+
+    # Wait for the swarm to auto-register new agents
+    while not list(filter(lambda agent_id: agent_id[1] == 1, mavswarm.agent_ids)):
+        print("Waiting for the system to recognize agents in the network...")
+        time.sleep(0.5)
+
+    # Send a debug message to all agents in the swarm
+    future = mavswarm.send_debug_message(args.name, [args.data1,args.data2,args.data3])
+    future.add_done_callback(print_message_response_cb)
+
+    # Wait for the arm command to complete
+    while not future.done():
+        pass
+
+    # Disconnect from the swarm
+    mavswarm.disconnect()
+
+    return
+
+
+if __name__ == "__main__":
+    main()

pymavswarm/mavswarm.py

@@ -1118,7 +1118,7 @@ def executor(agent_id: AgentID) -> None:
     def send_debug_message(
         self,
         name: str,
-        value: int | float,
+        value: int | float | list,
         agent_ids: AgentID | list[AgentID] | None = None,
         retry: bool = False,
         message_timeout: float = 2.5,
@@ -1133,7 +1133,7 @@ def send_debug_message(
         :param name: debug message name
         :type name: str
         :param value: debug message value
-        :type value: int | float
+        :type value: int | float | list
         :param agent_ids: optional list of target agent IDs, defaults to None
         :type agent_ids: AgentID | list[AgentID] | None,
             optional
@@ -1152,9 +1152,9 @@ def send_debug_message(
         if not isinstance(name, str):
             raise TypeError(f"Invalid name provided. Expected string, got {type(name)}")
 
-        if not isinstance(value, int) and not isinstance(value, float):
+        if not isinstance(value, int) and not isinstance(value, float) and not isinstance(value, list):
             raise TypeError(
-                f"Invalid value provided. Expected an int or a float, got {type(value)}"
+                f"Invalid value provided. Expected an int, float or list, got {type(value)}"
             )
 
         def executor(agent_id: AgentID) -> None:
@@ -1163,15 +1163,24 @@ def executor(agent_id: AgentID) -> None:
                 self._connection.mavlink_connection.target_system = agent_id[0]
                 self._connection.mavlink_connection.target_component = agent_id[1]
 
-                # Send flight mode
+                # Send debug message
                 if isinstance(value, int):
                     self._connection.mavlink_connection.mav.named_value_int_send(
                         int(time.time()), str.encode(name), value
                     )
-                else:
+                elif isinstance(value,float):
                     self._connection.mavlink_connection.mav.named_value_float_send(
                         int(time.time()), str.encode(name), value
                     )
+                elif isinstance(value, list):
+                    if len(value) != 3:
+                        raise ValueError(
+                            f"Invalid number of debug vector elements provided. Expected 3, got {len(value)}"
+                        )
+                    self._connection.mavlink_connection.mav.debug_vect_send(
+                         str.encode(name),int(time.time()), *value
+                    )
+
 
             return