From 4d34c200dc22631768636ee0c2bf1a43eae8eff4 Mon Sep 17 00:00:00 2001 From: Cx330 <1487537121@qq.com> Date: Wed, 2 Jul 2025 13:37:37 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84ROS=E4=B8=AD=E7=9A=84?= =?UTF-8?q?=E8=AF=9D=E9=A2=98=E9=80=9A=E8=AE=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Topic_Newsletter/scripts/Publisher_TN.py | 2 +- src/Topic_Newsletter/scripts/Subscriber_TN.py | 44 +++---------------- 2 files changed, 6 insertions(+), 40 deletions(-) diff --git a/src/Topic_Newsletter/scripts/Publisher_TN.py b/src/Topic_Newsletter/scripts/Publisher_TN.py index 9c87cc3..fa7f6b2 100644 --- a/src/Topic_Newsletter/scripts/Publisher_TN.py +++ b/src/Topic_Newsletter/scripts/Publisher_TN.py @@ -19,7 +19,7 @@ def main(): msg.point.y = math.cos(0.05 * math.pi * k) # y 值 msg.point.z = 0.0 pub.publish(msg) # 发布话题 - rospy.loginfo("Published: x=%.3f, y=%.3f" % (msg.point.x, msg.point.y)) # 打印坐标信息 + rospy.loginfo("发布端发送坐标信息: x=%.3f, y=%.3f" % (msg.point.x, msg.point.y)) # 打印坐标信息 k += 1 rate.sleep() diff --git a/src/Topic_Newsletter/scripts/Subscriber_TN.py b/src/Topic_Newsletter/scripts/Subscriber_TN.py index c5adc56..1980d4e 100644 --- a/src/Topic_Newsletter/scripts/Subscriber_TN.py +++ b/src/Topic_Newsletter/scripts/Subscriber_TN.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import rospy -import random from geometry_msgs.msg import PointStamped, Point from visualization_msgs.msg import Marker, MarkerArray from std_msgs.msg import ColorRGBA @@ -9,28 +8,15 @@ from std_msgs.msg import ColorRGBA MAX_POINTS = 100 original_trajectory = [] -noisy_trajectory = [] -noisy_pub = None marker_pub = None def position_callback(msg): - global original_trajectory, noisy_trajectory - global noisy_pub, marker_pub - - noise_x = random.uniform(-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 # 对 x 添加噪声 - noisy_msg.point.y = msg.point.y + noise_y # 对 y 添加噪声 - noisy_msg.point.z = 0.0 + global original_trajectory + global marker_pub 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) marker_array = MarkerArray() original_line = Marker() @@ -45,18 +31,6 @@ def position_callback(msg): 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) - marker = Marker() marker.header.frame_id = "map" # 设置坐标系 marker.header.stamp = rospy.Time.now() @@ -66,26 +40,18 @@ def position_callback(msg): marker.scale.y = 0.025 # 点的直径 marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # 红色 - points = [] - for point in noisy_trajectory: - p = Point() - p.x = point.x - p.y = point.y - p.z = 0.0 - points.append(p) - marker.points = points + marker.points = [Point(x=p.x, y=p.y, z=p.z) for p in original_trajectory] marker_array.markers.append(marker) marker_pub.publish(marker_array) - rospy.loginfo("Published noisy point: x=%.3f, y=%.3f | Noisy: x=%.3f, y=%.3f",msg.point.x, msg.point.y, noisy_msg.point.x, noisy_msg.point.y) + rospy.loginfo("订阅端接收坐标信息: x=%.3f, y=%.3f ",msg.point.x, 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) - rospy.loginfo("Node initialized, waiting for position data...") + rospy.loginfo("订阅端节点启动成功，等待发布端信息。") rospy.spin() \ No newline at end of file