Dev maneuvre meppel

adapters.py

@@ -14,6 +14,7 @@
 import re
 #import RPi.GPIO as GPIO
 from serial import SerialException
+import time
 
 
 class SmoothieAdapter:
@@ -2690,28 +2691,73 @@ def get_last_positions_list(self):
 
         raise NotImplementedError(f"[{self.__class__.__name__}] -> Test without list")
 
-    def _read_from_gps(self):
-        """Returns GPS coordinates of the current position"""
+    def _read_from_gps(self, timeout=3.0):
+        """
+        Returns [latitude, longitude, quality] when a complete GNGGA sentence is received.
+        Returns None if no valid sentence within the timeout period (in seconds).
+        """
+        buffer = ""
+        start_time = time.time()
 
         while True:
+            if time.time() - start_time > timeout:
+                return None
+
             try:
-                read_line = self._serial.readline()
-                if isinstance(read_line, bytes):
-                    data = str(read_line)
-                    # if len(data) == 3:
-                    #    print("None GNGGA or RTCM threads")
-                    if "GNGGA" in data and ",,," not in data:
-                        # bad string with no position data
-                        # print(data)  # debug
-                        data = data.split(",")
-                        lati, longi = self._D2M2(data[2], data[3], data[4], data[5])
-                        point_quality = data[6]
-                        return [lati, longi, point_quality]  # , float(data[11])  # alti
-            except KeyboardInterrupt:
-                raise KeyboardInterrupt
-            except:
+                chunk = self._serial.read(self._serial.in_waiting or 1).decode(errors='ignore')
+                if not chunk:
+                    time.sleep(0.01)
+                    continue
+
+                buffer += chunk
+
+                while "\n" in buffer:
+                    line, buffer = buffer.split("\n", 1)
+                    line = line.strip()
+                    if not line.startswith("$GNGGA"):
+                        continue
+
+                    parts = line.split(",")
+                    if len(parts) < 7 or parts[2] == "":
+                        continue
+
+                    try:
+                        lat, lon = self._D2M2(parts[2], parts[3], parts[4], parts[5])
+                        quality = parts[6]
+                        return [lat, lon, quality]
+                    except Exception as e:
+                        print(f"[GPS] Parsing error: {e} | frame: {line}")
+                        continue
+
+            except Exception as e:
+                print("[GPS] Read error:", e)
                 continue
 
+    # def _read_from_gps(self):
+    #     """Returns GPS coordinates of the current position"""
+
+    #     while True:
+    #         try:
+    #             read_line = self._serial.readline()
+    #             print("read_line",read_line)
+    #             if not read_line:
+    #                 return None
+    #             if isinstance(read_line, bytes):
+    #                 data = str(read_line)
+    #                 # if len(data) == 3:
+    #                 #    print("None GNGGA or RTCM threads")
+    #                 if "GNGGA" in data and ",,," not in data:
+    #                     # bad string with no position data
+    #                     # print(data)  # debug
+    #                     data = data.split(",")
+    #                     lati, longi = self._D2M2(data[2], data[3], data[4], data[5])
+    #                     point_quality = data[6]
+    #                     return [lati, longi, point_quality]  # , float(data[11])  # alti
+    #         except KeyboardInterrupt:
+    #             raise KeyboardInterrupt
+    #         except:
+    #             continue
+
     def _D2M2(self, Lat, NS, Lon, EW):
         """Traduce NMEA format ddmmss to ddmmmm"""
 

check_config.py

