添加实验三部分程序

src/position_demo/scripts/publisher_node_three.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+import rospy
+from geometry_msgs.msg import Point
+import math
+
+def publisher():
+    # 创建一个发布者，将坐标点发布到 /position_topic 主题
+    point_pub = rospy.Publisher('position_topic', Point, queue_size=10)
+    
+    rospy.init_node('position_publisher', anonymous=True)
+    rate = rospy.Rate(10)  # 10Hz (100ms)
+
+    k = 0
+    while not rospy.is_shutdown():
+        # 根据方程计算 x 和 y 坐标
+        x = math.sin(0.015625 * k * math.pi)
+        y = math.cos(0.015625 * k * math.pi)
+
+        # 创建一个 Point 消息并发布
+        point = Point(x=x, y=y, z=0)
+        point_pub.publish(point)
+        rospy.loginfo(f"Published Point: x={x:.4f}, y={y:.4f}")
+
+        k += 1
+        rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        publisher()
+    except rospy.ROSInterruptException:
+        pass

src/position_demo/scripts/subscriber_node.py

@@ -18,12 +18,15 @@ def callback(data):
     marker.header.frame_id = "world"
     marker.header.stamp = rospy.Time.now()
 
-    # 设置 Marker 类型为线（可以选择其他类型，如点）
-    marker.type = Marker.LINE_STRIP
+    # 设置 Marker 类型为点（而不是线条）
+    marker.type = Marker.POINTS
     marker.action = Marker.ADD
 
-    # 设置线的颜色和宽度
-    marker.scale.x = 0.1  # 线的宽度
+    # 设置点的大小，直径为 0.001
+    marker.scale.x = 0.025  # 点的直径（x 和 y 均为直径）
+    marker.scale.y = 0.025  # 点的直径
+
+    # 设置点的颜色
     marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)  # 红色，透明度 1.0
 
     # 添加接收到的坐标点
@@ -33,7 +36,7 @@ def callback(data):
     point.z = data.z
     points.append(point)
 
-    # 将所有的点添加到线条（LINE_STRIP）中
+    # 将所有的点添加到 Marker 的点集合中
     marker.points = points
 
     # 发布 Marker

src/position_demo/scripts/subscriber_node_three.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+import rospy
+from geometry_msgs.msg import Point
+from visualization_msgs.msg import Marker
+from std_msgs.msg import ColorRGBA
+
+# 保存接收到的所有点，更新轨迹
+points = []
+x1 = 0
+y1 = 0
+
+def callback(data, n=[0]):
+    global points
+    global x1
+    global y1
+
+    if n[0] == 0:
+        x1 = data.x + 0.1
+        y1 = data.y + 0.1
+        n[0] = 1
+    
+    else:
+        x1 = data.x - 0.1
+        y1 = data.y - 0.1
+        n[0] = 0
+    
+    rospy.loginfo(f"Modified Point: x={x1:.4f}, y={y1:.4f}")
+
+    # 创建 Marker 消息
+    marker = Marker()
+    marker.header.frame_id = "world"
+    marker.header.stamp = rospy.Time.now()
+
+    # 设置 Marker 类型为点（而不是线条）
+    marker.type = Marker.POINTS
+    marker.action = Marker.ADD
+
+    # 设置点的大小，直径为 0.001
+    marker.scale.x = 0.025  # 点的直径（x 和 y 均为直径）
+    marker.scale.y = 0.025  # 点的直径
+
+    # 设置点的颜色
+    marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)  # 红色，透明度 1.0
+
+    # 添加接收到的坐标点
+    point = Point()
+    point.x = x1
+    point.y = y1
+    point.z = 0
+    points.append(point)
+
+    # 将所有的点添加到 Marker 的点集合中
+    marker.points = points
+
+    # 发布 Marker
+    marker_pub.publish(marker)
+
+def listener():
+    rospy.init_node('position_subscriber', anonymous=True)
+    
+    # 创建 Marker 发布者
+    global marker_pub
+    marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
+    
+    rospy.Subscriber('position_topic', Point, callback)
+
+    rospy.spin()
+
+if __name__ == '__main__':
+    listener()

src/position_demo/src/FftService.srv

@@ -0,0 +1,3 @@
+float64[128] data
+---
+float64[128] fft_result

src/position_demo/src/fft_service.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+import rospy
+import numpy as np
+from your_package.srv import FftService, FftServiceResponse
+
+# 服务回调函数，进行FFT变换
+def handle_fft_request(req):
+    # 获取请求中的数据
+    data = req.data
+    # 对数据进行FFT变换
+    fft_result = np.fft.fft(data)
+    # 返回实部和虚部的实数部分作为结果
+    return FftServiceResponse(fft_result.real)
+
+def fft_service():
+    rospy.init_node('fft_service')
+    # 定义服务
+    service = rospy.Service('fft_service', FftService, handle_fft_request)
+    rospy.loginfo("Ready to perform FFT on 128 data points.")
+    rospy.spin()
+
+if __name__ == "__main__":
+    fft_service()