From 294d201664ddb2bb5de633542f1aecfd17b09bbf Mon Sep 17 00:00:00 2001 From: Cx330 <1487537121@qq.com> Date: Sat, 28 Jun 2025 22:03:23 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=AE=9E=E9=AA=8C=E4=B8=89?= =?UTF-8?q?=E9=83=A8=E5=88=86=E7=A8=8B=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/publisher_node_three.py | 31 ++++++++ src/position_demo/scripts/subscriber_node.py | 25 ++++--- .../scripts/subscriber_node_three.py | 70 +++++++++++++++++++ src/position_demo/src/FftService.srv | 3 + src/position_demo/src/fft_service.py | 23 ++++++ 5 files changed, 141 insertions(+), 11 deletions(-) create mode 100755 src/position_demo/scripts/publisher_node_three.py create mode 100755 src/position_demo/scripts/subscriber_node_three.py create mode 100644 src/position_demo/src/FftService.srv create mode 100644 src/position_demo/src/fft_service.py diff --git a/src/position_demo/scripts/publisher_node_three.py b/src/position_demo/scripts/publisher_node_three.py new file mode 100755 index 0000000..ca028c8 --- /dev/null +++ b/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 diff --git a/src/position_demo/scripts/subscriber_node.py b/src/position_demo/scripts/subscriber_node.py index c29d456..c374b2a 100755 --- a/src/position_demo/scripts/subscriber_node.py +++ b/src/position_demo/scripts/subscriber_node.py @@ -3,10 +3,10 @@ import rospy from geometry_msgs.msg import Point from visualization_msgs.msg import Marker from std_msgs.msg import ColorRGBA - + # 保存接收到的所有点，更新轨迹 points = [] - + def callback(data): global points @@ -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,22 +36,22 @@ def callback(data): point.z = data.z points.append(point) - # 将所有的点添加到线条（LINE_STRIP）中 + # 将所有的点添加到 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() diff --git a/src/position_demo/scripts/subscriber_node_three.py b/src/position_demo/scripts/subscriber_node_three.py new file mode 100755 index 0000000..4bbe8f9 --- /dev/null +++ b/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() diff --git a/src/position_demo/src/FftService.srv b/src/position_demo/src/FftService.srv new file mode 100644 index 0000000..7d8a908 --- /dev/null +++ b/src/position_demo/src/FftService.srv @@ -0,0 +1,3 @@ +float64[128] data +--- +float64[128] fft_result \ No newline at end of file diff --git a/src/position_demo/src/fft_service.py b/src/position_demo/src/fft_service.py new file mode 100644 index 0000000..2ef3ea2 --- /dev/null +++ b/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()