import sys

sys.setrecursionlimit(2147483646)

import matplotlib

matplotlib.use('TkAgg') # 或 'Qt5Agg'

import matplotlib.pyplot as plt

from matplotlib.widgets import Button

from matplotlib.widgets import Cursor

import json

import numpy as np

import math

import rect

import point_with_arrow

class DebugData(object):

def __init__(self, cost_map, box_center_x, box_center_y,

box_half_width, box_half_length, box_heading_cos, box_heading_sin ):

"""init"""

self.cost_map = cost_map

self.box_center_x = box_center_x

self.box_center_y = box_center_y

self.box_half_width = box_half_width

self.box_half_length = box_half_length

self.box_heading_cos = box_heading_cos

self.box_heading_sin = box_heading_sin

class PathRecordAnalyzer(object):

"""path record analyzer"""

def __init__(self):

"""init"""

self.frame_to_plot = 0

self.frame_data_map = {}

self.init_seq_num = 0

self.zoom_level = 1

self.control_ax = []

# read & parse

if len(sys.argv) < 2:

self.print_help()

exit()

# elif len(sys.argv) == 3:

# if sys.argv[2].isdigit():

# self.init_seq_num = int(sys.argv[2])

# else:

# print("[INPUT ERROR] Start SeqNum Input Should Be A INT Number!")

# self.print_help()

json_path = sys.argv[1]

if json_path == "":

json_path = "/home/caros/cybertron/json/57.json"

# 读取JSON文件

with open(json_path, 'r') as f:

data = json.load(f)

#提取构造cost map的数据

self.sequence_num = data['sequence_num']

self.x_resolution = data['x_resolution']

self.y_resolution = data['y_resolution']

self.transform_tool = np.array(data['transform_map_to_local_'])

self.cost_maps = np.array(data['index_time_x_y'])

# 提取障碍物数据

self.box_center_x = np.array(data['index_time_box_center_x'])

self.box_center_y = np.array(data['index_time_box_center_y'])

self.box_half_width = np.array(data['index_time_box_half_width'])

self.box_half_length = np.array(data['index_time_box_half_length'])

self.box_heading_cos = np.array(data['index_time_box_heading_cos'])

self.box_heading_sin = np.array(data['index_time_box_heading_sin'])

# # 找到最小和最大的x值 TODO

# self.min_x_limit = np.min(self.box_center_x)

# self.max_x_limit = np.max(self.box_center_x)

# # 找到最小和最大的y值

# self.min_y_limit = np.min(self.box_center_y)

# self.max_y_limit = np.max(self.box_center_y)

self.vehicle_x = data["ego_center_x"]

self.vehicle_y = data["ego_center_y"]

self.vehicle_heading = data["ego_heading"]

self.ego_half_length = data["ego_half_length"]

self.ego_half_width = data["ego_half_width"]

print("ego_center_x: ",self.vehicle_x, "ego_center_y: ", self.vehicle_y, "heading: ", self.vehicle_heading, "ego_half_length: ", self.ego_half_length, "ego_half_width: ", self.ego_half_width)

# 获取对应的 sin 和 cos 值

self.ego_sin_value = math.sin(self.vehicle_heading)

self.ego_cos_value = math.cos(self.vehicle_heading)

self.planning_start_point_x = data["planning_start_point_x"]

self.planning_start_point_y = data["planning_start_point_y"]

self.planning_start_point_theta = data["planning_start_point_theta"]

print("planning_start_point_theta: ", self.planning_start_point_theta, "planning_start_point_x: ", self.planning_start_point_x, "planning_start_point_y: ", self.planning_start_point_y)

self.planning_start_point_cos_value = math.cos(self.planning_start_point_theta)

self.planning_start_point_sin_value = math.sin(self.planning_start_point_theta)

for i in range((self.cost_maps.shape[0])):

self.init_one_frame_data(i)

# plot

self.plt_prepare()

plt.show()

def init_one_frame_data(self,i):

"""init data for every time frame, total num is 25"""

if i <= 0 :

self.frame_to_plot = 0

