#!/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()