guo@guo-Dell-G15-5520:~/8.4$ python odom_tra.py
Exception in Tkinter callback
Traceback (most recent call last):
File "/usr/lib/python3.8/tkinter/__init__.py", line 1892, in __call__
return self.func(*args)
File "odom_tra.py", line 264, in on_save_button_click
extracted_data = extract_data(log_file, start_line, end_line)
File "odom_tra.py", line 22, in extract_data
with open(log_file, 'r') as f:
FileNotFoundError: [Errno 2] No such file or directory: '/home/guo/tool/python脚本/imu_integration.log'
源代码为：import re
import csv
from datetime import datetime, timedelta
from tkinter import Tk, Label, Entry, Button, StringVar, messagebox, filedialog
import matplotlib.pyplot as plt
def parse_timestamp(timestamp_str):
formats = ['%Y-%m-%d %H:%M:%S,%f', '%Y-%m-%d %H:%M:%S']
for fmt in formats:
try:
return datetime.strptime(timestamp_str, fmt)
except ValueError:
continue
raise ValueError(f"Time data '{timestamp_str}' does not match any known format.")
def extract_data(log_file, start_line, end_line):
odom_data = []
map_data = []
robot_freq_dict = {}
imu_freq_dict = {}
with open(log_file, 'r') as f:
for i, line in enumerate(f, 1):
if start_line <= i <= end_line:
parts = line.split(',')
if len(parts) < 2:
continue
timestamp_part = parts[0].strip()
timestamp_parts = timestamp_part.split()
if len(timestamp_parts) < 2:
continue
timestamp_str = timestamp_parts[0] + ' ' + timestamp_parts[1]
try:
timestamp = parse_timestamp(timestamp_str)
except ValueError:
print(f"Failed to parse timestamp: {timestamp_str}")
continue
# Extract odom data
odom_match = re.search(r'odom = ([-+\d.eE+-]+),([-+\d.eE+-]+),([-+\d.eE+-]+),([-+\d.eE+-]+),([-+\d.eE+-]+),([-+\d.eE+-]+),([-+\d.eE+-]+),([-+\d.eE+-]+)', line)
if odom_match:
try:
odom_values = list(map(float, odom_match.groups()))
odom_data.append([timestamp] + odom_values)
except ValueError:
print(f"Failed to convert odom values: {odom_match.groups()}")
continue
# Extract map data
map_match = re.search(r'map = ([-+\d.eE+-]+),([-+\d.eE+-]+)(?:,\d+)?', line)
if map_match:
try:
x, y = map(float, map_match.groups())
map_data.append((x, y))
except ValueError:
print(f"Failed to convert map values: {map_match.groups()}")
continue
# Extract robot data frequency
robot_freq_match = re.search(r'@@@robot data frequency = (\d+\.\d+)Hz', line)
if robot_freq_match:
try:
freq = float(robot_freq_match.group(1))
robot_freq_dict[timestamp] = freq
except ValueError:
print(f"Failed to convert robot frequency: {robot_freq_match.group(1)}")
continue
# Extract IMU data frequency
imu_freq_match = re.search(r'@@@ IMU data frequency = (\d+\.\d+) Hz', line)
if imu_freq_match:
try:
freq = float(imu_freq_match.group(1))
imu_freq_dict[timestamp] = freq
except ValueError:
print(f"Failed to convert IMU frequency: {imu_freq_match.group(1)}")
continue
elif i > end_line:
break
robot_timestamps = sorted(robot_freq_dict.keys())
robot_freqs = [robot_freq_dict[t] for t in robot_timestamps]
imu_timestamps = sorted(imu_freq_dict.keys())
imu_freqs = [imu_freq_dict[t] for t in imu_timestamps]
if robot_timestamps and imu_timestamps:
base_time = min(robot_timestamps[0], imu_timestamps[0])
elif robot_timestamps:
base_time = robot_timestamps[0]
elif imu_timestamps:
base_time = imu_timestamps[0]
else:
base_time = None
if base_time is not None:
relative_odom_timestamps = [(t - base_time).total_seconds() for t in [data[0] for data in odom_data]]
relative_robot_timestamps = [(t - base_time).total_seconds() for t in robot_timestamps]
relative_imu_timestamps = [(t - base_time).total_seconds() for t in imu_timestamps]
else:
relative_odom_timestamps = []
relative_robot_timestamps = []
relative_imu_timestamps = []
return odom_data, map_data, relative_odom_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, robot_timestamps, imu_timestamps, relative_robot_timestamps, relative_imu_timestamps
def save_to_csv(odom_data, map_data, relative_robot_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, robot_timestamps, imu_timestamps):
# Save Odom Data to CSV
with open('odom_data.csv', mode='w', newline='') as file:
writer = csv.writer(file)
writer.writerow(['Timestamp', 'X', 'Y', 'X_Velocity', 'Y_Velocity', 'Odometry_Heading_Angle', 'IMU_Heading_Angle', 'Slope_Magnitude', 'Slope_Heading_Angle'])
writer.writerows(odom_data)
# Save Map Data to CSV
with open('map_data.csv', mode='w', newline='') as file:
writer = csv.writer(file)
writer.writerow(['X', 'Y'])
writer.writerows(map_data)
# Save Robot Frequency Data to CSV
with open('robot_frequency_data.csv', mode='w', newline='') as file:
writer = csv.writer(file)
writer.writerow(['Relative Time (s)', 'Frequency (Hz)', 'Timestamp'])
for rt, freq, ts in zip(relative_robot_timestamps, robot_freqs, robot_timestamps):
writer.writerow([rt, freq, ts.isoformat()])
# Save IMU Frequency Data to CSV
with open('imu_frequency_data.csv', mode='w', newline='') as file:
writer = csv.writer(file)
writer.writerow(['Relative Time (s)', 'Frequency (Hz)', 'Timestamp'])
for rt, freq, ts in zip(relative_imu_timestamps, imu_freqs, imu_timestamps):
writer.writerow([rt, freq, ts.isoformat()])
def plot_data(odom_data, map_data, relative_odom_timestamps, relative_robot_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, additional_points=None):
# Clear previous plots
plt.close('all')
# Plot Odom Data
plt.figure(figsize=(16, 18))
plt.subplot(3, 3, 1)
odom_x, odom_y = zip(*[(data[1], data[2]) for data in odom_data])
plt.plot(odom_x, odom_y, marker='o')
plt.title('Odom Data (X vs Y)')
plt.xlabel('X')
plt.ylabel('Y')
plt.axis('equal')
# Plot Map Data
plt.subplot(3, 3, 2)
map_x, map_y = zip(*map_data)
plt.plot(map_x, map_y, marker='o', color='green', label='Map')
if additional_points:
for point in additional_points:
plt.scatter(point[0], point[1], color='red', zorder=5, label='Additional Point' if additional_points.index(point) == 0 else "")
plt.title('Map Data (X vs Y)')
plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.axis('equal')
# Add zoom button for Map Data
def zoom_map_plot():
plt.figure(figsize=(12, 8))
plt.plot(map_x, map_y, marker='o', color='green', label='Map')
if additional_points:
for point in additional_points:
plt.scatter(point[0], point[1], color='red', zorder=5, label='Additional Point' if additional_points.index(point) == 0 else "")
plt.title('Zoomed Map Data (X vs Y)')
plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.grid(True)
plt.axis('equal') # Ensure equal scaling on both axes
plt.show()
map_zoom_button = Button(root, text="Zoom Map", command=zoom_map_plot)
map_zoom_button.grid(row=14, columnspan=2, pady=5)
# Plot Odom and Map Comparison
plt.subplot(3, 3, 3)
plt.plot(odom_x, odom_y, marker='o', label='Odom')
plt.plot(map_x, map_y, marker='x', color='green', label='Map')
if additional_points:
for point in additional_points:
plt.scatter(point[0], point[1], color='red', zorder=5, label='Additional Point' if additional_points.index(point) == 0 else "")
plt.title('Odom vs Map Comparison')
plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.axis('equal')
# Define a function to zoom into a specific plot
def zoom_plot(title, y_label, y_values):
plt.figure(figsize=(12, 8))
plt.plot(relative_odom_timestamps, y_values, marker='o', color='blue')
plt.title(title)
plt.xlabel('Relative Time (s)')
plt.ylabel(y_label)
plt.grid(True)
plt.show()
# Create buttons for each plot
buttons = [
("X Axis Velocity", "X Velocity", [data[3] for data in odom_data]),
("Y Axis Velocity", "Y Velocity", [data[4] for data in odom_data]),
("Odometry Heading Angle", "Odometry Heading Angle", [data[5] for data in odom_data]),
("IMU Heading Angle", "IMU Heading Angle", [data[6] for data in odom_data]),
("Slope Magnitude", "Slope Magnitude", [data[7] for data in odom_data]),
("Slope Heading Angle", "Slope Heading Angle", [data[8] for data in odom_data])
]
for idx, (label, y_label, y_values) in enumerate(buttons):
button = Button(root, text=f"Zoom {label}", command=lambda l=label, yl=y_label, v=y_values: zoom_plot(l, yl, v))
button.grid(row=idx+7, columnspan=2, pady=5)
# Add button for plotting IMU vs Odometry Heading Angle
def plot_heading_angle_comparison():
plt.figure(figsize=(12, 8))
plt.plot(relative_odom_timestamps, [data[5] for data in odom_data], marker='o', label='Odometry Heading Angle')
plt.plot(relative_odom_timestamps, [data[6] for data in odom_data], marker='x', color='orange', label='IMU Heading Angle')
plt.title('IMU vs Odometry Heading Angle')
plt.xlabel('Relative Time (s)')
plt.ylabel('Heading Angle (rad)')
plt.legend()
plt.grid(True)
plt.show()
heading_angle_compare_button = Button(root, text="Compare IMU & Odometry Heading Angles", command=plot_heading_angle_comparison)
heading_angle_compare_button.grid(row=15, columnspan=2, pady=5)
# Plot Robot Frequency Data
plt.subplot(3, 3, 4)
plt.plot(relative_robot_timestamps, robot_freqs, marker='o', color='purple')
plt.title('Robot Frequency Data')
plt.xlabel('Relative Time (s)')
plt.ylabel('Frequency (Hz)')
plt.grid(True)
# Plot IMU Frequency Data
plt.subplot(3, 3, 5)
plt.plot(relative_imu_timestamps, imu_freqs, marker='o', color='brown')
plt.title('IMU Frequency Data')
plt.xlabel('Relative Time (s)')
plt.ylabel('Frequency (Hz)')
plt.grid(True)
plt.tight_layout()
plt.show()
def on_save_button_click():
try:
start_line = int(start_line_var.get().strip())
end_line = int(end_line_var.get().strip())
if start_line >= end_line:
raise ValueError("Start line must be less than end line.")
log_file = '/home/guo/tool/python脚本/imu_integration.log'
# 提取数据
extracted_data = extract_data(log_file, start_line, end_line)
# 解包数据
global odom_data, map_data, relative_odom_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, robot_timestamps, imu_timestamps, relative_robot_timestamps, relative_imu_timestamps
odom_data, map_data, relative_odom_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, robot_timestamps, imu_timestamps, relative_robot_timestamps, relative_imu_timestamps = extracted_data
# 打印调试信息
print(f"Extracted ODOM Data: {len(odom_data)} entries")
print(f"Extracted MAP Data: {len(map_data)} entries")
print(f"Relative ODOM Timestamps: {len(relative_odom_timestamps)} entries")
print(f"Robot Frequencies: {len(robot_freqs)} entries")
print(f"Relative IMU Timestamps: {len(relative_imu_timestamps)} entries")
print(f"IMU Frequencies: {len(imu_freqs)} entries")
print(f"Robot Timestamps: {len(robot_timestamps)} entries")
print(f"IMU Timestamps: {len(imu_timestamps)} entries")
if not odom_data or not map_data:
messagebox.showwarning("No Data", "No valid data found within the specified range.")
return
save_to_csv(odom_data, map_data, relative_robot_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, robot_timestamps, imu_timestamps)
messagebox.showinfo("Success", "Data saved successfully.")
# 绘制图表
plot_data(odom_data, map_data, relative_odom_timestamps, relative_robot_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, additional_points=additional_points)
except ValueError as ve:
messagebox.showerror("Invalid Input", str(ve))
def on_add_point_button_click():
try:
point_x = float(point_x_var.get().strip())
point_y = float(point_y_var.get().strip())
# 将新点添加到地图数据中
global additional_points
additional_points.append((point_x, point_y))
# 重新绘制图表
plot_data(odom_data, map_data, relative_odom_timestamps, relative_robot_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, additional_points=additional_points)
except ValueError as ve:
messagebox.showerror("Invalid Input", "Please enter valid numeric values for X and Y coordinates.")
# 创建主窗口
root = Tk()
root.title("Comprehensive Log Data Extraction")
# 创建标签和输入框
Label(root, text="Start Line:").grid(row=0, column=0, padx=10, pady=5)
start_line_var = StringVar()
Entry(root, textvariable=start_line_var).grid(row=0, column=1, padx=10, pady=5)
Label(root, text="End Line:").grid(row=1, column=0, padx=10, pady=5)
end_line_var = StringVar()
Entry(root, textvariable=end_line_var).grid(row=1, column=1, padx=10, pady=5)
# 创建按钮
Button(root, text="Save Data to CSV", command=on_save_button_click).grid(row=2, columnspan=2, pady=10)
# 创建额外点输入框和按钮
Label(root, text="Point X:").grid(row=3, column=0, padx=10, pady=5)
point_x_var = StringVar()
Entry(root, textvariable=point_x_var).grid(row=3, column=1, padx=10, pady=5)
Label(root, text="Point Y:").grid(row=4, column=0, padx=10, pady=5)
point_y_var = StringVar()
Entry(root, textvariable=point_y_var).grid(row=4, column=1, padx=10, pady=5)
Button(root, text="Add Point to Map", command=on_add_point_button_click).grid(row=5, columnspan=2, pady=10)
# 初始化全局变量
global additional_points, odom_data, map_data, relative_odom_timestamps, robot_freqs, relative_imu_timestamps, imu_freqs, robot_timestamps, imu_timestamps, relative_robot_timestamps, relative_imu_timestamps
additional_points = []
odom_data = []
map_data = []
relative_odom_timestamps = []
robot_freqs = []
relative_imu_timestamps = []
imu_freqs = []
robot_timestamps = []
imu_timestamps = []
relative_robot_timestamps = []
relative_imu_timestamps = []
# 运行主循环
root.mainloop()
最新发布
本文介绍了如何进行IMU的航向角标定。通过执行GitHub上的SensorsCalibration项目，利用imu_heading模块进行编译并运行。在完成编译后，使用特定方法_id对数据进行处理，例如方法_id=1对应直行驾驶，方法_id=2对应自由行驶。最终，程序输出了标定结果，包括最佳得分、真值、GNSS值、偏移量以及校正后的数据组件等，帮助评估和调整IMU的航向角精度。
扫一扫
6078
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