if i >= 25:

self.frame_to_plot = 24

self.frame_to_plot = i

print("time_frame_to_plot ========== ", self.frame_to_plot)

self.frame_data_map[self.frame_to_plot] = DebugData(self.cost_maps[self.frame_to_plot],

self.box_center_x[self.frame_to_plot],

self.box_center_y[self.frame_to_plot],

self.box_half_width[self.frame_to_plot],

self.box_half_length[self.frame_to_plot],

self.box_heading_cos[self.frame_to_plot],

self.box_heading_sin[self.frame_to_plot]

)

# reset to init frame

if 24 == self.frame_to_plot:

self.frame_to_plot = self.init_seq_num

def roation_and_move(self, x_grid, y_grid):

X_flat = x_grid.flatten()

Y_flat = y_grid.flatten()

x_y_index_points = np.vstack((X_flat.ravel(), Y_flat.ravel())).T

# print(x_y_index_points.shape[0])

# 创建一个形状为 (N, 1) 的全1数组，用于扩展

ones_column = np.ones((x_y_index_points.shape[0], 1))

# 使用 numpy.concatenate 沿列方向（axis=1）拼接原数组与全1数组

x_y_local_points = np.concatenate((x_y_index_points, ones_column), axis=1)

# 此时，expanded_arr 的形状为 (N, 3)

# print(x_y_local_points)

# x_y_local_points = np.dot(rotation_matrix2, x_y_local_points.T).T

after_transform = np.dot(self.transform_tool, x_y_local_points.T).T

after_transform2d = after_transform[:, :2]

# 分离旋转后的坐标

print("after_transform2d")

print(after_transform2d)

X_rotated_flat = after_transform2d[:, 0]

Y_rotated_flat = after_transform2d[:, 1]

# 重塑回二维网格的形状

self.X_2d_local = X_rotated_flat.reshape(x_grid.shape)

self.Y_2d_local = Y_rotated_flat.reshape(y_grid.shape)

def find_best_grid_size(self, num_plots):

# 找到最接近平方数的行列组合

side_length = int(np.ceil(np.sqrt(num_plots)))

# 检查side_length的平方是否恰好等于num_plots

if side_length * side_length == num_plots:

return side_length, side_length # 如果是平方数，直接返回

# 如果不是平方数，则找到一个最接近的矩形解

# 从side_length开始递减，直到找到一个能被num_plots整除的数

for r in range(side_length, 1, -1):

if num_plots % r == 0:

cols = r

rows = num_plots // cols

break # 一旦找到解，就跳出循环

# 此时rows和cols是找到的最近似的矩形解，直接返回

return rows, cols

def init_mesh_grid(self):

x_size = self.cost_maps.shape[1]

y_size = self.cost_maps.shape[2]

x_coords = np.linspace(0, (x_size - 1) * self.x_resolution, x_size)

y_coords = np.linspace(0, (y_size - 1) * self.y_resolution, y_size)

print("x_coords shape: ", x_coords.shape, ", y_coords shape: ", y_coords.shape)

X1, Y1 = np.meshgrid(x_coords, y_coords, indexing='xy')

print("X1 shape: ", X1.shape, "Y1 shape: ", Y1.shape)

self.roation_and_move(X1, Y1)

print("X_2d_local shape: ", self.X_2d_local.shape, "Y_2d_local shape: ", self.Y_2d_local.shape)

def plt_prepare(self):

"""plt related prepare"""

self.init_mesh_grid()

self.fig = plt.figure(figsize=(20, 20))

self.plot_frame()

self.path_cursor = Cursor(self.axis, useblit=True, color='dodgerblue', linewidth=1)

self.callback = Index(self)

prev1frame = plt.axes([0.2, 0.01, 0.1, 0.05])

prev10frame = plt.axes([0.3, 0.01, 0.1, 0.05])

next1frame = plt.axes([0.4, 0.01, 0.1, 0.05])

next10frame = plt.axes([0.5, 0.01, 0.1, 0.05])

