378 lines
15 KiB
Python
378 lines
15 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
|
||
"""
|
||
FAST-LIO Distance Calculator + STM32 Communication
|
||
Real-time calculation of radar movement distance and position,
|
||
and send data to STM32 via UART
|
||
"""
|
||
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from nav_msgs.msg import Odometry
|
||
import math
|
||
import time
|
||
from datetime import datetime
|
||
import serial
|
||
import struct
|
||
import os
|
||
import sys
|
||
|
||
class DistanceCalculator(Node):
|
||
def __init__(self):
|
||
super().__init__('distance_calculator')
|
||
|
||
# Subscribe to odometry topic
|
||
self.subscription = self.create_subscription(
|
||
Odometry, '/Odometry', self.odom_callback, 10)
|
||
|
||
# Initialize variables
|
||
self.last_position = None
|
||
self.total_distance = 0.0
|
||
self.start_time = time.time()
|
||
self.last_log_time = time.time()
|
||
self.position_history = []
|
||
self.last_sent_time = time.time()
|
||
|
||
# 添加静态检测和噪声过滤
|
||
self.static_threshold = 0.002 # 静态检测阈值（米）
|
||
self.static_count = 0
|
||
self.static_position_buffer = []
|
||
self.max_static_buffer_size = 10
|
||
self.velocity_threshold = 0.001 # 速度阈值（米/秒）
|
||
self.last_position_filtered = None
|
||
|
||
# 系统启动稳定机制
|
||
self.startup_delay = 5.0 # 启动后5秒延迟发送
|
||
self.system_stable = False
|
||
self.data_valid_count = 0
|
||
self.min_valid_data_count = 10 # 需要10个连续有效数据才开始发送
|
||
|
||
# Initialize UART communication
|
||
self.uart_port = '/dev/ttyTHS1' # Jetson Orin NX UART1
|
||
self.baudrate = 115200
|
||
self.ser = None
|
||
|
||
try:
|
||
self.ser = serial.Serial(
|
||
port=self.uart_port,
|
||
baudrate=self.baudrate,
|
||
bytesize=serial.EIGHTBITS,
|
||
parity=serial.PARITY_NONE,
|
||
stopbits=serial.STOPBITS_ONE,
|
||
timeout=0.1
|
||
)
|
||
self.get_logger().info(f"UART port opened: {self.uart_port}, Baudrate: {self.baudrate}")
|
||
except Exception as e:
|
||
self.get_logger().error(f"Failed to open UART port: {str(e)}")
|
||
self.get_logger().info("Data will not be sent to STM32")
|
||
|
||
# Create log file
|
||
self.log_filename = f"trajectory_log_{datetime.now().strftime('%Y%m%d_%H%M%S')}.txt"
|
||
self.get_logger().info("Distance calculator started")
|
||
self.get_logger().info(f"Trajectory data will be saved to: {self.log_filename}")
|
||
self.get_logger().info("Waiting for radar positioning data...")
|
||
|
||
# Write log file header
|
||
with open(self.log_filename, 'w', encoding='utf-8') as f:
|
||
f.write("Timestamp,Runtime(s),X(m),Y(m),Z(m),Total Distance(m),Velocity X(m/s),Velocity Y(m/s),Velocity Z(m/s)\n")
|
||
|
||
def send_to_stm32(self, x, y, z, vx, vy, vz):
|
||
"""Send data to STM32 using anonymous protocol"""
|
||
if self.ser is None or not self.ser.is_open:
|
||
return False
|
||
|
||
try:
|
||
# Protocol format: [0xAA][0xFF][0x01][Data Length][Data][Sum Check][Add Check]
|
||
# Scale factors
|
||
POS_SCALE_FACTOR = 1000 # 1 mm precision
|
||
VEL_SCALE_FACTOR = 1000 # 1 mm/s precision
|
||
|
||
# 数据有效性检查
|
||
# 检查是否为有效数值
|
||
if not all(math.isfinite(val) for val in [x, y, z, vx, vy, vz]):
|
||
self.get_logger().warning("Invalid data detected (NaN or Inf), skipping transmission")
|
||
return False
|
||
|
||
# 位置范围限制（±30米）
|
||
MAX_POS = 30.0
|
||
MIN_POS = -30.0
|
||
x = max(MIN_POS, min(MAX_POS, x))
|
||
y = max(MIN_POS, min(MAX_POS, y))
|
||
z = max(MIN_POS, min(MAX_POS, z))
|
||
|
||
# 速度范围限制（±30 m/s）
|
||
MAX_VEL = 30.0
|
||
MIN_VEL = -30.0
|
||
vx = max(MIN_VEL, min(MAX_VEL, vx))
|
||
vy = max(MIN_VEL, min(MAX_VEL, vy))
|
||
vz = max(MIN_VEL, min(MAX_VEL, vz))
|
||
|
||
# Convert to integer values
|
||
x_int = int(x * POS_SCALE_FACTOR)
|
||
y_int = int(y * POS_SCALE_FACTOR)
|
||
z_int = int(z * POS_SCALE_FACTOR)
|
||
vx_int = int(vx * VEL_SCALE_FACTOR)
|
||
vy_int = int(vy * VEL_SCALE_FACTOR)
|
||
vz_int = int(vz * VEL_SCALE_FACTOR)
|
||
|
||
# 短整型范围检查（-32768 到 32767）
|
||
SHORT_MAX = 32767
|
||
SHORT_MIN = -32768
|
||
|
||
# 进一步限制确保在短整型范围内
|
||
x_int = max(SHORT_MIN, min(SHORT_MAX, x_int))
|
||
y_int = max(SHORT_MIN, min(SHORT_MAX, y_int))
|
||
z_int = max(SHORT_MIN, min(SHORT_MAX, z_int))
|
||
vx_int = max(SHORT_MIN, min(SHORT_MAX, vx_int))
|
||
vy_int = max(SHORT_MIN, min(SHORT_MAX, vy_int))
|
||
vz_int = max(SHORT_MIN, min(SHORT_MAX, vz_int))
|
||
|
||
# Pack data: 3 int16 (x,y,z) + 3 int16 (vx,vy,vz)
|
||
data_content = struct.pack('<hhhhhh',
|
||
x_int, y_int, z_int,
|
||
vx_int, vy_int, vz_int)
|
||
LEN = len(data_content) # Data length = 6 * 2 = 12 bytes
|
||
|
||
# Build frame
|
||
frame = bytearray()
|
||
frame.append(0xAA) # Header
|
||
frame.append(0xFF) # Target address
|
||
frame.append(0x01) # Function code
|
||
frame.append(LEN) # Data length
|
||
frame.extend(data_content)
|
||
|
||
# Calculate checksums
|
||
sum_check = 0
|
||
add_check = 0
|
||
for byte in frame:
|
||
sum_check = (sum_check + byte) & 0xFF
|
||
add_check = (add_check + sum_check) & 0xFF
|
||
|
||
# Add checksums
|
||
frame.append(sum_check)
|
||
frame.append(add_check)
|
||
|
||
# Send data
|
||
self.ser.write(frame)
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.get_logger().error(f"Error sending data: {str(e)}")
|
||
return False
|
||
|
||
def filter_static_noise(self, current_pos, vx, vy, vz):
|
||
"""过滤静态噪声，当设备静止时减少位置抖动"""
|
||
# 计算速度大小
|
||
velocity_magnitude = math.sqrt(vx**2 + vy**2 + vz**2)
|
||
|
||
# 如果速度很小，认为是静止状态
|
||
if velocity_magnitude < self.velocity_threshold:
|
||
# 添加到静态位置缓冲区
|
||
self.static_position_buffer.append((current_pos.x, current_pos.y, current_pos.z))
|
||
|
||
# 保持缓冲区大小
|
||
if len(self.static_position_buffer) > self.max_static_buffer_size:
|
||
self.static_position_buffer.pop(0)
|
||
|
||
# 如果有足够的静态数据，使用平均值
|
||
if len(self.static_position_buffer) >= 5:
|
||
avg_x = sum(pos[0] for pos in self.static_position_buffer) / len(self.static_position_buffer)
|
||
avg_y = sum(pos[1] for pos in self.static_position_buffer) / len(self.static_position_buffer)
|
||
avg_z = sum(pos[2] for pos in self.static_position_buffer) / len(self.static_position_buffer)
|
||
|
||
# 创建过滤后的位置
|
||
class FilteredPos:
|
||
def __init__(self, x, y, z):
|
||
self.x = x
|
||
self.y = y
|
||
self.z = z
|
||
|
||
return FilteredPos(avg_x, avg_y, avg_z), True
|
||
else:
|
||
# 运动状态，清空缓冲区
|
||
self.static_position_buffer.clear()
|
||
|
||
return current_pos, False
|
||
|
||
def odom_callback(self, msg):
|
||
current_time = time.time()
|
||
current_pos = msg.pose.pose.position
|
||
|
||
# Calculate runtime
|
||
elapsed_time = current_time - self.start_time
|
||
|
||
# Get linear velocity components
|
||
linear_vel = msg.twist.twist.linear
|
||
vx = linear_vel.x
|
||
vy = linear_vel.y
|
||
vz = linear_vel.z
|
||
|
||
# Calculate total velocity magnitude
|
||
velocity = math.sqrt(vx**2 + vy**2 + vz**2)
|
||
|
||
# 应用静态噪声过滤
|
||
filtered_pos, is_static = self.filter_static_noise(current_pos, vx, vy, vz)
|
||
if is_static:
|
||
self.get_logger().debug(f"Static mode: using filtered position")
|
||
|
||
# 使用过滤后的位置
|
||
current_pos = filtered_pos
|
||
|
||
if self.last_position is not None:
|
||
# Calculate distance between points
|
||
dx = current_pos.x - self.last_position.x
|
||
dy = current_pos.y - self.last_position.y
|
||
dz = current_pos.z - self.last_position.z
|
||
distance = math.sqrt(dx*dx + dy*dy + dz*dz)
|
||
|
||
# Update total distance
|
||
self.total_distance += distance
|
||
|
||
# Display info every 2 seconds
|
||
if current_time - self.last_log_time > 2.0:
|
||
self.display_status(current_pos, elapsed_time, vx, vy, vz, velocity)
|
||
self.last_log_time = current_time
|
||
|
||
# 检查系统稳定性
|
||
if not self.system_stable:
|
||
# 检查启动延迟
|
||
if elapsed_time < self.startup_delay:
|
||
self.get_logger().debug(f"System warming up... {self.startup_delay - elapsed_time:.1f}s remaining")
|
||
return
|
||
|
||
# 检查数据有效性
|
||
data_is_valid = (
|
||
abs(current_pos.x) < 10.0 and abs(current_pos.y) < 10.0 and abs(current_pos.z) < 10.0 and
|
||
abs(vx) < 5.0 and abs(vy) < 5.0 and abs(vz) < 5.0 and
|
||
all(math.isfinite(val) for val in [current_pos.x, current_pos.y, current_pos.z, vx, vy, vz])
|
||
)
|
||
|
||
if data_is_valid:
|
||
self.data_valid_count += 1
|
||
if self.data_valid_count >= self.min_valid_data_count:
|
||
self.system_stable = True
|
||
self.get_logger().info("System stabilized, starting UART transmission")
|
||
else:
|
||
self.data_valid_count = 0 # 重置计数
|
||
self.get_logger().debug("Waiting for stable data...")
|
||
|
||
# Send data to STM32 every 50ms (20Hz) - 优化发送频率
|
||
if self.system_stable and current_time - self.last_sent_time > 0.05:
|
||
if self.ser and self.ser.is_open:
|
||
success = self.send_to_stm32(
|
||
current_pos.x,
|
||
current_pos.y,
|
||
current_pos.z,
|
||
vx,
|
||
vy,
|
||
vz
|
||
)
|
||
if success:
|
||
self.get_logger().info(f"Sent to STM32: X={current_pos.x:.3f}, Y={current_pos.y:.3f}, Z={current_pos.z:.3f}, Vx={vx:.3f}, Vy={vy:.3f}, Vz={vz:.3f}")
|
||
self.last_sent_time = current_time
|
||
|
||
# Save position history
|
||
self.position_history.append({
|
||
'time': current_time,
|
||
'position': current_pos,
|
||
'velocity': (vx, vy, vz),
|
||
'distance': self.total_distance
|
||
})
|
||
|
||
# Write to log file
|
||
self.log_to_file(elapsed_time, current_pos, vx, vy, vz)
|
||
|
||
# Update last position
|
||
self.last_position = current_pos
|
||
|
||
def display_status(self, position, elapsed_time, vx, vy, vz, velocity):
|
||
"""Display current status"""
|
||
self.get_logger().info("=" * 60)
|
||
self.get_logger().info(f"Runtime: {elapsed_time:.1f} sec")
|
||
self.get_logger().info(f"Position: X={position.x:.3f}m, Y={position.y:.3f}m, Z={position.z:.3f}m")
|
||
self.get_logger().info(f"Total distance: {self.total_distance:.3f} m")
|
||
self.get_logger().info(f"Velocity X: {vx:.3f} m/s")
|
||
self.get_logger().info(f"Velocity Y: {vy:.3f} m/s")
|
||
self.get_logger().info(f"Velocity Z: {vz:.3f} m/s")
|
||
self.get_logger().info(f"Total speed: {velocity:.3f} m/s")
|
||
self.get_logger().info(f"Avg speed: {self.total_distance/elapsed_time:.3f} m/s")
|
||
|
||
# Add UART status
|
||
if self.ser and self.ser.is_open:
|
||
self.get_logger().info("UART communication: Connected")
|
||
else:
|
||
self.get_logger().warning("UART communication: Disconnected")
|
||
|
||
def log_to_file(self, elapsed_time, position, vx, vy, vz):
|
||
"""Log data to file"""
|
||
timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
|
||
|
||
with open(self.log_filename, 'a', encoding='utf-8') as f:
|
||
f.write(f"{timestamp},{elapsed_time:.3f},{position.x:.6f},{position.y:.6f},{position.z:.6f},{self.total_distance:.6f},{vx:.6f},{vy:.6f},{vz:.6f}\n")
|
||
|
||
def print_summary(self):
|
||
"""Print summary information"""
|
||
if len(self.position_history) > 0:
|
||
elapsed_time = time.time() - self.start_time
|
||
|
||
print("\n" + "="*80)
|
||
print("FAST-LIO Trajectory Report")
|
||
print("="*80)
|
||
print(f"Total runtime: {elapsed_time:.1f} sec ({elapsed_time/60:.1f} min)")
|
||
print(f"Total distance: {self.total_distance:.3f} m")
|
||
print(f"Avg speed: {self.total_distance/elapsed_time:.3f} m/s")
|
||
print(f"Data points: {len(self.position_history)}")
|
||
print(f"Trajectory saved to: {self.log_filename}")
|
||
|
||
# Add UART status
|
||
if self.ser and self.ser.is_open:
|
||
print(f"UART port: {self.uart_port} @ {self.baudrate} bps")
|
||
print(f"Protocol: Anonymous (Header 0xAA, Target 0xFF, ID 0x01)")
|
||
print(f"Data format: X/Y/Z/Vx/Vy/Vz (all int16)")
|
||
else:
|
||
print("UART communication: Disconnected")
|
||
|
||
if len(self.position_history) >= 2:
|
||
start_pos = self.position_history[0]['position']
|
||
end_pos = self.position_history[-1]['position']
|
||
|
||
# Calculate straight-line distance
|
||
dx = end_pos.x - start_pos.x
|
||
dy = end_pos.y - start_pos.y
|
||
dz = end_pos.z - start_pos.z
|
||
straight_distance = math.sqrt(dx*dx + dy*dy + dz*dz)
|
||
|
||
print(f"\nStart position: ({start_pos.x:.3f}, {start_pos.y:.3f}, {start_pos.z:.3f})")
|
||
print(f"End position: ({end_pos.x:.3f}, {end_pos.y:.3f}, {end_pos.z:.3f})")
|
||
print(f"Straight distance: {straight_distance:.3f} m")
|
||
print(f"Path efficiency: {(straight_distance/self.total_distance)*100:.1f}%")
|
||
|
||
print("="*80)
|
||
|
||
# Close UART
|
||
if self.ser and self.ser.is_open:
|
||
self.ser.close()
|
||
print("UART connection closed")
|
||
|
||
def main():
|
||
print("Starting FAST-LIO distance calculator...")
|
||
print("Tip: Press Ctrl+C to stop and view report")
|
||
print(f"Initializing UART: /dev/ttyTHS1 @ 115200 bps")
|
||
|
||
rclpy.init()
|
||
calculator = DistanceCalculator()
|
||
|
||
try:
|
||
rclpy.spin(calculator)
|
||
except KeyboardInterrupt:
|
||
print("\nStopping distance calculator...")
|
||
finally:
|
||
calculator.print_summary()
|
||
calculator.destroy_node()
|
||
rclpy.shutdown()
|
||
print("Program finished")
|
||
|
||
if __name__ == '__main__':
|
||
main()
|