From b5d0d09a1e712494f32b1d26e919f0f4e37db8dc Mon Sep 17 00:00:00 2001
From: Cx330 <1487537121@qq.com>
Date: Wed, 2 Jul 2025 15:08:02 +0800
Subject: [PATCH] =?UTF-8?q?=E8=A7=A3=E5=86=B3ROS=E6=9C=8D=E5=8A=A1?=
 =?UTF-8?q?=E4=B8=AD=E5=B7=B2=E7=9F=A5=E5=AD=98=E5=9C=A8=E7=9A=84=E9=97=AE?=
 =?UTF-8?q?=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Service/scripts/Service_S.py           |  1 -
 src/Service/scripts/Subscriber_Client_S.py | 66 +++++++++++++++++++++-
 2 files changed, 65 insertions(+), 2 deletions(-)

diff --git a/src/Service/scripts/Service_S.py b/src/Service/scripts/Service_S.py
index f5e7893..12b639d 100644
--- a/src/Service/scripts/Service_S.py
+++ b/src/Service/scripts/Service_S.py
@@ -44,5 +44,4 @@ if __name__ == '__main__':
     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
index 3dd43b1..3979923 100644
--- a/src/Service/scripts/Subscriber_Client_S.py
+++ b/src/Service/scripts/Subscriber_Client_S.py
@@ -144,4 +144,68 @@ def position_callback(msg):
 
         for i in range(len(current_x_magnitudes)):
             px = Point()
-            px.x = i * 0.1
\ No newline at end of file
+            px.x = i * 0.1
+            px.y = 2.0
+            px.z = (current_x_magnitudes[i] / float(FFT_WINDOW_SIZE)) * 10.0
+            x_spectrum.points.append(px)
+            py = Point()
+            py.x = px.x
+            py.y = 1.0
+            py.z = (current_y_magnitudes[i] / float(FFT_WINDOW_SIZE)) * 10.0
+            y_spectrum.points.append(py)
+
+        marker_array.markers.append(x_spectrum)
+        marker_array.markers.append(y_spectrum)
+
+        text_x = Marker()
+        text_x.header = msg.header
+        text_x.ns = "spectrum_labels"
+        text_x.type = Marker.TEXT_VIEW_FACING
+        text_x.action = Marker.ADD
+        text_x.scale.z = 0.3
+        text_x.color.r = 1.0
+        text_x.color.g = 1.0
+        text_x.color.b = 1.0
+        text_x.color.a = 1.0
+        text_x.id = 10
+        text_x.pose.position.x = -0.5
+        text_x.pose.position.y = 2.0
+        text_x.pose.position.z = 1.0
+        text_x.text = "坐标X频谱"
+        marker_array.markers.append(text_x)
+        
+        text_y = Marker()
+        text_y.header = msg.header
+        text_y.ns = "spectrum_labels"
+        text_y.type = Marker.TEXT_VIEW_FACING
+        text_y.action = Marker.ADD
+        text_y.scale.z = 0.3
+        text_y.color.r = 1.0
+        text_y.color.g = 1.0
+        text_y.color.b = 1.0
+        text_y.color.a = 1.0
+        text_y.id = 11
+        text_y.pose.position.x = -0.5
+        text_y.pose.position.y = 1.0
+        text_y.pose.position.z = 1.0
+        text_y.text = "坐标Y频谱"
+        marker_array.markers.append(text_y)
+
+    noisy_pub.publish(noisy_msg)
+    marker_pub.publish(marker_array)
+
+    rospy.loginfo("订阅端接收坐标信息: x=%.3f, y=%.3f | 加噪坐标信息: x=%.3f, y=%.3f",
+                  msg.point.x, msg.point.y, noisy_msg.point.x, noisy_msg.point.y)
+
+if __name__ == '__main__':
+    rospy.init_node('Subscriber_Client_S')
+    rospy.Subscriber('position', PointStamped, position_callback)
+
+    noisy_pub = rospy.Publisher('noisy_position', PointStamped, queue_size=10)
+    marker_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
+    fft_client = rospy.ServiceProxy('fft_service', FFT)
+    
+    rospy.loginfo("等待快速傅里叶FFT服务端启动")
+    rospy.wait_for_service('fft_service')
+    rospy.loginfo("FFT服务连接成功")
+    rospy.spin()
\ No newline at end of file