Merge pull request #714 from loic-fejoz/feature/waypoint

Waypoint creation/move/deletion

Author: ianmcorvidae

examples/waypoint.py

@@ -0,0 +1,55 @@
+"""Program to create and delete waypoint
+   To run:
+   python3 examples/waypoint.py --port /dev/ttyUSB0 create 45 test the_desc_2 '2024-12-18T23:05:23' 48.74 7.35
+   python3 examples/waypoint.py delete 45
+"""
+
+import argparse
+import datetime
+import sys
+
+import meshtastic
+import meshtastic.serial_interface
+
+parser = argparse.ArgumentParser(
+                    prog='waypoint',
+                    description='Create and delete Meshtastic waypoint')
+parser.add_argument('--port', default=None)
+parser.add_argument('--debug', default=False, action='store_true')
+
+subparsers = parser.add_subparsers(dest='cmd')
+parser_delete = subparsers.add_parser('delete', help='Delete a waypoint')
+parser_delete.add_argument('id', help="id of the waypoint")
+
+parser_create = subparsers.add_parser('create', help='Create a new waypoint')
+parser_create.add_argument('id', help="id of the waypoint")
+parser_create.add_argument('name', help="name of the waypoint")
+parser_create.add_argument('description', help="description of the waypoint")
+parser_create.add_argument('expire', help="expiration date of the waypoint as interpreted by datetime.fromisoformat")
+parser_create.add_argument('latitude', help="latitude of the waypoint")
+parser_create.add_argument('longitude', help="longitude of the waypoint")
+
+args = parser.parse_args()
+print(args)
+
+# By default will try to find a meshtastic device,
+# otherwise provide a device path like /dev/ttyUSB0
+if args.debug:
+    d = sys.stderr
+else:
+    d = None
+with meshtastic.serial_interface.SerialInterface(args.port, debugOut=d) as iface:
+    if args.cmd == 'create':
+        p = iface.sendWaypoint(
+            waypoint_id=int(args.id),
+            name=args.name,
+            description=args.description,
+            expire=int(datetime.datetime.fromisoformat(args.expire).timestamp()),
+            latitude=float(args.latitude),
+            longitude=float(args.longitude),
+        )
+    else:
+        p = iface.deleteWaypoint(int(args.id))
+    print(p)
+
+# iface.close()

meshtastic/mesh_interface.py

@@ -5,7 +5,9 @@
 import collections
 import json
 import logging
+import math
 import random
+import secrets
 import sys
 import threading
 import time
@@ -737,6 +739,113 @@ def onResponseTelemetry(self, p: dict):
                     "No response from node. At least firmware 2.1.22 is required on the destination node."
                 )
 