@@ -0,0 +1,168 @@
+import os
+import glob
+import datetime
+import shutil
+import pytz
+import pwd
+import grp
+import re
+import ast
+
+def get_latest_default_config_file(pattern="*_defaults.py"):
+    files = glob.glob(pattern)
+    version_pattern = re.compile(r"v(\d+)")  # capture "v123" comme version 123
+    versioned = []
+
+    for f in files:
+        match = version_pattern.search(f)
+        if match:
+            versioned.append((int(match.group(1)), f))
+        else:
+            # Si pas de version trouvée, version = 0
+            versioned.append((0, f))
+
+    if not versioned:
+        return None
+
+    # Trie d’abord par version puis par nom
+    versioned.sort(key=lambda x: (x[0], x[1]))
+    return versioned[-1][1]
+
+def extract_globals_static(path):
+    """Parse Python file and extract top-level variable assignments safely (no execution)."""
+    with open(path, "r") as f:
+        source = f.read()
+
+    try:
+        tree = ast.parse(source, filename=path)
+    except SyntaxError as e:
+        raise Exception(f"Invalid Python syntax in {path}: {e}")
+
+    result = {}
+    for node in tree.body:
+        # On ne garde que les assignations de haut niveau (pas dans une fonction)
+        if isinstance(node, ast.Assign):
+            for target in node.targets:
+                if isinstance(target, ast.Name):
+                    try:
+                        result[target.id] = ast.literal_eval(node.value)
+                    except Exception:
+                        # si la valeur n'est pas un littéral (ex: appel de fonction), on ignore
+                        result[target.id] = None
+    return result
+
+# load config, if failed - copy and load config backups until success or no more backups
+def is_config_empty(config_full_path: str):
+    with open(config_full_path, "r") as config_file:
+        for line in config_file:
+            if line not in ["", "\n"]:
+                return False
+    return True
+
+def validate_config_file(config_directory_path) -> bool:
+    """Validate a Python config file without importing it.
+
+    Checks that the file is non-empty and has valid Python syntax by compiling it.
+    Returns True if valid, False otherwise.
+    """
+    config_full_path = f"{config_directory_path}/config.py"
+    try:
+        # 1️⃣ check empty
+        if is_config_empty(config_full_path):
+            print(f"[{os.path.basename(__file__)}] ❌ Config file is empty.")
+            return False
+
+        # 2️⃣ check syntax
+        with open(config_full_path, "r") as f:
+            source = f.read()
+        compile(source, config_full_path, "exec")
+
+        # 3️⃣ find latest default
+        latest_default = get_latest_default_config_file(os.path.join(config_directory_path, "*_defaults.py"))
+        if not latest_default:
+            print(f"[{os.path.basename(__file__)}] ⚠️ No default config found to compare.")
+            return True  # syntax OK but no structure check possible
+
+        # 4️⃣ parse both files safely (no execution)
+        default_vars = extract_globals_static(latest_default)
+        user_vars = extract_globals_static(config_full_path)
+
+        # 5️⃣ compare keys
+        missing_keys = [k for k in default_vars if k not in user_vars]
+        extra_keys   = [k for k in user_vars if k not in default_vars]
+
+        if missing_keys:
+            print(f"[{os.path.basename(__file__)}] ⚠️ Missing keys in {config_full_path}: {missing_keys}")
+
+        if extra_keys:
+            print(f"[{os.path.basename(__file__)}] ℹ️ Extra keys found (not in defaults): {extra_keys}")
+
+        print(f"[{os.path.basename(__file__)}] ✅ Config validated successfully — syntax and keys are OK.")
+        return True
+    except Exception:
+        return False
+
+def prepare_valid_config(config_directory_path: str = "config", config_backup_path : str = "configBackup"):
+    try:
+        if not os.path.isfile(f"{config_directory_path}/config.py"):
+            raise Exception("config file doesn't exist")
+
+        if not validate_config_file(config_directory_path):
+            raise Exception("config file is empty or has invalid syntax")
+
+        # Config syntax is OK
+        print(f"[{os.path.basename(__file__)}] Config.py file works good !")
+    except KeyboardInterrupt:
+        raise KeyboardInterrupt
+    except Exception as exc:
+        print(f"[{os.path.basename(__file__)}] Failed to load current config.py ! ({str(exc)})")
+
+        # load config backups
+        config_backups = [path for path in glob.glob(
+            f"{config_backup_path}/*.py") if "config" in path]
+        for i in range(len(config_backups)):
+            ds = config_backups[i].split("_")[1:]  # date structure
+            ds.extend(ds.pop(-1).split(":"))
+            ds[-1] = ds[-1][:ds[-1].find(".")]
+            config_backups[i] = [
+                config_backups[i],
+                datetime.datetime(
+                    day=int(ds[0]),
+                    month=int(ds[1]),
+                    year=int(ds[2]),
+                    hour=int(ds[3]),
+                    minute=int(ds[4]),
+                    second=int(ds[5])
+                ).timestamp()
+            ]
+        # make last backups to be placed and used first
+        config_backups.sort(key=lambda item: item[1], reverse=True)
+
+        # try to find and set as current last valid config
+        for config_backup in config_backups:
+            try:
+                try:
+                    os.rename(
+                        f"{config_directory_path}/config.py",
+                        f"{config_directory_path}/ERROR_{datetime.datetime.now(pytz.timezone('Europe/Berlin')).strftime('%d-%m-%Y %H-%M-%S %f')}"
+                        f"_config.py")
+                except:
+                    pass
+                shutil.copy(config_backup[0], f"{config_directory_path}/config.py")
+                uid = pwd.getpwnam("violette").pw_uid
+                gid = grp.getgrnam("violette").gr_gid
+                os.chown(f"{config_directory_path}/config.py", uid, gid)
+
+                if not validate_config_file(config_directory_path):
+                    raise Exception("config file is empty or has invalid syntax")
+
+                print(f"[{os.path.basename(__file__)}] Successfully loaded config:", config_backup[0])
+                break
+            except KeyboardInterrupt:
+                raise KeyboardInterrupt
+            except Exception as e:
+                print(f"[{os.path.basename(__file__)}] {e}")
+                pass
+        else:
+            print(f"[{os.path.basename(__file__)}] Couldn't find proper '{config_directory_path}/config.py' file and '{config_backup_path}' directories!")
+            exit()