zoom_out = plt.axes([0.6, 0.01, 0.1, 0.05])

zoom_in = plt.axes([0.7, 0.01, 0.1, 0.05])

exitframe = plt.axes([0.8, 0.01, 0.1, 0.05])

self.bprev1 = Button(prev1frame, '-1')

self.bprev1.on_clicked(self.callback.prev1)

self.bprev10 = Button(prev10frame, '-10')

self.bprev10.on_clicked(self.callback.prev10)

self.bnext1 = Button(next1frame, '+1')

self.bnext1.on_clicked(self.callback.next1)

self.bnext10 = Button(next10frame, '+10')

self.bnext10.on_clicked(self.callback.next10)

self.zoom_out = Button(zoom_out, 'z_out')

self.zoom_out.on_clicked(self.callback.zoom_out)

self.zoom_in = Button(zoom_in, 'z_in')

self.zoom_in.on_clicked(self.callback.zoom_in)

self.bexit = Button(exitframe, 'exit')

self.bexit.on_clicked(self.callback.exit)

def plot_frame(self, zoom_level = 1):

"""plot frame_to_plot in frame_data_map"""

plt.style.use('_mpl-gallery-nogrid')

# title = "cost map json debug tool" + "\n" + " json: " + str(self.sequence_num) + " plot: " + str(self.frame_to_plot)

# self.fig.text(0.45, 1.0, # 左上角的位置 (x, y)

# title,

# fontsize=12, # 可以根据需要调整字体大小

# verticalalignment='top')

for ax in self.control_ax:

self.fig.delaxes(ax)

self.control_ax = []

if zoom_level == 1:

self.axis = self.fig.add_subplot(111)

self.control_ax.append(self.axis)

title = "cost map json debug tool" + "\n" + "json: " + str(self.sequence_num) + " plot: " + str(self.frame_to_plot)

self.axis.set_title(title)

self.axis.set_xlabel("X Coordinate (m)")

self.axis.set_ylabel("Y Coordinate (m)")

single_cost_map = self.frame_data_map[self.frame_to_plot].cost_map

self.plot_single_cost_map(self.axis, single_cost_map)

box_center_x = self.frame_data_map[self.frame_to_plot].box_center_x

box_center_y = self.frame_data_map[self.frame_to_plot].box_center_y

box_heading_cos = self.frame_data_map[self.frame_to_plot].box_heading_cos

box_heading_sin = self.frame_data_map[self.frame_to_plot].box_heading_sin

box_half_length = self.frame_data_map[self.frame_to_plot].box_half_length

box_half_width = self.frame_data_map[self.frame_to_plot].box_half_width

self.plot_obs(self.axis, box_center_x, box_center_y, box_heading_cos, box_heading_sin, box_half_length, box_half_width)

self.plot_ego_box(self.axis)

self.plot_start_point(self.axis)

self.axis.legend()

# self.axis.set_ylim(-10.0, 10.0)

# TODO

# if len(lk_debug.path_opt) != 0:

# self.axis.set_xlim(lk_debug.path_opt[0].s - 5.0, lk_debug.path_opt[-1].s + 5.0)

self.axis.grid(True)

self.axis.set_aspect(1)

plt.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.1)

else:

plot_num = zoom_level * zoom_level

rows, cols = self.find_best_grid_size(plot_num)

for i in range(plot_num):

r, c = divmod(i, cols)

print("current self.frame_to_plot + i ", self.frame_to_plot + i ," r, c ", r, c, "rows, cols ", rows, cols, "i + 1: ", i+ 1, "zoom_level: ", zoom_level)

self.axis = self.fig.add_subplot(rows, cols, i + 1)

self.control_ax.append(self.axis)

# 设置子图的标题，以便区分它们

ax = self.axis

title = " json: " + str(self.sequence_num) + " plot: " + str(self.frame_to_plot + i)

ax.set_title(title, fontsize=12 - zoom_level) # 这里将字体大小设置为12

# ax.set_xlabel(" X (m)")

# ax.set_ylabel(" Y (m)")

