diff --git a/src/Service/CMakeLists.txt b/src/Service/CMakeLists.txt new file mode 100644 index 0000000..a84a260 --- /dev/null +++ b/src/Service/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.0.2) +project(Service) + +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + geometry_msgs + visualization_msgs + message_generation +) + +add_service_files( + FILES + FFT.srv +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs geometry_msgs visualization_msgs +) \ No newline at end of file diff --git a/src/Service/package.xml b/src/Service/package.xml new file mode 100644 index 0000000..8bd3f64 --- /dev/null +++ b/src/Service/package.xml @@ -0,0 +1,79 @@ + + + Service + 0.0.0 + The Service package + + + + + root + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + geometry_msgs + message_generation + rospy + std_msgs + visualization_msgs + message_generation + + geometry_msgs + rospy + std_msgs + visualization_msgs + message_runtime + + geometry_msgs + rospy + std_msgs + visualization_msgs + message_runtime + geometry_msgs + visualization_msgs + + + + + + + + \ No newline at end of file diff --git a/src/Service/scripts/Publisher_S.py b/src/Service/scripts/Publisher_S.py new file mode 100644 index 0000000..fa7f6b2 --- /dev/null +++ b/src/Service/scripts/Publisher_S.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 + +import rospy +import math +from geometry_msgs.msg import PointStamped + +def main(): + rospy.init_node('Publisher_S') + pub = rospy.Publisher('position', PointStamped, queue_size=10) # 向 position 话题发布 PointStamped 类型的消息 + rate = rospy.Rate(10) # 每 0.1s 一条信息 + + k = 0 + + while not rospy.is_shutdown(): + msg = PointStamped() + msg.header.stamp = rospy.Time.now() + msg.header.frame_id = "map" # 基于 map 的坐标系 + msg.point.x = math.sin(0.05 * math.pi * k) # x 值 + msg.point.y = math.cos(0.05 * math.pi * k) # y 值 + msg.point.z = 0.0 + pub.publish(msg) # 发布话题 + rospy.loginfo("发布端发送坐标信息: x=%.3f, y=%.3f" % (msg.point.x, msg.point.y)) # 打印坐标信息 + k += 1 + rate.sleep() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/Service/scripts/Service_S.py b/src/Service/scripts/Service_S.py new file mode 100644 index 0000000..f5e7893 --- /dev/null +++ b/src/Service/scripts/Service_S.py @@ -0,0 +1,48 @@ +#!/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() \ No newline at end of file diff --git a/src/Service/scripts/Subscriber_Client_S.py b/src/Service/scripts/Subscriber_Client_S.py new file mode 100644 index 0000000..3dd43b1 --- /dev/null +++ b/src/Service/scripts/Subscriber_Client_S.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +import rospy +import random +from geometry_msgs.msg import PointStamped, Point +from visualization_msgs.msg import Marker, MarkerArray +from Service.srv import FFT + +MAX_POINTS = 100 +FFT_WINDOW_SIZE = 128 + +original_trajectory = [] +noisy_trajectory = [] +noisy_x_history = [] +noisy_y_history = [] +current_x_magnitudes = [] +current_y_magnitudes = [] +noisy_pub = None +marker_pub = None +fft_client = None + +def position_callback(msg): + global original_trajectory, noisy_trajectory + global noisy_x_history, noisy_y_history + global current_x_magnitudes, current_y_magnitudes + global noisy_pub, marker_pub, fft_client + + noise_x = random.uniform(-0.1, 0.1) # 产生 -0.1 ~0.1 的随机噪声 + noise_y = random.uniform(-0.1, 0.1) + + noisy_msg = PointStamped() + noisy_msg.header = msg.header + noisy_msg.point.x = msg.point.x + noise_x # 在原信号上叠加噪声 + noisy_msg.point.y = msg.point.y + noise_y + noisy_msg.point.z = 0.0 + + original_trajectory.append(msg.point) # 原点轨迹 + if len(original_trajectory) > MAX_POINTS: + original_trajectory.pop(0) + noisy_trajectory.append(noisy_msg.point) #加噪轨迹 + if len(noisy_trajectory) > MAX_POINTS: + noisy_trajectory.pop(0) + + noisy_x_history.append(noisy_msg.point.x) #保存历史坐标信息 + noisy_y_history.append(noisy_msg.point.y) + + marker_array = MarkerArray() + + # 原始信号轨迹绘制 + original_line = Marker() + original_line.header = msg.header + original_line.ns = "trajectory" + original_line.id = 0 + original_line.type = Marker.LINE_STRIP + original_line.action = Marker.ADD + original_line.scale.x = 0.02 + original_line.color.g = 1.0 # 绿色线 + original_line.color.a = 1.0 + original_line.points = list(original_trajectory) + marker_array.markers.append(original_line) + + # 加噪信号轨迹绘制 + noisy_line = Marker() + noisy_line.header = msg.header + noisy_line.ns = "trajectory" + noisy_line.id = 1 + noisy_line.type = Marker.LINE_STRIP + noisy_line.action = Marker.ADD + noisy_line.scale.x = 0.02 + noisy_line.color.r = 1.0 # 红色线 + noisy_line.color.a = 1.0 + noisy_line.points = list(noisy_trajectory) + marker_array.markers.append(noisy_line) + + # 原始信号位置点 + original_point = Marker() + original_point.header = msg.header + original_point.ns = "current_point" + original_point.id = 0 + original_point.type = Marker.SPHERE + original_point.action = Marker.ADD + original_point.pose.position = msg.point + original_point.pose.orientation.w = 1.0 + original_point.scale.x = 0.1 + original_point.scale.y = 0.1 + original_point.scale.z = 0.1 + original_point.color.g = 1.0 # 绿色球 + original_point.color.a = 1.0 + marker_array.markers.append(original_point) + + # 加噪信号位置点 + noisy_point = Marker() + noisy_point.header = msg.header + noisy_point.ns = "current_point" + noisy_point.id = 1 + noisy_point.type = Marker.SPHERE + noisy_point.action = Marker.ADD + noisy_point.pose.position = noisy_msg.point + noisy_point.pose.orientation.w = 1.0 + noisy_point.scale.x = 0.1 + noisy_point.scale.y = 0.1 + noisy_point.scale.z = 0.1 + noisy_point.color.r = 1.0 # 红色球 + noisy_point.color.a = 1.0 + marker_array.markers.append(noisy_point) + + # 当点数 >=128 时利用快速傅里叶变换FFT计算加噪信号频谱 + if len(noisy_x_history) >= FFT_WINDOW_SIZE: + try: + res = fft_client(data_x=noisy_x_history[-FFT_WINDOW_SIZE:], + data_y=noisy_y_history[-FFT_WINDOW_SIZE:], + window_size=FFT_WINDOW_SIZE) + rospy.loginfo("FFT computed successfully") + current_x_magnitudes[:] = res.magnitude_x + current_y_magnitudes[:] = res.magnitude_y + except rospy.ServiceException as e: + rospy.logerr("Failed to call FFT service: %s" % e) + + # 判断频谱,生成频谱图 + if current_x_magnitudes and current_y_magnitudes: + x_spectrum = Marker() + x_spectrum.header = msg.header + x_spectrum.ns = "坐标x频谱" + x_spectrum.id = 2 + x_spectrum.type = Marker.CUBE_LIST + x_spectrum.action = Marker.ADD + x_spectrum.scale.x = 0.08 + x_spectrum.scale.y = 0.01 + x_spectrum.scale.z = 1.0 + x_spectrum.color.b = 1.0 + x_spectrum.color.a = 0.8 + + y_spectrum = Marker() + y_spectrum.header = msg.header + y_spectrum.ns = "坐标y频谱" + y_spectrum.id = 3 + y_spectrum.type = Marker.CUBE_LIST + y_spectrum.action = Marker.ADD + y_spectrum.scale.x = 0.08 + y_spectrum.scale.y = 0.01 + y_spectrum.scale.z = 1.0 + y_spectrum.color.r = 1.0 + y_spectrum.color.a = 0.8 + + for i in range(len(current_x_magnitudes)): + px = Point() + px.x = i * 0.1 \ No newline at end of file diff --git a/src/Service/srv/FFT.srv b/src/Service/srv/FFT.srv new file mode 100644 index 0000000..45b98b8 --- /dev/null +++ b/src/Service/srv/FFT.srv @@ -0,0 +1,8 @@ +float64[] data_x +float64[] data_y +int32 window_size +--- +float64[] magnitude_x +float64[] magnitude_y +float64[] phase_x +float64[] phase_y \ No newline at end of file