90 lines
2.7 KiB
Python
90 lines
2.7 KiB
Python
"""
|
|
navlogger.py — Robot Tracker v009
|
|
Zeichnet alle Navigationsdaten auf.
|
|
Kompatibel mit speed_l/speed_r Befehlen (mm/s).
|
|
"""
|
|
|
|
import csv
|
|
import logging
|
|
import math
|
|
import time
|
|
from pathlib import Path
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
class NavLogger:
|
|
def __init__(self, log_dir: str = "logs"):
|
|
self.log_dir = Path(log_dir)
|
|
self.log_dir.mkdir(exist_ok=True)
|
|
self.file = None
|
|
self.writer = None
|
|
self.session_start = None
|
|
self._open_new_file()
|
|
|
|
def _open_new_file(self):
|
|
ts = time.strftime("%Y%m%d_%H%M%S")
|
|
path = self.log_dir / f"nav_{ts}.csv"
|
|
self.file = open(path, "w", newline="")
|
|
self.writer = csv.writer(self.file)
|
|
self.writer.writerow([
|
|
"timestamp",
|
|
"elapsed_s",
|
|
"robot_x_mm", "robot_y_mm", "robot_theta",
|
|
"goal_x_mm", "goal_y_mm",
|
|
"dist_to_goal_mm",
|
|
"angle_error_deg",
|
|
"cmd_speed_l", "cmd_speed_r", "cmd_status",
|
|
"num_obstacles",
|
|
"nearest_obstacle_id",
|
|
"nearest_obstacle_dist_mm",
|
|
"all_obstacle_dists",
|
|
])
|
|
self.session_start = time.time()
|
|
logger.info(f"NavLogger: Neue Log-Datei {path}")
|
|
|
|
def log(
|
|
self,
|
|
robot_x: float, robot_y: float, robot_theta: float,
|
|
goal_x: float, goal_y: float,
|
|
cmd: dict,
|
|
obstacles: list,
|
|
angle_error: float,
|
|
):
|
|
now = time.time()
|
|
elapsed = round(now - self.session_start, 3)
|
|
dist_goal = math.sqrt((goal_x - robot_x)**2 + (goal_y - robot_y)**2)
|
|
|
|
nearest_id = None
|
|
nearest_dist = float("inf")
|
|
obs_dists = []
|
|
|
|
for obs in obstacles:
|
|
d = math.sqrt((robot_x - obs["x"])**2 + (robot_y - obs["y"])**2)
|
|
obs_dists.append(f"{obs['id']}:{d:.0f}")
|
|
if d < nearest_dist:
|
|
nearest_dist = d
|
|
nearest_id = obs["id"]
|
|
|
|
# Kompatibel mit speed_l/speed_r (v009) und left/right (v008)
|
|
speed_l = cmd.get("speed_l", cmd.get("left", 0))
|
|
speed_r = cmd.get("speed_r", cmd.get("right", 0))
|
|
|
|
self.writer.writerow([
|
|
round(now, 3),
|
|
elapsed,
|
|
round(robot_x, 1), round(robot_y, 1), round(robot_theta, 1),
|
|
round(goal_x, 1), round(goal_y, 1),
|
|
round(dist_goal, 1),
|
|
round(angle_error, 1),
|
|
round(speed_l, 1), round(speed_r, 1), cmd.get("status", ""),
|
|
len(obstacles),
|
|
nearest_id,
|
|
round(nearest_dist, 1) if nearest_dist < float("inf") else "",
|
|
" | ".join(obs_dists),
|
|
])
|
|
self.file.flush()
|
|
|
|
def close(self):
|
|
if self.file:
|
|
self.file.close()
|