# ax.contour(X_2d_local, Y_2d_local, cost_maps[i], levels=levels)

single_cost_map = self.frame_data_map[self.frame_to_plot + i].cost_map

self.plot_single_cost_map(ax, single_cost_map)

box_center_x = self.frame_data_map[self.frame_to_plot + i].box_center_x

box_center_y = self.frame_data_map[self.frame_to_plot + i].box_center_y

box_heading_cos = self.frame_data_map[self.frame_to_plot + i].box_heading_cos

box_heading_sin = self.frame_data_map[self.frame_to_plot + i].box_heading_sin

box_half_length = self.frame_data_map[self.frame_to_plot + i].box_half_length

box_half_width = self.frame_data_map[self.frame_to_plot + i].box_half_width

self.plot_obs(ax, box_center_x, box_center_y, box_heading_cos, box_heading_sin, box_half_length, box_half_width)

self.plot_ego_box(ax)

self.plot_start_point(ax)

ax.tick_params(axis='both', which='major', labelsize=12 - zoom_level)

self.axis.grid(True)

# plot_obs(i, ax)

# plot_ego_box(ax)

# plot_start_point(ax)

plt.style.use('_mpl-gallery-nogrid')

# plt.tight_layout()

# self.axis.set_aspect(1)

plt.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.1)

print ("self.frame_to_plot11", self.frame_to_plot)

def plot_single_cost_map(self, ax, single_cost_map):

ax.pcolormesh(self.X_2d_local, self.Y_2d_local, single_cost_map.T)

# self.axis.plot(self.X_2d_local, self.Y_2d_local, 'b--', label='left lane bound')

def plot_obs(self, ax, box_center_x, box_center_y,

box_heading_cos, box_heading_sin, box_half_length, box_half_width):

obs_size = box_center_x.shape[0]

for i in range(obs_size):

# if i == 1:

# print("box_center_x[i] is ",box_center_x[i])

rect_obj = rect.Rectangle(box_center_x[i], box_center_y[i], box_heading_cos[i],

box_heading_sin[i], box_half_length[i], box_half_width[i])

rect_obj.draw_rectangle_and_arrow(ax, edgecolor="b")

def plot_ego_box(self, ax):

rect_obj = rect.Rectangle(self.vehicle_x, self.vehicle_y, self.ego_cos_value, self.ego_sin_value,

self.ego_half_length, self.ego_half_width)

rect_obj.draw_rectangle_and_arrow(ax, edgecolor="r", label = "Ego Vehicle Box")

def plot_start_point(self, ax):

point_obj = point_with_arrow.PointWithArrow(self.planning_start_point_x, self.planning_start_point_y,

self.planning_start_point_cos_value, self.planning_start_point_sin_value)

point_obj.draw(ax)

def print_help(self):

"""print help"""

print("****** How To Use path_record_analyzer.py ****** ")

print("python3 cost_map_debug.py\t[json_file]")

class MouseEventManager(object):

"""MouseEventManager"""

x, y = 0.0, 0.0

xoffset, yoffset = -20, 20

text_template = 'x: %0.2f\ny: %0.2f'

annotation = False

def on_click(self, event):

"""click callback"""

# if mouse button is not right, return

# 1: left, 2: middle, 3: right

if event.button != 3:

return

self.x, self.y = event.xdata, event.ydata

if self.x is not None:

print('mouse click x: %.2f, y: %.2f' % (event.xdata, event.ydata))

if self.annotation:

self.annotation.set_visible(False)

label_text = self.text_template % (self.x, self.y)

self.annotation = event.inaxes.annotate(label_text,

xy=(self.x, self.y), xytext=(self.xoffset, self.yoffset),

textcoords='offset points', ha='right', va='bottom',

bbox=dict(boxstyle='round,pad=0.5', fc='lightcyan', alpha=0.5),

arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0')

)

self.annotation.set_visible(True)

self.annotation.figure.canvas.draw()

class Index(object):

"""button callback function"""

def __init__(self, path_record_analyzer):

self.path_record_analyzer = path_record_analyzer

