完善ROS中的话题通讯

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

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 original_trajectory
-    global noisy_pub, marker_pub
+    global 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
 
     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 = []
+    marker.points = [Point(x=p.x, y=p.y, z=p.z) for p in original_trajectory]
-    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_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()