config/config_v20_defaults.py

@@ -1,14 +1,15 @@
 """Configuration file."""
 
 
-CONFIG_VERSION = "2.2.1"
+CONFIG_VERSION = "2.2.2"
 
 
 # ======================================================================================================================
 # CONTENTS:
 
 # NAVIGATION ROUTING SETTINGS
-# ROBOT PATH (TRAJECTORY PLANNER) CREATION SETTINGS
+# TRAJECTORY PLANNER SETTINGS
+# NAVIGATION TEST MODE SETTINGS
 # EXTRACTION SETTINGS
 # VESC SETTINGS
 # PENETROMETRY SETTINGS
@@ -26,7 +27,6 @@
 # CAMERA SETTINGS
 # PATHS SETTINGS
 # PREDICTION SETTINGS
-# NAVIGATION TEST MODE SETTINGS
 # PHYSICAL BLOCAGE SETTINGS
 # UNSORTED KEYS
 # ======================================================================================================================
@@ -91,6 +91,7 @@
 CONTINUE_PREVIOUS_PATH = False
 PREVIOUS_PATH_POINTS_FILE = "path_points.dat"
 PREVIOUS_PATH_INDEX_FILE = "path_index.txt"
+PREVIOUS_GNSS_INDEX_FILE = "path_gnss_index.txt"
 
 #Cyril covid
 ORIGIN_AVERAGE_SAMPLES = 8
@@ -104,7 +105,7 @@
 #SPEEDS
 SI_SPEED_UI = 1 #0.8 for 12v
 SI_SPEED_FWD = 0.175 
-SI_SPEED_REV = -0.5
+SI_SPEED_REV = -0.175 #-0.5 for forward backward
 SI_SPEED_FAST = 0.5 
 MULTIPLIER_SI_SPEED_TO_RPM = -14285 #multiplier to go from speed to rpm vesc
 
@@ -121,16 +122,17 @@
 
 
 # ======================================================================================================================
-# ROBOT PATH (TRAJECTORY PLANNER) CREATION SETTINGS
+# TRAJECTORY PLANNER SETTINGS
 # ======================================================================================================================
 #Only one of the following three parameters must be true
 TRADITIONAL_PATH = False #Snail path
 BEZIER_PATH = True #Snail path and use of bezier curve for turns
 FORWARD_BACKWARD_PATH = False #Path where the robot goes straight in extraction then reverses without extraction ....
