1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
| #!/usr/bin/env python
| # 检查轨迹详细数据
|
| with open('controller_log.csv', 'r') as f:
| lines = f.readlines()
|
| # 找到 reached_start
| reached_idx = None
| for i, line in enumerate(lines):
| if 'reached_start' in line:
| reached_idx = i
| break
|
| if reached_idx:
| print(f"Found reached_start at line {reached_idx}")
|
| # 前20帧的详细数据
| print("\nFirst 20 frames after reached_start:")
| count = 0
| for i in range(reached_idx + 1, min(reached_idx + 100, len(lines))):
| if 'running' in lines[i]:
| parts = lines[i].strip().split(',')
| if len(parts) >= 11:
| t, x, y, heading, tx, ty, tdist, tidx, xte, herr = parts[:10]
| print(f"t={t:>6s} pos=({x:>6s},{y:>6s}) h={heading:>5s} target=({tx:>6s},{ty:>6s}) idx={tidx:>3s} xte={xte:>6s} h_err={herr:>6s}")
| count += 1
| if count >= 20:
| break
|
| # 多个时间点的位置
| print("\n\nPosition every 10 seconds:")
| target_times = [10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120]
| for target_t in target_times:
| for i in range(reached_idx + 1, len(lines)):
| if 'running' in lines[i]:
| parts = lines[i].strip().split(',')
| if len(parts) >= 3:
| t = float(parts[0])
| if t >= target_t:
| x, y, heading = parts[1], parts[2], parts[3]
| print(f"t={t:>6.1f}s: pos=({x:>7s},{y:>7s}) heading={heading:>6s}")
| break
|
|
