根据日志绘制障碍物轮廓点和中心点

news2024/12/24 20:53:43

绘制log中的障碍物凸包点，首先给出log日志中的障碍物的凸包点

[Info]-[PointCloudHandle:88]:[2024-07-14,09:55:41.052]-back obj size 6
[Info]-[PointCloudHandle:92]:[2024-07-14,09:55:41.052]-back obj size 6 cur idx 1
[Info]-[PointCloudHandle:93]:[2024-07-14,09:55:41.052]-back obj center_x 9.247219 center_y -14.778276 center z:2.492869
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:8.02 y:-15.56,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:8.02 y:-14.04,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:9.96 y:-14.04,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:9.96 y:-15.56,
[Info]-[PointCloudHandle:92]:[2024-07-14,09:55:41.052]-back obj size 6 cur idx 2
[Info]-[PointCloudHandle:93]:[2024-07-14,09:55:41.052]-back obj center_x 8.240705 center_y -14.276690 center z:2.027562
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:8.05 y:-14.38,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:8.05 y:-14.19,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:8.43 y:-14.19,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:8.43 y:-14.38,
[Info]-[PointCloudHandle:92]:[2024-07-14,09:55:41.052]-back obj size 6 cur idx 3
[Info]-[PointCloudHandle:93]:[2024-07-14,09:55:41.052]-back obj center_x -1.659360 center_y -9.992173 center z:2.039360
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:-7.11 y:-0.68,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:-5.84 y:-0.23,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:-4.99 y:-1.12,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:7.44 y:-13.94,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:7.57 y:-15.43,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:6.11 y:-16.12,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:4.85 y:-16.36,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:-5.24 y:-14.37,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:9 x:-6.33 y:-13.56,
[Info]-[PointCloudHandle:92]:[2024-07-14,09:55:41.052]-back obj size 6 cur idx 4
[Info]-[PointCloudHandle:93]:[2024-07-14,09:55:41.052]-back obj center_x -16.760958 center_y -4.745333 center z:1.604066
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-21.27 y:-5.08,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-21.42 y:-4.62,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-19.36 y:-2.77,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-18.59 y:-2.75,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-15.66 y:-3.99,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-13.77 y:-4.58,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-15.86 y:-7.12,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:8 x:-16.02 y:-7.95,
[Info]-[PointCloudHandle:92]:[2024-07-14,09:55:41.052]-back obj size 6 cur idx 5
[Info]-[PointCloudHandle:93]:[2024-07-14,09:55:41.052]-back obj center_x 7.400077 center_y -4.330252 center z:1.465278
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:6 x:5.70 y:-6.68,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:6 x:6.36 y:-0.28,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:6 x:8.30 y:-0.29,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:6 x:9.13 y:-8.90,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:6 x:8.72 y:-10.07,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:6 x:7.91 y:-9.27,
[Info]-[PointCloudHandle:92]:[2024-07-14,09:55:41.052]-back obj size 6 cur idx 6
[Info]-[PointCloudHandle:93]:[2024-07-14,09:55:41.052]-back obj center_x -7.823660 center_y -0.408213 center z:1.034263
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:-7.99 y:-0.49,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:-7.99 y:-0.32,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:-7.50 y:-0.32,
[Info]-[PointCloudHandle:98]:[2024-07-14,09:55:41.052]-obj_num:4 x:-7.50 y:-0.49,

其中代码如下:

import re
import matplotlib.pyplot as plt

# 假设日志文件名为 'log_file.txt'
log_file = 'log_file.txt'

# 读取日志文件内容
with open(log_file, 'r') as file:
    lines = file.readlines()

# 解析日志文件内容
obstacles = []
current_obstacle = None

for line in lines:
    if 'cur idx' in line:
        if current_obstacle:
            obstacles.append(current_obstacle)
        current_obstacle = {'center': None, 'points': []}
    elif 'back obj center_x' in line:
        match = re.search(r'center_x (\S+) center_y (\S+)', line)
        if match:
            center_x, center_y = float(match.group(1)), float(match.group(2))
            current_obstacle['center'] = (center_x, center_y)
    elif 'obj_num' in line:
        match = re.search(r'x:(\S+) y:(\S+)', line)
        if match:
            point_x, point_y = match.group(1).replace(',', ''), match.group(2).replace(',', '')
            point_x, point_y = float(point_x), float(point_y)
            current_obstacle['points'].append((point_x, point_y))

if current_obstacle:
    obstacles.append(current_obstacle)

# 可视化障碍物
plt.figure(figsize=(10, 10))
for obs in obstacles:
    center = obs['center']
    points = obs['points']
    
    # 绘制中心点
    plt.scatter(center[0], center[1], color='red', s=100, label='Center' if obs == obstacles[0] else "")
    
    # 绘制轮廓点
    points_x = [p[0] for p in points]
    points_y = [p[1] for p in points]
    plt.scatter(points_x, points_y, color='blue', s=50, label='Contour Point' if obs == obstacles[0] else "")
    
    # 绘制轮廓线
    points_x.append(points_x[0])  # 闭合轮廓
    points_y.append(points_y[0])  # 闭合轮廓
    plt.plot(points_x, points_y, color='green', linewidth=2, label='Contour Line' if obs == obstacles[0] else "")

plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.title('Obstacles Visualization')
plt.grid(True)
plt.show()