flame-wro-fe 1.0.0

package/raspySrc/raspy/settings.json

@@ -0,0 +1,198 @@
+{
+  "version": 2,
+  "filters": {
+    "GRAY": 56,
+    "WALL_MAX_SAT": 102,
+    "WALL_EXCLUDE_COLORS": true,
+    "WALL_EXCLUDE_DILATE": 1,
+    "REDLO": [
+      111,
+      149,
+      48
+    ],
+    "REDHI": [
+      130,
+      239,
+      167
+    ],
+    "GREENLO": [
+      51,
+      66,
+      33
+    ],
+    "GREENHI": [
+      73,
+      171,
+      136
+    ],
+    "ORANGELO": [
+      98,
+      0,
+      88
+    ],
+    "ORANGEHI": [
+      118,
+      234,
+      208
+    ],
+    "BLUELO": [
+      173,
+      0,
+      54
+    ],
+    "BLUEHI": [
+      14,
+      255,
+      240
+    ],
+    "PINKLO": [
+      158,
+      160,
+      100
+    ],
+    "PINKHI": [
+      170,
+      255,
+      255
+    ]
+  },
+  "contours": {
+    "minSize": 80.0,
+    "pillar_morph_kernel": 5,
+    "pillar_min_width": 4,
+    "pillar_min_height": 8,
+    "pillar_min_fill_ratio": 0.18
+  },
+  "camera": {
+    "frame_width": 640,
+    "frame_height": 400,
+    "format": "RGB888",
+    "crop_height": 190,
+    "flip_180": true,
+    "awb_enable": false,
+    "mtx": [
+      [
+        495.44408865,
+        0.0,
+        294.45510998
+      ],
+      [
+        0.0,
+        504.36905411,
+        259.77204181
+      ],
+      [
+        0.0,
+        0.0,
+        1.0
+      ]
+    ],
+    "dist": [
+      [
+        -0.64066312,
+        0.41901729,
+        -0.00366722,
+        0.01957175,
+        -0.14404429
+      ]
+    ]
+  },
+  "roi": {
+    "line": {
+      "x": 270,
+      "y": 180,
+      "w": 100,
+      "h": 30
+    },
+    "left_wall": {
+      "x": 0,
+      "y": 0,
+      "w": 100,
+      "h": 150
+    },
+    "right_wall": {
+      "x": 540,
+      "y": 0,
+      "w": 100,
+      "h": 150
+    }
+  },
+  "pd": {
+    "kp": 4.5,
+    "kd": 8.0,
+    "target_black_portion": 0.45
+  },
+  "control": {
+    "speed": 160,
+    "turn_speed": 155,
+    "avoid_speed": 115,
+    "done_sleep_seconds": 1.0,
+    "max_correction": 1,
+    "max_steering_offset": 75,
+    "steering_sign": -1,
+    "drive_sign": -1,
+    "servo_center_deg": 90,
+    "servo_min_deg": 15,
+    "servo_max_deg": 165
+  },
+  "behavior": {
+    "pillars": true,
+    "turns": 120,
+    "fixed_round_dir": 0,
+    "round_dir_vote_threshold": 10,
+    "line_marker_min_portion": 0.25,
+    "turn_delay_seconds": 0,
+    "pillar_turn_delay_seconds": 0.1,
+    "turn_timeout_seconds": 1.1,
+    "state_hold_seconds": 0.25,
+    "finish_delay_seconds": 1.5,
+    "pillar_track_area": 600,
+    "pillar_avoid_area": 2800,
+    "pillar_avoid_cooldown_seconds": 0.55,
+    "pillar_avoid_seconds": 2.2,
+    "pillar_first_phase_seconds": 0.85,
+    "pillar_red_first_correction": 0.95,
+    "pillar_red_second_correction": -0.72,
+    "pillar_green_first_correction": -0.95,
+    "pillar_green_second_correction": 0.72,
+    "pillar_wall_guard": true,
+    "pillar_wall_guard_threshold": 0.62,
+    "pillar_wall_guard_correction": 1.0,
+    "pillar_area_ema_alpha": 0.5,
+    "pillar_lost_hold_frames": 4,
+    "pillar_match_max_dx": 90,
+    "pillar_track_confirm_frames": 1,
+    "pillar_avoid_confirm_frames": 1
+  },
+  "serial": {
+    "enabled": true,
+    "port": "/dev/ttyACM0",
+    "auto_detect": true,
+    "baud_rate": 9600,
+    "ready_timeout_seconds": 6.0,
+    "write_timeout_seconds": 1.0,
+    "send_interval_seconds": 0.02,
+    "keepalive_interval_seconds": 0.15
+  },
+  "web": {
+    "enabled": true,
+    "host": "0.0.0.0",
+    "port": 5000
+  },
+  "decision_log": {
+    "enabled": true,
+    "path": "decision_log.jsonl",
+    "cycle_interval_seconds": 0.25
+  },
+  "debug": {
+    "log_every_seconds": 0.5,
+    "preview_masks": true,
+    "sample_radius": 4,
+    "sample_hue_margin": 8,
+    "sample_sv_margin": 35,
+    "sample_gray_margin": 18,
+    "save_frames": false,
+    "frame_dir": "debug_frames",
+    "frame_every_seconds": 1.0
+  }
+}

package/raspySrc/raspy/settings.py

@@ -0,0 +1,206 @@
+import json
+import os
+from copy import deepcopy
+
+
+DEFAULT_CONFIG = {
+  "version": 2,
+  "filters": {
+    "GRAY": 110,
+    "WALL_MAX_SAT": 90,
+    "WALL_EXCLUDE_COLORS": True,
+    "WALL_EXCLUDE_DILATE": 7,
+    "REDLO": [0, 100, 80],
+    "REDHI": [4, 255, 255],
+    "GREENLO": [30, 80, 30],
+    "GREENHI": [90, 255, 255],
+    "ORANGELO": [7, 20, 10],
+    "ORANGEHI": [37, 255, 255],
+    "BLUELO": [100, 85, 90],
+    "BLUEHI": [130, 255, 255],
+    "PINKLO": [158, 160, 100],
+    "PINKHI": [170, 255, 255]
+  },
+  "contours": {
+    "minSize": 80.0,
+    "pillar_morph_kernel": 5,
+    "pillar_min_width": 4,
+    "pillar_min_height": 8,
+    "pillar_min_fill_ratio": 0.18
+  },
+  "camera": {
+    "frame_width": 640,
+    "frame_height": 480,
+    "format": "RGB888",
+    "crop_height": 258,
+    "flip_180": True,
+    "awb_enable": False,
+    "mtx": [
+      [495.44408865, 0.0, 294.45510998],
+      [0.0, 504.36905411, 259.77204181],
+      [0.0, 0.0, 1.0]
+    ],
+    "dist": [[-0.64066312, 0.41901729, -0.00366722, 0.01957175, -0.14404429]]
+  },
+  "roi": {
+    "line": {"x": 270, "y": 180, "w": 100, "h": 30},
+    "left_wall": {"x": 0, "y": 0, "w": 100, "h": 150},
+    "right_wall": {"x": 540, "y": 0, "w": 100, "h": 150}
+  },
+  "pd": {
+    "kp": 4.5,
+    "kd": 8.0,
+    "target_black_portion": 0.45
+  },
+  "control": {
+    "speed": 100,
+    "turn_speed": 100,
+    "avoid_speed": 100,
+    "done_sleep_seconds": 1.0,
+    "max_correction": 1.0,
+    "max_steering_offset": 55,
+    "steering_sign": -1,
+    "drive_sign": 1,
+    "servo_center_deg": 90,
+    "servo_min_deg": 15,
+    "servo_max_deg": 165
+  },
+  "behavior": {
+    "pillars": False,
+    "turns": 12,
+    "fixed_round_dir": 0,
+    "round_dir_vote_threshold": 10,
+    "line_marker_min_portion": 0.25,
+    "turn_delay_seconds": 0.7,
+    "pillar_turn_delay_seconds": 0.3,
+    "turn_timeout_seconds": 0.85,
+    "state_hold_seconds": 0.4,
+    "finish_delay_seconds": 3.1,
+    "pillar_track_area": 190,
+    "pillar_avoid_area": 530,
+    "pillar_avoid_cooldown_seconds": 1.0,
+    "pillar_avoid_seconds": 2.5,
+    "pillar_first_phase_seconds": 0.95,
+    "pillar_red_first_correction": 0.95,
+    "pillar_red_second_correction": -0.72,
+    "pillar_green_first_correction": -0.95,
+    "pillar_green_second_correction": 0.72,
+    "pillar_wall_guard": True,
+    "pillar_wall_guard_threshold": 0.62,
+    "pillar_wall_guard_correction": 1.0,
+    "pillar_area_ema_alpha": 0.35,
+    "pillar_lost_hold_frames": 3,
+    "pillar_match_max_dx": 90,
+    "pillar_track_confirm_frames": 2,
+    "pillar_avoid_confirm_frames": 2
+  },
+  "serial": {
+    "enabled": True,
+    "port": "/dev/ttyACM0",
+    "auto_detect": True,
+    "baud_rate": 9600,
+    "ready_timeout_seconds": 6.0,
+    "write_timeout_seconds": 1.0,
+    "send_interval_seconds": 0.02,
+    "keepalive_interval_seconds": 0.15
+  },
+  "web": {
+    "enabled": True,
+    "host": "0.0.0.0",
+    "port": 5000
+  },
+  "decision_log": {
+    "enabled": True,
+    "path": "decision_log.jsonl",
+    "cycle_interval_seconds": 0.25
+  },
+  "debug": {
+    "log_every_seconds": 0.5,
+    "preview_masks": True,
+    "sample_radius": 4,
+    "sample_hue_margin": 8,
+    "sample_sv_margin": 35,
+    "sample_gray_margin": 18,
+    "save_frames": False,
+    "frame_dir": "debug_frames",
+    "frame_every_seconds": 1.0
+  }
+}
+
+
+class ConfigLoader:
+  def __init__(self, file_path):
+    self.file_path = file_path
+    self.config = deepcopy(DEFAULT_CONFIG)
+    self.load_config()
+
+  def load_config(self):
+    if not os.path.exists(self.file_path):
+      return
+    with open(self.file_path, "r", encoding="utf-8") as file:
+      loaded = json.load(file)
+    if not isinstance(loaded, dict):
+      raise ValueError("config root must be an object")
+    self.config = merge_dicts(deepcopy(DEFAULT_CONFIG), loaded)
+    self._apply_legacy_keys(loaded)
+
+  def _apply_legacy_keys(self, loaded):
+    if "ArduinoSerialPort" in loaded and not loaded.get("serial", {}).get("port"):
+      self.config["serial"]["port"] = loaded["ArduinoSerialPort"]
+    if "PD" in loaded:
+      pd = loaded["PD"]
+      if "kp" in pd:
+        self.config["pd"]["kp"] = pd["kp"]
+      if "kd" in pd:
+        self.config["pd"]["kd"] = pd["kd"]
+    if "ROI" in loaded and isinstance(loaded["ROI"], dict):
+      self.config["roi"] = merge_dicts(self.config["roi"], loaded["ROI"])
+
+  def get_property(self, key, default=None):
+    return self.config.get(key, default)
+
+  def get_path(self, path, default=None):
+    current = self.config
+    for part in str(path).split("."):
+      if not isinstance(current, dict) or part not in current:
+        return default
+      current = current[part]
+    return current
+
+  def set_path(self, path, value):
+    parts = str(path).split(".")
+    current = self.config
+    for part in parts[:-1]:
+      if part not in current or not isinstance(current[part], dict):
+        current[part] = {}
+      current = current[part]
+    current[parts[-1]] = value
+
+  def save_config(self, path=None):
+    target = path or self.file_path
+    with open(target, "w", encoding="utf-8") as file:
+      json.dump(self.config, file, indent=2)
+      file.write("\n")
+
+
+def merge_dicts(base, overrides):
+  for key, value in overrides.items():
+    if isinstance(value, dict) and isinstance(base.get(key), dict):
+      base[key] = merge_dicts(base[key], value)
+    else:
+      base[key] = value
+  return base
+
+
+def parse_override_value(text):
+  try:
+    return json.loads(text)
+  except json.JSONDecodeError:
+    lowered = text.lower()
+    if lowered == "true":
+      return True
+    if lowered == "false":
+      return False
+    if lowered == "null":
+      return None
+    return text

package/raspySrc/raspy/vehicle-runtime.service

@@ -0,0 +1,12 @@
+[Unit]
+Description=Run WRO vehicle runtime
+After=multi-user.target
+
+[Service]
+Type=simple
+WorkingDirectory=/home/pi/program/onboard
+ExecStart=/usr/bin/python3 /home/pi/program/onboard/robot_runtime.py --pillars
+Restart=on-failure
+
+[Install]
+WantedBy=multi-user.target

package/raspySrc/raspy/vision_pipeline.py

@@ -0,0 +1,142 @@
+import cv2
+import numpy as np
+
+from settings import ConfigLoader
+from vision_types import Pillar
+
+class Pipeline:
+  def __init__(self, configloader: ConfigLoader):
+    self.configloader = configloader
+
+  def undistort(self, image: np.ndarray):
+    mtx, dist = np.array(self.configloader.get_property("camera")['mtx']), np.array(self.configloader.get_property("camera")['dist'])
+    return cv2.undistort(image, mtx, dist, None, mtx)
+
+  def crop(self, image: np.ndarray):
+    crop_height = int(self.configloader.get_property("camera")['crop_height'])
+    return image[crop_height:,:]
+
+  @staticmethod
+  def hsv_range_mask(hsv: np.ndarray, lower, upper):
+    lower = tuple(lower)
+    upper = tuple(upper)
+    if lower[0] <= upper[0]:
+      return cv2.inRange(hsv, lower, upper)
+
+    low_wrap = (0, lower[1], lower[2])
+    high_wrap = upper
+    low_direct = lower
+    high_direct = (179, upper[1], upper[2])
+    return cv2.bitwise_or(
+      cv2.inRange(hsv, low_direct, high_direct),
+      cv2.inRange(hsv, low_wrap, high_wrap),
+    )
+
+  def filter_RG_Bl(self, hsv: np.ndarray, color_image: np.ndarray):
+    """
+    Extracts red/green pillars and the dark wall mask.
+    """
+    filters = self.configloader.get_property("filters")
+    redMin = tuple(filters['REDLO'])
+    redMax = tuple(filters['REDHI'])
+    greenMin = tuple(filters['GREENLO'])
+    greenMax = tuple(filters['GREENHI'])
+    orangeMin = tuple(filters['ORANGELO'])
+    orangeMax = tuple(filters['ORANGEHI'])
+    blueMin = tuple(filters['BLUELO'])
+    blueMax = tuple(filters['BLUEHI'])
+    pinkMin = tuple(filters['PINKLO'])
+    pinkMax = tuple(filters['PINKHI'])
+    grayThresh = int(filters['GRAY'])
+    wallMaxSat = int(filters.get('WALL_MAX_SAT', 90))
+    excludeColors = bool(filters.get('WALL_EXCLUDE_COLORS', True))
+    excludeDilate = int(filters.get('WALL_EXCLUDE_DILATE', 7))
+
+    rMask = self.hsv_range_mask(hsv, redMin, redMax)
+    gMask = self.hsv_range_mask(hsv, greenMin, greenMax)
+    oMask = self.hsv_range_mask(hsv, orangeMin, orangeMax)
+    bMask = self.hsv_range_mask(hsv, blueMin, blueMax)
+    pMask = self.hsv_range_mask(hsv, pinkMin, pinkMax)
+
+    blurredR = cv2.medianBlur(rMask, 5)
+    blurredG = cv2.medianBlur(gMask, 5)
+    grayImage = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY)
+    blurredImg = cv2.GaussianBlur(grayImage, (3, 3), 0)
+    darkMask = cv2.inRange(blurredImg, 0, grayThresh)
+    lowSatMask = cv2.inRange(hsv[:, :, 1], 0, wallMaxSat)
+    blackimg = cv2.bitwise_and(darkMask, lowSatMask)
+
+    if excludeColors:
+      colorMask = cv2.bitwise_or(rMask, gMask)
+      colorMask = cv2.bitwise_or(colorMask, oMask)
+      colorMask = cv2.bitwise_or(colorMask, bMask)
+      colorMask = cv2.bitwise_or(colorMask, pMask)
+      if excludeDilate > 0:
+        kernelSize = excludeDilate if excludeDilate % 2 == 1 else excludeDilate + 1
+        kernel = np.ones((kernelSize, kernelSize), np.uint8)
+        colorMask = cv2.dilate(colorMask, kernel)
+      blackimg = cv2.bitwise_and(blackimg, cv2.bitwise_not(colorMask))
+
+    blackimg = cv2.medianBlur(blackimg, 3)
+    return {"green": blurredG, "red": blurredR, "black": blackimg}
+
+
+  def filter_OB(self, hsv: np.ndarray):
+    """
+    Extracts the orange and blue colors from the image -> this is used to detect the turn markers
+    """
+    orangeMin = tuple(self.configloader.get_property("filters")['ORANGELO'])
+    orangeMax = tuple(self.configloader.get_property("filters")['ORANGEHI'])
+    blueMin = tuple(self.configloader.get_property("filters")['BLUELO'])
+    blueMax = tuple(self.configloader.get_property("filters")['BLUEHI'])
+    oMask = self.hsv_range_mask(hsv, orangeMin, orangeMax)
+    bMask = self.hsv_range_mask(hsv, blueMin, blueMax)
+    # blur images to remove noise
+    blurredO = cv2.medianBlur(oMask, 5)
+    blurredB = cv2.medianBlur(bMask, 5)
+    # return [blurredO, blurredB]
+    return {"orange": blurredO, "blue": blurredB}
+  
+  def filter_parking(self, hsv: np.ndarray):
+    pinkMin = tuple(self.configloader.get_property("filters")['PINKLO'])
+    pinkMax = tuple(self.configloader.get_property("filters")['PINKHI'])
+    pMask = cv2.inRange(hsv, pinkMin, pinkMax)
+    blurredP = cv2.medianBlur(pMask, 5)
+    return blurredP
+
+  def get_pillars(self, imgIn: np.ndarray, type = "RED") -> list[Pillar]:
+    """
+      Extracts pillars from filtered image
+    """
+    contours_config = self.configloader.get_property("contours")
+    minSize = float(contours_config['minSize'])
+    minWidth = int(contours_config.get("pillar_min_width", 4))
+    minHeight = int(contours_config.get("pillar_min_height", 8))
+    minFillRatio = float(contours_config.get("pillar_min_fill_ratio", 0.18))
+    kernelSize = int(contours_config.get("pillar_morph_kernel", 5))
+
+    mask = cv2.medianBlur(imgIn, 5)
+    if kernelSize > 1:
+      kernelSize = kernelSize if kernelSize % 2 == 1 else kernelSize + 1
+      kernel = np.ones((kernelSize, kernelSize), np.uint8)
+      mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
+      mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
+
+    contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    
+    processedContours = []
+    for contour in contours:
+      size = cv2.contourArea(contour)
+      if size > minSize:
+        rx, ry, w, h = cv2.boundingRect(contour)
+        width = int(w)
+        height = int(h)
+        bboxArea = max(1, width * height)
+        fillRatio = float(size) / float(bboxArea)
+
+        if width < minWidth or height < minHeight or fillRatio < minFillRatio:
+          continue
+
+        x = int(rx + width / 2)
+        processedContours.append(Pillar(x, width, height, type, ry, size))
+    return processedContours

package/raspySrc/raspy/vision_types.py

@@ -0,0 +1,42 @@
+import numpy as np
+
+
+class Pillar:
+  ignore = False
+  big_correction = False
+  def __init__(self, screen_x: int, width: int, height: int, color: str, y: int = 0, contour_area: float = 0.0):
+    self.screen_x = screen_x
+    self.width = width
+    self.height = height
+    self.color = color
+    self.y = y
+    self.contour_area = float(contour_area)
+    self.stable_area = None
+    self.seen_frames = 1
+    self.held = False
+
+  @property
+  def x(self):
+    return int(self.screen_x - self.width / 2)
+
+  @property
+  def rect(self):
+    return (self.x, self.y, self.width, self.height)
+
+  @property
+  def raw_area(self):
+    return int(self.width * self.height)
+
+  @property
+  def area(self):
+    if self.stable_area is not None:
+      return int(round(self.stable_area))
+    return self.raw_area
+
+
+def extract_ROI(image: np.ndarray, startxy: list, endxy: list) -> np.ndarray:
+  """
+  Extracts the ROIs from the image
+  """
+  
+  return image[startxy[1]:endxy[1], startxy[0]:endxy[0]]