48 lines
1.5 KiB
Python
48 lines
1.5 KiB
Python
#!/usr/bin/env python3
|
|
|
|
import rospy
|
|
import math, cmath
|
|
from Service.srv import FFT, FFTResponse
|
|
|
|
def computeFFT(input_data):
|
|
N = len(input_data)
|
|
magnitude = [0.0] * (N//2 + 1) # 存储幅值
|
|
phase = [0.0] * (N//2 + 1) # 存储相位
|
|
|
|
for k in range(N//2 + 1):
|
|
s = 0+0j
|
|
|
|
for n in range(N):
|
|
angle = -2 * math.pi * k * n / N # 角度计算
|
|
s += input_data[n] * complex(math.cos(angle), math.sin(angle)) # 快速傅里叶变换公式
|
|
|
|
magnitude[k] = abs(s) # 幅值
|
|
phase[k] = cmath.phase(s) # 相位
|
|
|
|
return magnitude, phase
|
|
|
|
def fft_callback(req):
|
|
if len(req.data_x) != req.window_size or len(req.data_y) != req.window_size:
|
|
rospy.logerr("数据量不符合128点FFT要求")
|
|
return FFTResponse()
|
|
|
|
mag_x, phase_x = computeFFT(req.data_x) # 对坐标 x 和 y 分别进行快速傅里叶变换FFT
|
|
mag_y, phase_y = computeFFT(req.data_y)
|
|
|
|
res = FFTResponse()
|
|
res.magnitude_x = mag_x # 坐标 x 的幅度
|
|
res.magnitude_y = mag_y # 坐标 y 的幅度
|
|
res.phase_x = phase_x # 坐标 x 的相位
|
|
res.phase_y = phase_y # 坐标 y 的相位
|
|
|
|
rospy.loginfo("FFT 服务完成 %d 个点" % req.window_size)
|
|
return res
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('Service_S')
|
|
|
|
service = rospy.Service('fft_service', FFT, fft_callback) # ROS 服务
|
|
|
|
rospy.loginfo("服务端快速傅里叶变换程序执行成功")
|
|
|
|
rospy.spin() |