self.reset_mouse_event()

def reset_mouse_event(self):

"""reset mouse event"""

self.mouse_manager = MouseEventManager()

self.path_record_analyzer.fig.canvas.mpl_connect('button_release_event', self.mouse_manager.on_click)

def next(self, step):

"""next button callback function"""

self.path_record_analyzer.frame_to_plot += step

if self.path_record_analyzer.frame_to_plot not in self.path_record_analyzer.frame_data_map:

print("[ERROR][next] target frame " + str(self.path_record_analyzer.frame_to_plot) + " not found!")

self.path_record_analyzer.frame_to_plot -= step

return

self.path_record_analyzer.plot_frame()

plt.show()

self.reset_mouse_event()

def internal_zoom_out(self):

"""zoom out button callback function"""

zoom_level = self.path_record_analyzer.zoom_level + 1

if zoom_level > 5:

print("[ERROR][internal_zoom_out] max zoom level reached: ", zoom_level)

zoom_level = zoom_level - 1

self.path_record_analyzer.zoom_level = zoom_level

num = zoom_level * zoom_level

frame_to_plot = self.path_record_analyzer.frame_to_plot + num - 1

if frame_to_plot not in self.path_record_analyzer.frame_data_map:

print("[ERROR][next] target frame " + str(frame_to_plot) + " not found! ")

self.path_record_analyzer.frame_to_plot = 25 - num

print("self.path_record_analyzer.frame_to_plot," , self.path_record_analyzer.frame_to_plot)

self.path_record_analyzer.plot_frame(self.path_record_analyzer.zoom_level)

if frame_to_plot not in self.path_record_analyzer.frame_data_map:

print("[ERROR][next] target frame " + str(frame_to_plot) + " not found! ")

self.path_record_analyzer.frame_to_plot = 24

else:

self.path_record_analyzer.frame_to_plot = frame_to_plot

plt.show()

self.reset_mouse_event()

def internal_zoom_in(self):

"""zoom in button callback function"""

zoom_level = self.path_record_analyzer.zoom_level - 1

if zoom_level < 1:

print("[ERROR][internal_zoom_in] max zoom level reached: ", zoom_level)

zoom_level = zoom_level + 1

self.path_record_analyzer.zoom_level = zoom_level

num = zoom_level * zoom_level

frame_to_plot = self.path_record_analyzer.frame_to_plot + num

if frame_to_plot not in self.path_record_analyzer.frame_data_map:

print("[ERROR][next] target frame " + str(frame_to_plot) + " not found! ")

self.path_record_analyzer.frame_to_plot = 25 - num

self.path_record_analyzer.plot_frame(self.path_record_analyzer.zoom_level)

if frame_to_plot not in self.path_record_analyzer.frame_data_map:

print("[ERROR][next] target frame " + str(frame_to_plot) + " not found! ")

self.path_record_analyzer.frame_to_plot = 24

else:

self.path_record_analyzer.frame_to_plot = frame_to_plot

plt.show()

self.reset_mouse_event()

def next1(self, event):

"""next 1 button callback function"""

self.next(1)

def next10(self, event):

"""next 10 button callback function"""

self.next(10)

def zoom_out(self, event):

self.internal_zoom_out()

def zoom_in(self, event):

self.internal_zoom_in()

def prev(self, step):

"""prev button callback function"""

self.path_record_analyzer.frame_to_plot -= step

if self.path_record_analyzer.frame_to_plot not in self.path_record_analyzer.frame_data_map:

print("[ERROR][prev] target frame " + str(self.path_record_analyzer.frame_to_plot) + " not found!")

self.path_record_analyzer.frame_to_plot += step

return

self.path_record_analyzer.plot_frame()

plt.show()

self.reset_mouse_event()

def prev1(self, event):

"""prev button callback function"""

self.prev(1)

def prev10(self, event):

"""prev button callback function"""

self.prev(10)

def exit(self, event):

"""next thread button callback function"""

sys.exit(0)

if __name__ == "__main__":

foo = PathRecordAnalyzer()