diff --git a/adapters.py b/adapters.py index e13dd86..e261dce 100644 --- a/adapters.py +++ b/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""" diff --git a/check_config.py b/check_config.py new file mode 100644 index 0000000..3d2a616 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/config/config_v20_defaults.py b/config/config_v20_defaults.py index 9f16a35..19074fe 100644 --- a/config/config_v20_defaults.py +++ b/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 # ====================================================================================================================== diff --git a/config/updater.py b/config/updater.py index 06a8de0..987dd4a 100644 --- a/config/updater.py +++ b/config/updater.py @@ -1,6 +1,4 @@ -import glob -import re -import os +import glob, re, os, pwd, grp version_defaults_file = sorted([(int(m.group(1)),m.string) for m in (re.compile(".*v([0-9]*).*").match(line) for line in glob.glob("*_defaults.py"))], key=lambda x: (x[0], x[1])) @@ -54,3 +52,26 @@ write_file.write(result) continue write_file.write(line) + +try: + user_name = "violette" + group_name = "violette" + + # Get UID and GID for the target user and group + uid = pwd.getpwnam(user_name).pw_uid + gid = grp.getgrnam(group_name).gr_gid + + # Change owner and group + os.chown("config.py", uid, gid) + + # Set permissions to -rw-r--r-- + os.chmod("config.py", 0o644) + + print(f"✅ config.py ownership set to {user_name}:{group_name} with permissions 644.") +except KeyError: + print("⚠️ The user or group 'violette' does not exist on this system.") +except PermissionError: + print("⚠️ Permission denied: run the script with sufficient privileges (sudo).") +except Exception as e: + print(f"⚠️ Unexpected error while changing file ownership: {e}") + diff --git a/field.txt b/field.txt deleted file mode 120000 index 8566c7e..0000000 --- a/field.txt +++ /dev/null @@ -1 +0,0 @@ -fields/Evvc1%20oost.txt \ No newline at end of file diff --git a/last_angle_wheels.txt b/last_angle_wheels.txt deleted file mode 100644 index 1d65b3e..0000000 --- a/last_angle_wheels.txt +++ /dev/null @@ -1 +0,0 @@ -0.00 \ No newline at end of file diff --git a/main.py b/main.py index 1615987..4a37946 100644 --- a/main.py +++ b/main.py @@ -7,7 +7,6 @@ import threading import os import sys -from turtle import speed import time import traceback from matplotlib.patches import Polygon @@ -19,18 +18,15 @@ import json import glob import importlib -import subprocess -import safe_import_of_config -safe_import_of_config.make_import() +from check_config import prepare_valid_config +prepare_valid_config() from config import config import adapters import navigation import utility import detection -import stubs -import extraction import datacollection from extraction import ExtractionManagerV3 from shared_class.robot_synthesis import RobotSynthesis @@ -190,6 +186,9 @@ def move_to_point_and_extract(coords_from_to: list, :param cur_field: None or list of 4 ABCD points which are describing current field robot is working on. :return: """ + + if coords_from_to[0] == coords_from_to[1]: + return if config.ALLOW_FIELD_LEAVING_PROTECTION and cur_field is not None and len(cur_field) > 2: enable_field_leaving_protection = True @@ -772,7 +771,6 @@ def move_to_point_and_extract(coords_from_to: list, distance = nav.get_distance(cur_pos, coords_from_to[1]) - last_corridor_side = current_corridor_side perpendicular, current_corridor_side = nav.get_deviation( coords_from_to[0], coords_from_to[1], cur_pos) @@ -1308,38 +1306,6 @@ def corner_finish_rounds(turning_radius: float): return int((get_rectangle_isosceles_side(turning_radius))/config.FIELD_REDUCE_SIZE)+1 -def add_forward_backward_path(abcd_points: list, nav: navigation.GPSComputing, logger: utility.Logger, SI_speed_fwd: float, SI_speed_rev: float, currently_path: list): - raise NotImplementedError( - "an obsolete code, use build_forward_backward_path() instead") - - if not config.ADD_FORWARD_BACKWARD_TO_END_PATH and not config.FORWARD_BACKWARD_PATH: - return currently_path - - a, b, c, d = abcd_points[0], abcd_points[1], abcd_points[2], abcd_points[3] - - fwd = SI_speed_fwd - rev = SI_speed_rev - - while nav.get_distance(b, c) > config.SPIRAL_SIDES_INTERVAL: - - if not add_points_to_path(currently_path, [b, fwd]): - return currently_path - - if not add_points_to_path(currently_path, [a, rev]): - return currently_path - - a = compute_x1_x2(a, d, config.SPIRAL_SIDES_INTERVAL, nav)[0] - b = compute_x1_x2(b, c, config.SPIRAL_SIDES_INTERVAL, nav)[0] - - if not add_points_to_path(currently_path, [b, fwd]): - return currently_path - - if not add_points_to_path(currently_path, [a, rev]): - return currently_path - - return currently_path - - def build_forward_backward_path(abcd_points: list, nav: navigation.GPSComputing, logger: utility.Logger, @@ -1401,248 +1367,94 @@ def build_forward_backward_path(abcd_points: list, return path -def build_bezier_with_corner_path(abcd_points: list, nav: navigation.GPSComputing, logger: utility.Logger, SI_speed_fwd: float, SI_speed_rev: float): - raise NotImplementedError( - "an obsolete code, use build_bezier_path() instead") - - path = [] - a, b, c, d = abcd_points[0], abcd_points[1], abcd_points[2], abcd_points[3] - - fwd = SI_speed_fwd - rev = SI_speed_rev - - _break = False - - # get moving points A1 - ... - D2 spiral - a1, a2 = compute_x1_x2_points(a, b, nav, logger) - b1, b2 = compute_x1_x2_points(b, c, nav, logger) - c1, c2 = compute_x1_x2_points(c, d, nav, logger) - d1, d2 = compute_x1_x2_points(d, a, nav, logger) - a1_spiral = nav.get_coordinate(a1, a, 90, config.SPIRAL_SIDES_INTERVAL) - _, a_spiral = compute_x1_x2(d, a, config.SPIRAL_SIDES_INTERVAL, nav) - - if not add_points_to_path(path, [a, fwd]): - raise RuntimeError("Failed to add original point A into generated path. " - "This could happen if input field's point A is None.") - - first_bezier_turn = compute_bezier_points(a2, b, b1) - second_bezier_turn = compute_bezier_points(b2, c, c1) - third_bezier_turn = compute_bezier_points(c2, d, d1) - fourth_bezier_turn = compute_bezier_points(d2, a_spiral, a1_spiral) - - # minimum turning radius given in millimeter - turning_radius = config.MANEUVER_START_DISTANCE - - if config.ADD_CORNER_TO_BEZIER_PATH: - rnd = 0 - rnds = corner_finish_rounds(turning_radius) - - # example a 3meter radius requires 4 corners finish - for rnd in range(rnds+1): - # check if there's a point(s) which shouldn't be used as there's no place for robot maneuvers - mxt = "corner rnd "+str(rnd)+"/"+str(rnds) - - # the direction is given along with the point in meter per second, signed - # go to line forward, step back to the turning point "a1" - - if not add_points_to_path(path, [b, fwd, "B "+mxt]): - return path - for index in range(0, len(first_bezier_turn)): - if index == 0: - if not add_points_to_path(path, [first_bezier_turn[index], rev]): - return path - else: - if not add_points_to_path(path, [first_bezier_turn[index], fwd]): - return path - if not add_points_to_path(path, [b, rev, mxt]): - return path - - if not add_points_to_path(path, [c, fwd, "C "+mxt]): - return path - for index in range(0, len(second_bezier_turn)): - if index == 0: - if not add_points_to_path(path, [second_bezier_turn[index], rev]): - return path - else: - if not add_points_to_path(path, [second_bezier_turn[index], fwd]): - return path - if not add_points_to_path(path, [c, rev, mxt]): - return path - - if not add_points_to_path(path, [d, fwd, "D "+mxt]): - return path - for index in range(0, len(third_bezier_turn)): - if index == 0: - if not add_points_to_path(path, [third_bezier_turn[index], rev]): - return path - else: - if not add_points_to_path(path, [third_bezier_turn[index], fwd]): - return path - if not add_points_to_path(path, [d, rev, mxt]): - return path - - if not add_points_to_path(path, [a, fwd, "A "+mxt]): - return path - for index in range(0, len(fourth_bezier_turn)): - if index == 0: - if not add_points_to_path(path, [fourth_bezier_turn[index], rev]): - return path - else: - if not add_points_to_path(path, [fourth_bezier_turn[index], fwd]): - return path - # if not add_points_to_path(path, [a,rev,mxt] ): - if not add_points_to_path(path, [a_spiral, rev, mxt]): - return path - - # get A'B'C'D' (prepare next ABCD points) - b1_int, b2_int = compute_x1_x2_int_points(b, c, nav, logger) - d1_int, d2_int = compute_x1_x2_int_points(d, a, nav, logger) - - if not check_points_for_nones(b1_int, b2_int, d1_int, d2_int): - return path - - a_new, b_new = compute_x1_x2_int_points( - d2_int, b1_int, nav, logger) - c_new, d_new = compute_x1_x2_int_points( - b2_int, d1_int, nav, logger) - - if not check_points_for_nones(a_new, b_new, c_new, d_new): - return path - - a, b, c, d, d2_int_prev = a_new, b_new, c_new, d_new, d2_int - - # get moving points A1 - ... - D2 spiral - a1, a2 = compute_x1_x2_points(d2_int_prev, b, nav, logger) - b1, b2 = compute_x1_x2_points(b, c, nav, logger) - c1, c2 = compute_x1_x2_points(c, d, nav, logger) - d1, d2 = compute_x1_x2_points(d, a, nav, logger) - - for point in [a, b, c, d, a1, b1, c1, d1, a2, b2, c2, d2]: - if point is None: - return path - - a1_spiral = nav.get_coordinate( - a1, a, 90, config.SPIRAL_SIDES_INTERVAL) - _, a_spiral = compute_x1_x2( - d, a, config.SPIRAL_SIDES_INTERVAL, nav) - - for point in [a1_spiral, a_spiral]: - if point is None: - if not _break: - _break = True - break - if _break: - break - - first_bezier_turn = compute_bezier_points(a2, b, b1) - second_bezier_turn = compute_bezier_points(b2, c, c1) - third_bezier_turn = compute_bezier_points(c2, d, d1) - fourth_bezier_turn = compute_bezier_points(d2, a_spiral, a1_spiral) - +def build_maneuvre_path( + abcd_points, + abcd_points_prev, + nav, + logger, + SI_speed_fwd, + SI_speed_rev, + continue_path = list()): + + path = continue_path + + _ , a2 = compute_x1_x2_points(abcd_points[0], abcd_points[1], nav, logger) + + if a2 is not None: + a, b, c, d = abcd_points[0], abcd_points[1], abcd_points[2], abcd_points[3] + else: + a, b, c, d = abcd_points_prev[0], abcd_points_prev[1], abcd_points_prev[2], abcd_points_prev[3] + path = path[:(-config.NUMBER_OF_BEZIER_POINT*4)] + + if nav.get_distance(a,b) > nav.get_distance(b,c): + a, b, c, d = d, a, b, c + if len(path) == 0: + path.append([b,SI_speed_fwd]) + else: + if len(path) == 0: + path.append([a,SI_speed_fwd]) + _ , a2 = compute_x1_x2_points(a, b, nav, logger) + b1, _ = compute_x1_x2_points(b, c, nav, logger) + + b_corner_bezier = compute_bezier_points(a2, b, b1) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], b_corner_bezier)): + raise RuntimeError( + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + path.append([c,SI_speed_fwd]) + + angle_current_half = 90 + sign_current_half = 1 + + largeur_zone_final = nav.get_distance(c,d) + while True: - # get A'B'C'D' (prepare next ABCD points) - b1_int, b2_int = compute_x1_x2_int_points(b, c, nav, logger) - d1_int, d2_int = compute_x1_x2_int_points(d, a, nav, logger) - - if not check_points_for_nones(b1_int, b2_int, d1_int, d2_int): - raise RuntimeError("Some of intermediate points [B1 B2 D1 D2] for next spiral generation are None. " - "This may happen if current distance between points is too small for robot maneuvers.") - - d2_int_prev = d2_int - - a_new, b_new = compute_x1_x2_int_points(d2_int, b1_int, nav, logger) - c_new, d_new = compute_x1_x2_int_points(b2_int, d1_int, nav, logger) - - if not check_points_for_nones(a_new, b_new, c_new, d_new): - raise RuntimeError("Some of next iteration field points [A_new B_new C_new D_new] are None. " - "This may happen if current distance between points is too small for robot maneuvers.") - - # get moving points A1 - ... - D2 spiral - a1, a2 = compute_x1_x2_points(d2_int_prev, b, nav, logger) - b1, b2 = compute_x1_x2_points(b, c, nav, logger) - c1, c2 = compute_x1_x2_points(c, d, nav, logger) - d1, d2 = compute_x1_x2_points(d, a, nav, logger) - - if None in [a, b, c, d, a1, b1, c1, d1, a2, b2, c2, d2]: - if nav.get_distance(a, b) >= nav.get_distance(b, c): - a = compute_x1_x2(a, d, config.SPIRAL_SIDES_INTERVAL, nav)[0] - b = compute_x1_x2(b, c, config.SPIRAL_SIDES_INTERVAL, nav)[0] - return add_forward_backward_path([a, b, c, d], nav, logger, SI_speed_fwd, SI_speed_rev, path) - else: - raise RuntimeError("Some of [A A1 A2 B B1 B2 C C1 C2 D D1 D2] points are None AND AB < BC. " - "Old code, not sure why author raises an exception for such condition.") - - a1_spiral = nav.get_coordinate(a1, a, 90, config.SPIRAL_SIDES_INTERVAL) - _, a_spiral = compute_x1_x2(d, a, config.SPIRAL_SIDES_INTERVAL, nav) - - if None in [a1_spiral, a_spiral]: + + largeur_zone_final_en_cours = nav.get_distance(c,d) + + if largeur_zone_final/2 > largeur_zone_final_en_cours: + angle_current_half = 270 + sign_current_half = -1 + + angle = int(-nav.get_angle(b,c,c,d)) + angle = angle/abs(angle) * angle_current_half + + point_x1 = nav.get_point_on_vector(c, b, config.MANEUVER_START_DISTANCE) + + point_x2 = nav.get_coordinate(point_x1, c, angle, config.MANEUVER_START_DISTANCE) + + b_corner_bezier = compute_bezier_points(c, point_x1, point_x2) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_rev], b_corner_bezier)): raise RuntimeError( - "One of [A_spiral A1_spiral] points are None. This case actually should never happen.") - - first_bezier_turn = compute_bezier_points(a2, b, b1) - second_bezier_turn = compute_bezier_points(b2, c, c1) - third_bezier_turn = compute_bezier_points(c2, d, d1) - fourth_bezier_turn = compute_bezier_points(d2, a_spiral, a1_spiral) - - if nav.get_distance(a, b) >= nav.get_distance(b, c): - - if None in first_bezier_turn+second_bezier_turn: - return add_forward_backward_path([a, b, c, d], nav, logger, SI_speed_fwd, SI_speed_rev, path) - - first_bezier_turn_with_speed = [[point, fwd] - for point in first_bezier_turn] - second_bezier_turn_with_speed = [ - [point, fwd] for point in second_bezier_turn] - # check if there's a point(s) which shouldn't be used as there's no place for robot maneuvers - if not add_points_to_path(path, *(first_bezier_turn_with_speed+second_bezier_turn_with_speed)): - raise Exception( - "Error during generate path (build_bezier_with_corner_path:01) !") - - if None in third_bezier_turn+fourth_bezier_turn: - return add_forward_backward_path([c, d, a, b], nav, logger, SI_speed_fwd, SI_speed_rev, path) - - third_bezier_turn_with_speed = [[point, fwd] - for point in third_bezier_turn] - fourth_bezier_turn_with_speed = [ - [point, fwd] for point in fourth_bezier_turn] - # check if there's a point(s) which shouldn't be used as there's no place for robot maneuvers - if not add_points_to_path(path, *(third_bezier_turn_with_speed+fourth_bezier_turn_with_speed)): - raise Exception( - "Error during generate path (build_bezier_with_corner_path:02) !") - - else: - - first_bezier_turn_with_speed = [[point, fwd] - for point in first_bezier_turn] - # check if there's a point(s) which shouldn't be used as there's no place for robot maneuvers - if not add_points_to_path(path, *(first_bezier_turn_with_speed)): - raise Exception( - "Error during generate path (build_bezier_with_corner_path:03) !") - - if None in second_bezier_turn+third_bezier_turn: - return add_forward_backward_path([b, c, d, a], nav, logger, SI_speed_fwd, SI_speed_rev, path) - second_bezier_turn_with_speed = [ - [point, fwd] for point in second_bezier_turn] - third_bezier_turn_with_speed = [[point, fwd] - for point in third_bezier_turn] - # check if there's a point(s) which shouldn't be used as there's no place for robot maneuvers - if not add_points_to_path(path, *(second_bezier_turn_with_speed+third_bezier_turn_with_speed)): - raise Exception( - "Error during generate path (build_bezier_with_corner_path:04) !") - - _, next_a2 = compute_x1_x2_points(d2_int, b_new, nav, logger) - next_b1, _ = compute_x1_x2_points(b_new, c_new, nav, logger) - - if None in fourth_bezier_turn or None in [next_a2, b_new, next_b1]: - return add_forward_backward_path([d, a, b, c], nav, logger, SI_speed_fwd, SI_speed_rev, path) - # check if there's a point(s) which shouldn't be used as there's no place for robot maneuvers - fourth_bezier_turn_with_speed = [ - [point, fwd] for point in fourth_bezier_turn] - if not add_points_to_path(path, *(fourth_bezier_turn_with_speed)): - raise Exception( - "Error during generate path (build_bezier_with_corner_path:05 !") - - a, b, c, d, d2_int_prev = a_new, b_new, c_new, d_new, d2_int + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + point_x3 = nav.get_point_on_vector(point_x2, point_x1, -config.SPIRAL_SIDES_INTERVAL*sign_current_half) + + path.append([point_x3,SI_speed_rev]) + + point_x4 = nav.get_point_on_vector(point_x3, point_x1, config.MANEUVER_START_DISTANCE) + + point_x5 = nav.get_coordinate(point_x4, point_x3, angle, config.MANEUVER_START_DISTANCE) + + b_corner_bezier = compute_bezier_points(point_x3, point_x4, point_x5) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], b_corner_bezier)): + raise RuntimeError( + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + point_x6 = nav.get_point_on_vector(point_x4, point_x5, -config.MANEUVER_START_DISTANCE) + + path.append([point_x6,SI_speed_rev]) + + point_x7 = nav.get_coordinate(b, c, angle, config.SPIRAL_SIDES_INTERVAL*sign_current_half) + + path.append([point_x7,SI_speed_fwd]) + + if nav.get_deviation(a,d,c)[0] < config.SPIRAL_SIDES_INTERVAL: + break + + c, b, a, d = point_x7, point_x6, d, a + + return path def build_bezier_path(abcd_points: list, @@ -1652,7 +1464,7 @@ def build_bezier_path(abcd_points: list, SI_speed_rev: float): """Builds spiral path to fill given ABCD field. - Fills field's missing center with zigzag (forward-backward) movement if config.ADD_FORWARD_BACKWARD_TO_END_PATH + Fills field's missing center with zigzag (forward-backward) movement if config.ADD_FORWARD_BACKWARD_TO_END_OF_BEZIER_PATH is set to True. Returns python list of gps [[latitude, longitude], speed] points.""" @@ -1750,7 +1562,7 @@ def build_bezier_path(abcd_points: list, a, b, c, d = a_new, b_new, c_new, d_new - if config.ADD_FORWARD_BACKWARD_TO_END_PATH: + if config.ADD_FORWARD_BACKWARD_TO_END_OF_BEZIER_PATH: if center_fill_start_point == 0: msg = "Asked to fill field's center during path building, but filling start position point flag was not " \ "changed from it's initial value." @@ -1775,6 +1587,15 @@ def build_bezier_path(abcd_points: list, msg = "Asked to fill field's center during path building, but filling start position point flag value " \ "is not supported." raise NotImplementedError(msg) + elif config.ADD_MANEUVER_PATH_TO_END_OF_BEZIER_PATH: + path = build_maneuvre_path( + abcd_points, + [a, b, c, d], + nav, + logger, + SI_speed_fwd, + SI_speed_rev, + path) return path @@ -1971,6 +1792,21 @@ def get_bezier_indexes(path_points: list): return bezier_indexes +def get_no_bezier_indexes(path_points: list): + bezier_indexes = get_bezier_indexes(path_points) + return list(set(path_points)^set(bezier_indexes)) + +def get_new_path_start_index(path_points: list, path_start_index, logger_full: utility.Logger): + # Finding the last point in traditional path before path_start_index + non_bezier_indexes = get_no_bezier_indexes(path_points) + + # we take the last index <= path_start_index + last_trad_index = max([i for i in non_bezier_indexes if i <= path_start_index]) + if path_start_index != last_trad_index: + msg = f"Reprise modifiée : retour au dernier point traditional path ({last_trad_index}) au lieu de {path_start_index}" + print(msg) + logger_full.write(msg + "\n") + return last_trad_index def main(): time_start = utility.get_current_time() @@ -2290,6 +2126,7 @@ def main(): logger_full.write_and_flush(msg + "\n") notification.close() exit() + elif path_start_index >= len(path_points) or path_start_index < 1: loading_previous_index_failed = True msg = f"Path start index {path_start_index} is out of path points list range (loaded " \ @@ -2305,6 +2142,10 @@ def main(): path_start_index = 1 with open(config.PREVIOUS_PATH_INDEX_FILE, "w") as path_index_file: path_index_file.write(str(path_start_index)) + + # new_path_start_index = get_new_path_start_index(path_points, path_start_index, logger_full) + # path_start_index = new_path_start_index + # load field points and generate new path or continue previous path errors case if not config.CONTINUE_PREVIOUS_PATH or loading_previous_path_failed: @@ -2347,6 +2188,14 @@ def main(): logger_full, config.SI_SPEED_FWD, config.SI_SPEED_REV) + elif config.MANEUVER_PATH: + path_points = build_maneuvre_path( + field_gps_coords, + field_gps_coords, + nav, + logger_full, + config.SI_SPEED_FWD, + config.SI_SPEED_REV) msg = "Generated " + str(len(path_points)) + " points." logger_full.write(msg + "\n") @@ -2408,7 +2257,10 @@ def main(): # path points visiting loop with open( config.PREVIOUS_PATH_INDEX_FILE, - "r+" if os.path.isfile(config.PREVIOUS_PATH_INDEX_FILE) else "w") as path_index_file: + "r+" if os.path.isfile(config.PREVIOUS_PATH_INDEX_FILE) else "w") as path_index_file : + #open( + # config.PREVIOUS_GNSS_INDEX_FILE, + #"r+" if os.path.isfile(config.PREVIOUS_GNSS_INDEX_FILE) else "w") as GNSS_index_file : # TODO: temp. wheels mechanics hotfix. please don't repeat things I did here they are not good. if config.ENABLE_ADDITIONAL_WHEELS_TURN: if config.TRADITIONAL_PATH: @@ -2618,7 +2470,6 @@ def main(): i_inf = i + 1 if i + 1 < path_end_index else path_end_index i_sup = i + 1 + config.FUTURE_NUMBER_OF_POINTS \ if i + config.FUTURE_NUMBER_OF_POINTS < path_end_index else path_end_index - move_to_point_and_extract( [path_points[i - 1][0], path_points[i][0]], gps, @@ -2682,8 +2533,7 @@ def main(): i_inf = i-config.DELTA_DISPLAY_INSTRUCTION_PATH if i >= config.DELTA_DISPLAY_INSTRUCTION_PATH else 0 i_sup = i+config.DELTA_DISPLAY_INSTRUCTION_PATH if i + \ config.DELTA_DISPLAY_INSTRUCTION_PATH < path_end_index else path_end_index-1 - display_instruction_path = [elem[0] - for elem in path_points[i_inf:i_sup]] + display_instruction_path = path_points[i_inf:i_sup] if ui_msg_queue is not None and config.DISPLAY_INSTRUCTION_PATH: ui_msg_queue.send(json.dumps( @@ -2811,6 +2661,12 @@ def main(): path_index_file.seek(0) path_index_file.write(str(i + 1)) path_index_file.flush() + + #TODO : put the code back here + # GNSS_index_file.seek(0) + # continue_point = get_new_path_start_index(path_points, i + 1, logger_full) + # GNSS_index_file.write(path_points[continue_point]) + # GNSS_index_file.flush() """ msg = "Starting memory cleaning" diff --git a/path_gnss_index.txt b/path_gnss_index.txt new file mode 100644 index 0000000..e8f0077 --- /dev/null +++ b/path_gnss_index.txt @@ -0,0 +1,3 @@ +Parking +46.154274553856226 -1.118446395998319 0.175 +46.154331919664685 -1.118514648117582 0.175 \ No newline at end of file diff --git a/safe_import_of_config.py b/safe_import_of_config.py deleted file mode 100644 index 5ad36d9..0000000 --- a/safe_import_of_config.py +++ /dev/null @@ -1,81 +0,0 @@ -import os -import glob -import datetime -import shutil -import pytz -import pwd -import grp - -# 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 make_import(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 is_config_empty(f"{config_directory_path}/config.py"): - raise Exception("config file is empty") - - from config import config - print("Config.py file works good !") - except KeyboardInterrupt: - raise KeyboardInterrupt - except Exception as exc: - print(f"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 is_config_empty(f"{config_directory_path}/config.py"): - raise Exception("config file is empty") - - from config import config - print("Successfully loaded config:", config_backup[0]) - break - except KeyboardInterrupt: - raise KeyboardInterrupt - except Exception as e: - print(e) - pass - else: - print(f"Couldn't find proper '{config_directory_path}/config.py' file and '{config_backup_path}' directories!") - exit() \ No newline at end of file diff --git a/test_manoeuvre.py b/test_manoeuvre.py new file mode 100644 index 0000000..c964b01 --- /dev/null +++ b/test_manoeuvre.py @@ -0,0 +1,444 @@ +import numpy as np + +import navigation +import utility + +from check_config import prepare_valid_config +prepare_valid_config() +from config import config + +def compute_x1_x2_int_points(point_a: list, point_b: list, nav: navigation.GPSComputing, logger: utility.Logger): + """ + Computes spiral interval points x1, x2 + :param point_a: + :param point_b: + :param nav: + :param logger: + :return: + """ + + cur_vec_dist = nav.get_distance(point_a, point_b) + + # check if moving vector is too small for maneuvers + if config.SPIRAL_SIDES_INTERVAL * 2 >= cur_vec_dist: + msg = "No place for maneuvers; Config spiral interval (that will be multiplied by 2): " + \ + str(config.SPIRAL_SIDES_INTERVAL) + " Current moving vector distance is: " + str(cur_vec_dist) + \ + " Given points are: " + str(point_a) + " " + str(point_b) + if config.VERBOSE: + print(msg) + logger.write(msg + "\n") + return None, None + + point_x1_int = nav.get_point_on_vector( + point_a, point_b, config.SPIRAL_SIDES_INTERVAL) + point_x2_int = nav.get_point_on_vector( + point_a, point_b, cur_vec_dist - config.SPIRAL_SIDES_INTERVAL) + return point_x1_int, point_x2_int + +def check_points_for_nones(*args): + """Checks if any of given points is None. + + Returns True if all given points are not Nones. + Returns False if any of given points is None.""" + + for point in args: + if point is None: + return False + return True + +def compute_bezier_points(point_0, point_1, point_2): + t = np.linspace(0, 1, config.NUMBER_OF_BEZIER_POINT) + coords = list() + for i in t: + x = (point_0[0] - 2 * point_1[0] + point_2[0]) * (i ** 2) + \ + (2 * point_1[0] - 2 * point_0[0]) * i + point_0[0] + y = (point_0[1] - 2 * point_1[1] + point_2[1]) * (i ** 2) + \ + (2 * point_1[1] - 2 * point_0[1]) * i + point_0[1] + coords.append([x, y]) + return coords + +def compute_x1_x2_points(point_a: list, point_b: list, nav: navigation.GPSComputing, logger: utility.Logger, add=0): + """ + Computes p. x1 with config distance from p. A and p. x2 with the same distance from p. B. Distance is loaded from + config file. Returns None if AB <= that distance (as there's no place for robot maneuvers). + + :param point_a: + :param point_b: + :param nav: + :param logger: + :return: + """ + + cur_vec_dist = nav.get_distance(point_a, point_b) + + # check if moving vector is too small for maneuvers + if config.MANEUVER_START_DISTANCE * 2 + add >= cur_vec_dist: + msg = "No place for maneuvers; config start maneuver distance is (that will be multiplied by 2): " + \ + str(config.MANEUVER_START_DISTANCE) + " current moving vector distance is: " + str(cur_vec_dist) + \ + " Given points are: " + str(point_a) + " " + str(point_b) + # print(msg) + logger.write(msg + "\n") + return None, None + + point_x1 = nav.get_point_on_vector( + point_a, point_b, config.MANEUVER_START_DISTANCE) + point_x2 = nav.get_point_on_vector( + point_a, point_b, cur_vec_dist - config.MANEUVER_START_DISTANCE) + return point_x1, point_x2 + +def add_points_to_path(path: list, *args): + """Tries to add given points into given path. + + Returns True if all points are added successfully + Returns False if one of given points is None + + If point is None - previous not None points will be added, further points addition will is canceled and False is + returned""" + + for point in args: + if point is None: + return False + if len(point) > 1: + if point[0] is None: + return False + path.append(point) + return True + +def build_forward_backward_path(abcd_points: list, + nav: navigation.GPSComputing, + logger: utility.Logger, + SI_speed_fwd: float, + SI_speed_rev: float, + path: list = None): + """Builds zigzag (forward-backward) path to fill given ABCD field. + Can process 4 non 90 degrees corners fields. + + Will append zigzag points into the existing path if it is not None, otherwise creates a path from scratch. + Returns python list of gps [[latitude, longitude], speed] points.""" + + if type(abcd_points) != list: + msg = f"Given ABCD path must be a list, got {type(abcd_points).__name__} instead" + raise TypeError(msg) + + if len(abcd_points) != 4: + msg = f"Expected 4 ABCD points as input field, got {str(len(abcd_points))} points instead" + raise ValueError(msg) + + for point_name, point in zip("ABCD", abcd_points): + if type(point) != list: + msg = f"Point {point_name} of given ABCD field must be a list, got {type(point).__name__} instead" + raise TypeError(msg) + if len(point) < 2: + msg = f"Point {point_name} of given ABCD field must contain >=2 items, found {str(len(point))} instead" + raise ValueError(msg) + + if path is None: + path = [] + elif type(path) != list: + msg = f"Given ABCD path must be a list type, got {type(path).__name__} instead" + raise TypeError(msg) + + a, b, c, d = abcd_points[0], abcd_points[1], abcd_points[2], abcd_points[3] + + # separate stop-flags and BC & AD length control allows correct processing 4 corner non 90 degrees fields + bc_dist_ok = ad_dist_ok = True + + while bc_dist_ok or ad_dist_ok: + if not add_points_to_path(path, [b, SI_speed_fwd]): + msg = f"Failed to add point B={str(b)} to path. This expected never to happen." + raise RuntimeError(msg) + + if not add_points_to_path(path, [a, SI_speed_rev]): + msg = f"Failed to add point A={str(a)} to path. This expected never to happen." + raise RuntimeError(msg) + + if nav.get_distance(b, c) >= config.SPIRAL_SIDES_INTERVAL: + b = nav.get_point_on_vector(b, c, config.SPIRAL_SIDES_INTERVAL) + else: + bc_dist_ok = False + + if nav.get_distance(a, d) >= config.SPIRAL_SIDES_INTERVAL: + a = nav.get_point_on_vector(a, d, config.SPIRAL_SIDES_INTERVAL) + else: + ad_dist_ok = False + + return path +ADD_FINAL_VIRAGE = True +ADD_DIRECT_TO_FINAL_CORNER = True +ADD_FORWARD_BACKWARD_TO_END_OF_BEZIER_PATH = False + +def build_maneuvre_path( + abcd_points, + abcd_points_prev, + nav, + logger, + SI_speed_fwd, + SI_speed_rev, + continue_path = list()): + + path = continue_path + + _ , a2 = compute_x1_x2_points(abcd_points[0], abcd_points[1], nav, logger) + + if a2 is not None: + a, b, c, d = abcd_points[0], abcd_points[1], abcd_points[2], abcd_points[3] + else: + a, b, c, d = abcd_points_prev[0], abcd_points_prev[1], abcd_points_prev[2], abcd_points_prev[3] + path = path[:(-config.NUMBER_OF_BEZIER_POINT*4)] + + if nav.get_distance(a,b) > nav.get_distance(b,c): + a, b, c, d = d, a, b, c + if len(path) == 0: + path.append([b,SI_speed_fwd]) + else: + if len(path) == 0: + path.append([a,SI_speed_fwd]) + _ , a2 = compute_x1_x2_points(a, b, nav, logger) + b1, _ = compute_x1_x2_points(b, c, nav, logger) + + b_corner_bezier = compute_bezier_points(a2, b, b1) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], b_corner_bezier)): + raise RuntimeError( + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + path.append([c,SI_speed_fwd]) + + angle_current_half = 90 + sign_current_half = 1 + + largeur_zone_final = nav.get_distance(c,d) + + while True: + + largeur_zone_final_en_cours = nav.get_distance(c,d) + + if largeur_zone_final/2 > largeur_zone_final_en_cours: + angle_current_half = 270 + sign_current_half = -1 + + angle = int(-nav.get_angle(b,c,c,d)) + angle = angle/abs(angle) * angle_current_half + + point_x1 = nav.get_point_on_vector(c, b, config.MANEUVER_START_DISTANCE) + + point_x2 = nav.get_coordinate(point_x1, c, angle, config.MANEUVER_START_DISTANCE) + + b_corner_bezier = compute_bezier_points(c, point_x1, point_x2) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_rev], b_corner_bezier)): + raise RuntimeError( + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + point_x3 = nav.get_point_on_vector(point_x2, point_x1, -config.SPIRAL_SIDES_INTERVAL*sign_current_half) + + path.append([point_x3,SI_speed_rev]) + + point_x4 = nav.get_point_on_vector(point_x3, point_x1, config.MANEUVER_START_DISTANCE) + + point_x5 = nav.get_coordinate(point_x4, point_x3, angle, config.MANEUVER_START_DISTANCE) + + b_corner_bezier = compute_bezier_points(point_x3, point_x4, point_x5) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], b_corner_bezier)): + raise RuntimeError( + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + point_x6 = nav.get_point_on_vector(point_x4, point_x5, -config.MANEUVER_START_DISTANCE) + + path.append([point_x6,SI_speed_rev]) + + point_x7 = nav.get_coordinate(b, c, angle, config.SPIRAL_SIDES_INTERVAL*sign_current_half) + + path.append([point_x7,SI_speed_fwd]) + + if nav.get_deviation(a,d,c)[0] < config.SPIRAL_SIDES_INTERVAL: + break + + c, b, a, d = point_x7, point_x6, d, a + + return path + +def build_bezier_path(abcd_points: list, + nav: navigation.GPSComputing, + logger: utility.Logger, + SI_speed_fwd: float, + SI_speed_rev: float): + + """Builds spiral path to fill given ABCD field. + + Fills field's missing center with zigzag (forward-backward) movement if config.ADD_FORWARD_BACKWARD_TO_END_OF_BEZIER_PATH + is set to True. + Returns python list of gps [[latitude, longitude], speed] points.""" + + if config.ADD_CORNER_TO_BEZIER_PATH: + raise NotImplementedError( + "config.ADD_CORNER_TO_BEZIER_PATH feature is not ready in new path builder yet") + + if type(abcd_points) != list: + msg = f"Given ABCD path must be a list, got {type(abcd_points).__name__} instead" + raise TypeError(msg) + + if len(abcd_points) != 4: + msg = f"Expected 4 ABCD points as input field, got {str(len(abcd_points))} points instead" + raise ValueError(msg) + + for point_name, point in zip("ABCD", abcd_points): + if type(point) != list: + msg = f"Point {point_name} of given ABCD field must be a list, got {type(point).__name__} instead" + raise TypeError(msg) + if len(point) < 2: + msg = f"Point {point_name} of given ABCD field must contain >=2 items, found {str(len(point))} instead" + raise ValueError(msg) + + a, b, c, d = abcd_points[0], abcd_points[1], abcd_points[2], abcd_points[3] + path = [] + center_fill_start_point = 0 # 0 is unidentified, 1 is A, 2 is D + + if not add_points_to_path(path, [a, SI_speed_fwd]): + raise RuntimeError( + "Failed to add point A (the once of input field description points) into generated path") + + a_prev, b_prev, c_prev, d_prev= list(),list(),list(),list() + a1, a2, b1, b2, c1, c2, d1 = list(),list(),list(),list(),list(),list(),list() + + while True: + # get moving points A1 - ... - D2 spiral + a1, a2 = compute_x1_x2_points(a, b, nav, logger) + b1, b2 = compute_x1_x2_points(b, c, nav, logger) + c1, c2 = compute_x1_x2_points(c, d, nav, logger) + d1, _ = compute_x1_x2_points(d, a, nav, logger) + if not check_points_for_nones(a1, a2, b1, b2, c1, c2, d1): + center_fill_start_point = 1 + break + + b_corner_bezier = compute_bezier_points(a2, b, b1) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], b_corner_bezier)): + raise RuntimeError( + "Failed to add B corner's bezier curve to path. This expected never to happen.") + + c_corner_bezier = compute_bezier_points(b2, c, c1) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], c_corner_bezier)): + raise RuntimeError( + "Failed to add C corner's bezier curve to path. This expected never to happen.") + + d_corner_bezier = compute_bezier_points(c2, d, d1) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], d_corner_bezier)): + raise RuntimeError( + "Failed to add D corner's bezier curve to path. This expected never to happen.") + + # check before computing d2 and A corner bezier curve (see d2 computing comments below for details) + if nav.get_distance(d, a) <= config.MANEUVER_START_DISTANCE * 2 + config.SPIRAL_SIDES_INTERVAL \ + or nav.get_distance(a, b) <= config.MANEUVER_START_DISTANCE: + center_fill_start_point = 2 + break + + # d2 isn't as other x2 points as d2 distance from A is spiral_sides_interval + start_turn_distance + # instead of just start_turn_distance, so DA acceptable length computing is different (+spiral side interval) + d2 = nav.get_point_on_vector( + a, d, config.SPIRAL_SIDES_INTERVAL + config.MANEUVER_START_DISTANCE) + a_spiral = nav.get_point_on_vector(a, d, config.SPIRAL_SIDES_INTERVAL) + # a1_spiral point is inside the initial field, corner of D-A_spiral-A1_spiral = 90 degrees + a1_spiral = nav.get_coordinate( + a_spiral, d, 90, config.MANEUVER_START_DISTANCE) + + a_corner_bezier = compute_bezier_points(d2, a_spiral, a1_spiral) + if not add_points_to_path(path, *map(lambda gps_point: [gps_point, SI_speed_fwd], a_corner_bezier)): + raise RuntimeError( + "Failed to add A corner's bezier curve to path. This expected never to happen.") + + # get A'B'C'D' (intermediate points used to compute new ABCD points for next iteration) + # (int points are requiring given vector length >= spiral_sides_interval * 2 + # it is very small value and can be exceeded only if robot can turn almost inplace) + b1_int, b2_int = compute_x1_x2_int_points(b, c, nav, logger) + d1_int, d2_int = compute_x1_x2_int_points(d, a, nav, logger) + if not check_points_for_nones(b1_int, b2_int, d1_int, d2_int): + msg = "Some of intermediate points [B1_int B2_int D1_int D2_int] for next spiral generation are None. " \ + "This could happen if spiral shift value is higher than robot's maneuverability. " \ + "Check config.MANEUVER_START_DISTANCE and config.SPIRAL_SIDES_INTERVAL for wrong values." + raise RuntimeError(msg) + + a_new, b_new = compute_x1_x2_int_points(d2_int, b1_int, nav, logger) + c_new, d_new = compute_x1_x2_int_points(b2_int, d1_int, nav, logger) + if not check_points_for_nones(a_new, b_new, c_new, d_new): + msg = "Some of points [A_new B_new C_new D_new] for next spiral generation iteration are None. " \ + "This could happen if spiral shift value is higher than robot's maneuverability. " \ + "Check config.MANEUVER_START_DISTANCE and config.SPIRAL_SIDES_INTERVAL for wrong values." + raise RuntimeError(msg) + + a_prev, b_prev, c_prev, d_prev = a, b, c, d + a, b, c, d = a_new, b_new, c_new, d_new + + if center_fill_start_point == 1: + print("robot is going to stop spiral movement at point A") + if center_fill_start_point == 2: + print("robot is going to stop spiral movement at point D") + + path = build_maneuvre_path( + [a, b, c, d], + [a_prev, b_prev, c_prev, d_prev], + nav, + logger, + SI_speed_fwd, + SI_speed_rev, + path + ) + + return path + + if config.ADD_FORWARD_BACKWARD_TO_END_OF_BEZIER_PATH: + if center_fill_start_point == 0: + msg = "Asked to fill field's center during path building, but filling start position point flag was not " \ + "changed from it's initial value." + raise RuntimeError(msg) + elif center_fill_start_point == 1: # when robot is going to stop spiral movement at point A'n + path = build_forward_backward_path( + [a, b, c, d], + nav, + logger, + SI_speed_fwd, + SI_speed_rev, + path) + elif center_fill_start_point == 2: # when robot is going to stop spiral movement at point D'n + path = build_forward_backward_path( + [d, a, b, c], + nav, + logger, + SI_speed_fwd, + SI_speed_rev, + path) + else: + msg = "Asked to fill field's center during path building, but filling start position point flag value " \ + "is not supported." + raise NotImplementedError(msg) + + return path + +if __name__=="__main__": + + logger_full = utility.Logger("./log_full.txt", append_file=False) + nav = navigation.GPSComputing() + field_gps_coords = utility.load_coordinates(config.INPUT_GPS_FIELD_FILE) + + #field_gps_coords = [field_gps_coords[1],field_gps_coords[2],field_gps_coords[3],field_gps_coords[0]] + #field_gps_coords = [field_gps_coords[2],field_gps_coords[3],field_gps_coords[0],field_gps_coords[1]] + #field_gps_coords = [field_gps_coords[3],field_gps_coords[0],field_gps_coords[1],field_gps_coords[2]] + + path_points = build_bezier_path( + field_gps_coords, + nav, + logger_full, + config.SI_SPEED_FWD, + config.SI_SPEED_REV + ) + + # path_points = build_maneuvre_path( + # [field_gps_coords[0], field_gps_coords[1], field_gps_coords[2], field_gps_coords[3]], + # [field_gps_coords[0], field_gps_coords[1], field_gps_coords[2], field_gps_coords[3]], + # nav, + # logger_full, + # config.SI_SPEED_FWD, + # config.SI_SPEED_REV + # ) + + with open("manoeuvre_path_points.txt", "w") as f: + for coords, value in path_points: + line = f"{coords[0]} {coords[1]} {value}\n" + f.write(line) # écriture dans le fichier \ No newline at end of file diff --git a/uiWebRobot/application.py b/uiWebRobot/application.py index 733b0e6..16f9123 100644 --- a/uiWebRobot/application.py +++ b/uiWebRobot/application.py @@ -1,8 +1,8 @@ import sys sys.path.append('../') -import safe_import_of_config -safe_import_of_config.make_import("../config", "../configBackup") +from check_config import prepare_valid_config +prepare_valid_config("../config", "../configBackup") from state_machine.Events import Events from state_machine.utilsFunction import * @@ -45,7 +45,7 @@ def __init__(self): self.__robot_state_client = RobotStateClient() self.init_params() self.demo_pause_client = utility.DemoPauseClient( - config.DEMO_PAUSES_HOST, config.DEMO_PAUSES_PORT) + self.__config.DEMO_PAUSES_HOST, self.__config.DEMO_PAUSES_PORT) def exit(self): @@ -220,6 +220,10 @@ def on_socket_data(self, data): elif data["type"] == "removeField": if isinstance(self.get_state_machine().currentState, WaitWorkingState): self.get_state_machine().on_socket_data(data) + + elif data["type"] == "getContinuePoint": + if isinstance(self.get_state_machine().currentState, WaitWorkingState): + self.get_state_machine().on_socket_data(data) def on_socket_broadcast(self, data): emit(data["type"], data, broadcast=True) diff --git a/uiWebRobot/state_machine/states/WaitWorkingState.py b/uiWebRobot/state_machine/states/WaitWorkingState.py index 1fb9899..e562b2b 100644 --- a/uiWebRobot/state_machine/states/WaitWorkingState.py +++ b/uiWebRobot/state_machine/states/WaitWorkingState.py @@ -5,7 +5,7 @@ import threading import os import json -from urllib.parse import quote +from urllib.parse import quote, unquote from uiWebRobot.state_machine import State from uiWebRobot.state_machine.states import CreateFieldState @@ -358,6 +358,28 @@ def on_socket_data(self, data): elif data["type"] == "wait_working_state_refresh" : self.__check_ui_refresh_thread_alive = False + + elif data["type"] == 'getContinuePoint': + previous_gnss_file_path = "../" + config.PREVIOUS_GNSS_INDEX_FILE + if os.path.exists(previous_gnss_file_path): + with open(previous_gnss_file_path, "r") as f: + previous_gnss_index_lines = f.readlines() + if len(previous_gnss_index_lines) == 3: + + link_path = os.path.realpath("../field.txt") + current_field = (link_path.split("/")[-1]).split(".")[0] + current_field = unquote(current_field, encoding='utf-8') + + if current_field == previous_gnss_index_lines[0].strip(): + self.socketio.emit('showContinuePoint', + json.dumps({"A": previous_gnss_index_lines[1].strip().split(), "B": previous_gnss_index_lines[2].strip().split()}), + namespace='/map') + else: + msg = f"[{self.__class__.__name__}] -> File {previous_gnss_file_path} does not exist" + self.logger.write_and_flush(msg + "\n") + print(msg) + + return self return self def getStatusOfControls(self): diff --git a/uiWebRobot/state_machine/states/WorkingState.py b/uiWebRobot/state_machine/states/WorkingState.py index f886d6b..47187bf 100644 --- a/uiWebRobot/state_machine/states/WorkingState.py +++ b/uiWebRobot/state_machine/states/WorkingState.py @@ -47,6 +47,7 @@ def __init__(self, socketio: SocketIO, logger: utility.Logger, isAudit: bool, is self.extracted_plants = dict() self.last_path_all_points = list() self.previous_sessions_working_time = None + self.__queue_penetrometry_data = None self.__gearbox_protection = GearboxProtection() self.statusOfUIObject = FrontEndObjects(fieldButton=ButtonState.DISABLE, @@ -134,7 +135,7 @@ def on_event(self, event): msg = f"[{self.__class__.__name__}] -> Send KeyboardInterrupt to main" self.logger.write_and_flush(msg + "\n") print(msg) - os.killpg(os.getpgid(self.main.pid), signal.SIGINT) + os.killpg(os.getpgid(self.main.pid), signal.SIGINT) time.sleep(3) if config.UI_VERBOSE_LOGGING: @@ -290,30 +291,38 @@ def sendLastStatistics(self): self.socketio.emit('statistics', data, namespace='/server', broadcast=True) def _main_msg_thread_tf(self): - - self.queue_penetrometry_data = None - if config.PENETROMETRY_ANALYSE_MODE: - # Waiting for queue creating by adapter - while(self.queue_penetrometry_data is None): + + while self._main_msg_thread_alive: + + if hasattr(self, "main"): + if self.main.poll() is not None: + self._main_msg_thread_alive = False + self.__main_not_received_stop = False + if config.UI_VERBOSE_LOGGING: + msg = f"[{self.__class__.__name__}] -> Detect main dead !" + self.logger.write_and_flush(msg + "\n") + print(msg) + continue + + if config.PENETROMETRY_ANALYSE_MODE: + # Waiting for queue creating by adapter try: - self.queue_penetrometry_data = posix_ipc.MessageQueue(config.PENETROMETRY_DATA_QUEUE_NAME) + self.__queue_penetrometry_data = posix_ipc.MessageQueue(config.PENETROMETRY_DATA_QUEUE_NAME) except posix_ipc.ExistentialError: pass - - - while self._main_msg_thread_alive: + if self.__queue_penetrometry_data is None: + continue - if self.queue_penetrometry_data is not None: + if self.__queue_penetrometry_data is not None: msg = None try: - msg = self.queue_penetrometry_data.receive(0.01) + msg = self.__queue_penetrometry_data.receive(0.01) except posix_ipc.BusyError: pass # If queue is empty continue loop, it will refill if msg is not None: print(f"Envoie des données de l'extraction au client WEB") self.socketio.emit('penetrometry_datas', json.loads(msg[0]), namespace="/server", broadcast=True) - try: msg = self.msgQueue.receive(timeout=2) data = json.loads(msg[0]) @@ -375,6 +384,7 @@ def _main_msg_thread_tf(self): elif "display_instruction_path" in data: data = data["display_instruction_path"] + print(f"[{self.__class__.__name__}] Display instruction path received with {data} points") self.socketio.emit('updateDisplayInstructionPath', json.dumps([elem[::-1] for elem in data]), namespace='/map', broadcast=True) @@ -416,15 +426,15 @@ def _main_msg_thread_tf(self): pass # Closing file descriptor and removing queue if it exist - if self.queue_penetrometry_data is not None: - msg = f"[{self.__class__.__name__}] -> Close queue_penetrometry_data..." + if self.__queue_penetrometry_data is not None: + msg = f"[{self.__class__.__name__}] -> Close __queue_penetrometry_data..." self.logger.write_and_flush(msg + "\n") print(msg) - self.queue_penetrometry_data.close() - msg = f"[{self.__class__.__name__}] -> Unlink queue_penetrometry_data..." + self.__queue_penetrometry_data.close() + msg = f"[{self.__class__.__name__}] -> Unlink __queue_penetrometry_data..." self.logger.write_and_flush(msg + "\n") print(msg) try: - self.queue_penetrometry_data.unlink() + self.__queue_penetrometry_data.unlink() except posix_ipc.ExistentialError: pass \ No newline at end of file diff --git a/uiWebRobot/state_machine/utilsFunction.py b/uiWebRobot/state_machine/utilsFunction.py index 6767b5a..130d996 100644 --- a/uiWebRobot/state_machine/utilsFunction.py +++ b/uiWebRobot/state_machine/utilsFunction.py @@ -119,13 +119,16 @@ def send_last_pos_thread_tf(send_last_pos_thread_alive: bool, socketio: SocketIO - socketio : socket connected with ui - logger """ + print("[Last pos thread] -> Starting GPSUbloxAdapterWithoutThread") with adapters.GPSUbloxAdapterWithoutThread(config.GPS_PORT, config.GPS_BAUDRATE, 1) as gps: while send_last_pos_thread_alive(): lastPos = gps.get_fresh_position() - if config.ALLOW_GPS_BAD_QUALITY_NTRIP_RESTART and lastPos[2]!='4': - NavigationV3.restart_ntrip_service(logger) - socketio.emit('updatePath', json.dumps([[[lastPos[1], lastPos[0]]], lastPos[2]]), namespace='/map', broadcast=True) - socketio.emit('updateGPSQuality', lastPos[2], namespace='/gps', broadcast=True) + #print(f"[Last pos thread] -> Last GPS position: {lastPos}") + if lastPos is not None: + if config.ALLOW_GPS_BAD_QUALITY_NTRIP_RESTART and lastPos[2]!='4': + NavigationV3.restart_ntrip_service(logger) + socketio.emit('updatePath', json.dumps([[[lastPos[1], lastPos[0]]], lastPos[2]]), namespace='/map', broadcast=True) + socketio.emit('updateGPSQuality', lastPos[2], namespace='/gps', broadcast=True) def initVesc(logger: utility.Logger) -> adapters.VescAdapterV4 : diff --git a/uiWebRobot/static/js/mapUtil.js b/uiWebRobot/static/js/mapUtil.js index 9b54898..8393d33 100644 --- a/uiWebRobot/static/js/mapUtil.js +++ b/uiWebRobot/static/js/mapUtil.js @@ -93,8 +93,6 @@ function createMap(coords_field, coords_other) { } map.on('load', function () { - - map.loadImage('http://' + document.domain + ':' + location.port + '/static/nav.png', (error, image) => { if (error) throw error; map.addImage('nav-img', image); @@ -232,7 +230,9 @@ function createMap(coords_field, coords_other) { 'icon-rotate': ['get', 'rotate'], 'icon-rotation-alignment': 'map', 'icon-image': 'nav-img', - 'icon-size': 0.3 + 'icon-size': 0.3, + 'icon-ignore-placement': true, + 'icon-allow-overlap': true } }); } @@ -358,16 +358,38 @@ function createMap(coords_field, coords_other) { }); } - map.getSource('lastPos').setData({ - 'type': 'Feature', - 'geometry': { - 'type': 'Point', - 'coordinates': last_coord - }, - "properties": { - 'quality': quality - }, - }); + if (typeof (map.getSource('lastPos')) == "undefined") { + map.addSource('lastPos', { + 'type': 'geojson', + 'data': { + 'type': 'Feature', + 'geometry': { + 'type': 'Point', + 'coordinates': last_coord + } + } + }); + map.addLayer({ + id: "lastPosLayer", + type: "circle", + source: "lastPos", + paint: { + "circle-radius": 5, + "circle-color": "#ff00e0", + }, + }); + } else { + map.getSource('lastPos').setData({ + 'type': 'Feature', + 'geometry': { + 'type': 'Point', + 'coordinates': last_coord + }, + "properties": { + 'quality': quality + }, + }); + } if (coords.length > 1 || !firstFocus) { map.panTo(last_coord); @@ -378,12 +400,174 @@ function createMap(coords_field, coords_other) { socketMap.on('updateLastPath', function (dataServ) { lastPathCoords = JSON.parse(dataServ); }); - socketio.emit('data', { type: "getLastPath" }); + + //Continue point + map.loadImage('http://' + document.domain + ':' + location.port + '/static/nav_continue.png', (error, image_continue) => { + if (error) throw error; + map.addImage('nav_continue-img', image_continue); + socketMap.on('showContinuePoint', function (dataServ) { + A_B_continue_points = JSON.parse(dataServ); + let A = A_B_continue_points["A"]; + let B = A_B_continue_points["B"]; + var degrees_continue = 0; + var start_point_continue_btn = []; + if (coords_field.length > 0) { + degrees_continue = Math.atan2(A[0] - B[0], A[1] - B[1]) * 180 / Math.PI; + start_point_continue_btn = [A[1], A[0]]; + } + if (typeof (map.getSource('field_continue')) == "undefined") { + map.addSource('field_continue', { + 'type': 'geojson', + 'data': { + 'type': 'Feature', + 'geometry': { + 'type': 'Point', + 'coordinates': start_point_continue_btn + }, + "properties": { + 'rotate': degrees_continue + }, + } + }); + map.addLayer({ + 'id': 'field_continueLayer', + 'type': 'symbol', + 'source': 'field_continue', + 'layout': { + 'icon-rotate': ['get', 'rotate'], + 'icon-rotation-alignment': 'map', + 'icon-image': 'nav_continue-img', + 'icon-size': 0.3, + 'icon-ignore-placement': true, + 'icon-allow-overlap': true + } + }, 'field_startLayer'); // Ajouter avant field_startLayer + } + }); + }); + + socketio.emit('data', { type: "getContinuePoint" }); }); } +// --- Create or update a MultiLine layer --- +function updateLineLayer(map, idSource, idLayer, segments, color) { + const geojsonData = { + 'type': 'Feature', + 'geometry': { + 'type': 'MultiLineString', + 'coordinates': segments + } + }; + + if (typeof map.getSource(idSource) === "undefined") { + map.addSource(idSource, { + 'type': 'geojson', + 'data': geojsonData + }); + + const paint = { + 'line-color': color, + 'line-width': 3 + }; + + map.addLayer({ + 'id': idLayer, + 'type': 'line', + 'source': idSource, + 'layout': { + 'line-join': 'round', + 'line-cap': 'round' + }, + 'paint': paint + }); + } else { + map.getSource(idSource).setData(geojsonData); + } +} + +function updateDisplayInstructionPathLayer(map, dataServ) { + dataServ = JSON.parse(dataServ); + + let forwardSegments = []; + let backwardSegments = []; + let currentSegment = []; + let currentType = null; + + for (let i = 0; i < dataServ.length - 1; i++) { + const [curSpeed, curPosRaw] = dataServ[i]; + const [nextSpeed, nextPosRaw] = dataServ[i + 1]; + + // Convert [lat, lon] → [lon, lat] + const curPos = [curPosRaw[1], curPosRaw[0]]; + const nextPos = [nextPosRaw[1], nextPosRaw[0]]; + const segmentType = curSpeed >= 0 ? 'forward' : 'backward'; + + // Detect direction change (forward ↔ backward) + if (currentType !== segmentType && currentSegment.length > 0) { + if (currentType === 'forward') forwardSegments.push([...currentSegment]); + else backwardSegments.push([...currentSegment]); + currentSegment = []; + } + + // Add both points to current segment + currentSegment.push(curPos, nextPos); + currentType = segmentType; + } + + // Push the last remaining segment + if (currentSegment.length > 0) { + if (currentType === 'forward') forwardSegments.push(currentSegment); + else backwardSegments.push(currentSegment); + } + + // --- Draw forward (orange) and backward (blue) lines --- + updateLineLayer(map, 'instruction_line_forward', 'instruction_lineLayer_forward', forwardSegments, '#FF8C15'); + updateLineLayer(map, 'instruction_line_backward', 'instruction_lineLayer_backward', backwardSegments, '#157CFF'); + + // --- Points colored based on speed --- + const pointFeatures = dataServ.map(([speed, [lat, lon]]) => ({ + 'type': 'Feature', + 'geometry': { 'type': 'Point', 'coordinates': [lon, lat] }, + 'properties': { 'speed': speed } + })); + + const pointCollection = { 'type': 'FeatureCollection', 'features': pointFeatures }; + + const circlePaint = { + 'circle-radius': 3.5, + 'circle-color': [ + 'case', + ['>=', ['get', 'speed'], 0], + '#FF8C15', // forward + '#157CFF' // backward + ] + }; + + if (typeof map.getSource('instruction_point') === "undefined") { + map.addSource('instruction_point', { + 'type': 'geojson', + 'data': pointCollection + }); + map.addLayer({ + 'id': 'instruction_pointLayer', + 'type': 'circle', + 'source': 'instruction_point', + 'paint': circlePaint + }); + } else { + map.getSource('instruction_point').setData(pointCollection); + } +} + +// --- Socket listener --- +socketMap.on('updateDisplayInstructionPath', function (dataServ) { + updateDisplayInstructionPathLayer(map, dataServ); +}); + + socketMap.on('updateDisplayInstructionPath', function (dataServ) { dataServ = JSON.parse(dataServ) //Instruction_line @@ -453,6 +637,12 @@ socketMap.on('updateDisplayInstructionPath', function (dataServ) { }); socketMap.on('newField', function (dataServ) { + + if (typeof (map.getSource('field_continue')) != "undefined") { + map.removeLayer('field_continueLayer'); + map.removeSource('field_continue'); + } + dataServ = JSON.parse(dataServ); if (dataServ["current_field_name"] == "") { @@ -628,4 +818,6 @@ socketMap.on('newField', function (dataServ) { socketBroadcast.emit('data', { type: "reloader", status: false }); + socketio.emit('data', { type: "getContinuePoint" }); + }); \ No newline at end of file diff --git a/uiWebRobot/static/nav.png b/uiWebRobot/static/nav.png index 92ff5c3..62fca73 100644 Binary files a/uiWebRobot/static/nav.png and b/uiWebRobot/static/nav.png differ diff --git a/uiWebRobot/static/nav_continue.png b/uiWebRobot/static/nav_continue.png new file mode 100644 index 0000000..1e9300c Binary files /dev/null and b/uiWebRobot/static/nav_continue.png differ diff --git a/v3_make_photos_manual.py b/v3_make_photos_manual.py index 73eeeb0..7def333 100644 --- a/v3_make_photos_manual.py +++ b/v3_make_photos_manual.py @@ -7,8 +7,6 @@ import utility import select -OUTPUT_DIR = "" - def markup_5_points(image): img_y_c, img_x_c = int(image.shape[0] / 2), int(image.shape[1] / 2) # center @@ -31,6 +29,7 @@ def manual_photos_making(camera): label = input("Please type a label to be added to photos: ") sep = " " counter = 1 + global OUTPUT_DIR path_piece = OUTPUT_DIR + label + sep while True: @@ -53,6 +52,7 @@ def run_performance_test(camera): label = input("Please type a label to be added to photos: ") sep = " " counter = 1 + global OUTPUT_DIR path_piece = OUTPUT_DIR + label + sep paused = False @@ -80,6 +80,7 @@ def main(): print("Usage: python v3_make_photos_manual.py ") sys.exit(1) + global OUTPUT_DIR OUTPUT_DIR = sys.argv[1] if not os.path.exists(OUTPUT_DIR): create = input(f"The directory '{OUTPUT_DIR}' does not exist. Do you want to create it? (y/n): ").strip().lower()