+    def onResponseWaypoint(self, p: dict):
+        """on response for waypoint"""
+        if p["decoded"]["portnum"] == "WAYPOINT_APP":
+            self._acknowledgment.receivedWaypoint = True
+            w = mesh_pb2.Waypoint()
+            w.ParseFromString(p["decoded"]["payload"])
+            print(f"Waypoint received: {w}")
+        elif p["decoded"]["portnum"] == "ROUTING_APP":
+            if p["decoded"]["routing"]["errorReason"] == "NO_RESPONSE":
+                our_exit(
+                    "No response from node. At least firmware 2.1.22 is required on the destination node."
+                )
+
+    def sendWaypoint(
+        self,
+        name,
+        description,
+        expire: int,
+        waypoint_id: Optional[int] = None,
+        latitude: float = 0.0,
+        longitude: float = 0.0,
+        destinationId: Union[int, str] = BROADCAST_ADDR,
+        wantAck: bool = True,
+        wantResponse: bool = False,
+        channelIndex: int = 0,
+    ): # pylint: disable=R0913
+        """
+        Send a waypoint packet to some other node (normally a broadcast)
+
+        Returns the sent packet. The id field will be populated in this packet and
+        can be used to track future message acks/naks.
+        """
+        w = mesh_pb2.Waypoint()
+        w.name = name
+        w.description = description
+        w.expire = expire
+        if waypoint_id is None:
+            # Generate a waypoint's id, NOT a packet ID.
+            # same algorithm as https://github.com/meshtastic/js/blob/715e35d2374276a43ffa93c628e3710875d43907/src/meshDevice.ts#L791
+            seed = secrets.randbits(32)
+            w.id = math.floor(seed * math.pow(2, -32) * 1e9)
+            logging.debug(f"w.id:{w.id}")
+        else:
+            w.id = waypoint_id
+        if latitude != 0.0:
+            w.latitude_i = int(latitude * 1e7)
+            logging.debug(f"w.latitude_i:{w.latitude_i}")
+        if longitude != 0.0:
+            w.longitude_i = int(longitude * 1e7)
+            logging.debug(f"w.longitude_i:{w.longitude_i}")
+
+        if wantResponse:
+            onResponse = self.onResponseWaypoint
+        else:
+            onResponse = None
+
+        d = self.sendData(
+            w,
+            destinationId,
+            portNum=portnums_pb2.PortNum.WAYPOINT_APP,
+            wantAck=wantAck,
+            wantResponse=wantResponse,
+            onResponse=onResponse,
+            channelIndex=channelIndex,
+        )
+        if wantResponse:
+            self.waitForWaypoint()
+        return d
+
+    def deleteWaypoint(
+        self,
+        waypoint_id: int,
+        destinationId: Union[int, str] = BROADCAST_ADDR,
+        wantAck: bool = True,
+        wantResponse: bool = False,
+        channelIndex: int = 0,
+    ):
+        """
+        Send a waypoint deletion packet to some other node (normally a broadcast)
+
+        NB: The id must be the waypoint's id and not the id of the packet creation.
+        
+        Returns the sent packet. The id field will be populated in this packet and
+        can be used to track future message acks/naks.
+        """
+        p = mesh_pb2.Waypoint()
+        p.id = waypoint_id
+        p.expire = 0
+
+        if wantResponse:
+            onResponse = self.onResponseWaypoint
+        else:
+            onResponse = None
+
+        d = self.sendData(
+            p,
+            destinationId,
+            portNum=portnums_pb2.PortNum.WAYPOINT_APP,
+            wantAck=wantAck,
+            wantResponse=wantResponse,
+            onResponse=onResponse,
+            channelIndex=channelIndex,
+        )
+        if wantResponse:
+            self.waitForWaypoint()
+        return d
+
     def _addResponseHandler(
         self,
         requestId: int,
@@ -861,6 +970,12 @@ def waitForPosition(self):
         if not success:
             raise MeshInterface.MeshInterfaceError("Timed out waiting for position")
 
+    def waitForWaypoint(self):
+        """Wait for waypoint"""
+        success = self._timeout.waitForWaypoint(self._acknowledgment)
+        if not success:
+            raise MeshInterface.MeshInterfaceError("Timed out waiting for waypoint")
+
     def getMyNodeInfo(self) -> Optional[Dict]:
         """Get info about my node."""
         if self.myInfo is None or self.nodesByNum is None:

meshtastic/util.py

@@ -254,6 +254,16 @@ def waitForPosition(self, acknowledgment) -> bool:
             time.sleep(self.sleepInterval)
         return False
 
+    def waitForWaypoint(self, acknowledgment) -> bool:
+        """Block until waypoint response is received. Returns True if waypoint response has been received."""
+        self.reset()
+        while time.time() < self.expireTime:
+            if getattr(acknowledgment, "receivedWaypoint", None):
+                acknowledgment.reset()
+                return True
+            time.sleep(self.sleepInterval)
+        return False
+
 class Acknowledgment:
     "A class that records which type of acknowledgment was just received, if any."
 
@@ -265,6 +275,7 @@ def __init__(self) -> None:
         self.receivedTraceRoute = False
         self.receivedTelemetry = False
         self.receivedPosition = False
+        self.receivedWaypoint = False
 
     def reset(self) -> None:
         """reset"""
@@ -274,6 +285,7 @@ def reset(self) -> None:
         self.receivedTraceRoute = False
         self.receivedTelemetry = False
         self.receivedPosition = False
+        self.receivedWaypoint = False
 
 
 class DeferredExecution: