A-SW Hub / Repositories
Local Gitea · READ ONLY
← 저장소 Repositories
REPOSITORY

daedongrobotics/greenhouse-lps-navigation Private

[참조 MVP] 온실 LPS 측위·주행 — 온실 로컬 측위값과 작업열 경로로 이동 명령을 생성하는 참조 MVP입니다.

https://git.agrithing.ai/daedongrobotics/greenhouse-lps-navigation.git
4 commits
2 items · feat/concrete-mvp-v2
·/ asw_component/component.py
python · 1.1 KB · 954e529 Download
from __future__ import annotationsfrom math import atan2, degrees, hypotdef execute(payload: dict) -> dict:    pose, goal = payload["pose"], payload["goal"]    quality = float(payload.get("localizationQuality", 0))    threshold = float(payload.get("minimumQuality", 0.75))    dx, dy = float(goal["x"]) - float(pose["x"]), float(goal["y"]) - float(pose["y"])    distance = hypot(dx, dy)    desired = degrees(atan2(dy, dx)) if distance else float(pose.get("headingDeg", 0))    heading_error = (desired - float(pose.get("headingDeg", 0)) + 180) % 360 - 180    accepted = quality >= threshold    arrived = accepted and distance <= 0.1    speed = 0.0 if not accepted or arrived else min(float(payload.get("maxSpeedMps", 0.8)), max(0.15, distance * 0.4))    status = "LOCALIZATION_DEGRADED" if not accepted else ("ARRIVED" if arrived else "NAVIGATING")    return {"component": "daedongrobotics/greenhouse-lps-navigation", "accepted": accepted,            "status": status, "target": "lpu", "distanceM": round(distance, 3),            "headingErrorDeg": round(heading_error, 2), "speedMps": round(speed, 3),            "command": "STOP" if speed == 0 else "DRIVE"}