初步完成ROS服务程序工作

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

src/Service/package.xml

@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>Service</name>
+  <version>0.0.0</version>
+  <description>The Service package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="root@todo.todo">root</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/Service</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend>
+  
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

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

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

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

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