解决ROS服务中已知存在的问题

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()

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
+            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()