+MANEUVER_PATH = False 
 
-#This params work only if TRADITIONAL_PATH or BEZIER_PATH are true. 
-ADD_FORWARD_BACKWARD_TO_END_PATH = True #Adds the path FORWARD_BACKWARD to complete the missing center.
-#This params work only if BEZIER_PATH are true. 
+#This params work only if BEZIER_PATH are true : 
+ADD_FORWARD_BACKWARD_TO_END_OF_BEZIER_PATH = False #Adds the path FORWARD_BACKWARD to complete the missing center.
+ADD_MANEUVER_PATH_TO_END_OF_BEZIER_PATH = True #Adds the path MANEUVER_PATH to complete the missing center.
 ADD_CORNER_TO_BEZIER_PATH = False #Add management of corner for bezier curve
 
 #This params work only if BEZIER_CORNER_PATH are true.
@@ -151,6 +153,22 @@
 SMOOTH_APPROACHING_MAX_POINTS = NUMBER_OF_BEZIER_POINT * 4 + 10
 
 
+# ======================================================================================================================
+# NAVIGATION TEST MODE SETTINGS
+# ======================================================================================================================
+DISPLAY_INSTRUCTION_PATH = True #Allows to display the robot guide points on the ui.
+DELTA_DISPLAY_INSTRUCTION_PATH = 15 #Number of guide points display on the ui.
+
+NAVIGATION_TEST_MODE = False # mode allowing the robot to do A->B, B->A
+#The robot will aim for the furthest point, 
+#when it reaches this point it will wait for a press on enter to go to the furthest point from it.
+POINT_A = [[46.1546931, -1.1198362], -0.5] #Point coordinate for test navigation mode, [[lat,long],speed]
+# the speed represents the speed the robot will apply to reach this point.
+POINT_B = [[46.1545618, -1.119885], 0.5] #Point coordinate for test navigation mode, [[lat,long],speed]
+# the speed represents the speed the robot will apply to reach this point.
+RELOAD_CONFIG_DURING_NAVIGATION_TEST = False
+
+
 # ======================================================================================================================
 # EXTRACTION SETTINGS
 # ======================================================================================================================
@@ -538,7 +556,7 @@
 # GPS (USA)             1074        1077
 # GLONASS (Russie)      1084        1087
 # Galileo (UE)          1094        1097
-NTRIP_SLEEP_TIME = 10 # Time in seconds between two sessions of getting data (MSM and ARP)
+NTRIP_SLEEP_TIME = 5 # Time in seconds between two sessions of getting data (MSM and ARP)
 
 CASTER_RESPONSE_DECODE= "ascii"  #"iso-8859-16" for swissgreen
 
@@ -696,21 +714,6 @@
 ZONE_THRESHOLD_DEGREE = [(436,5),(697,7),(796,17),(849,15),(953,6)]
 
 
-# ======================================================================================================================
-# NAVIGATION TEST MODE SETTINGS
-# ======================================================================================================================
-NAVIGATION_TEST_MODE = False # mode allowing the robot to do A->B, B->A
-#The robot will aim for the furthest point, 
-#when it reaches this point it will wait for a press on enter to go to the furthest point from it.
-DISPLAY_INSTRUCTION_PATH = False #Allows to display the robot guide points on the ui.
-DELTA_DISPLAY_INSTRUCTION_PATH = 15 #Number of guide points display on the ui.
-POINT_A = [[46.1546931, -1.1198362], -0.5] #Point coordinate for test navigation mode, [[lat,long],speed]
-# the speed represents the speed the robot will apply to reach this point.
-POINT_B = [[46.1545618, -1.119885], 0.5] #Point coordinate for test navigation mode, [[lat,long],speed]
-# the speed represents the speed the robot will apply to reach this point.
-RELOAD_CONFIG_DURING_NAVIGATION_TEST = False
-
-
 # ======================================================================================================================
 # PHYSICAL BLOCAGE SETTINGS
 # ======================================================================================================================