作业提交里的t_shape代码有错，这里更新了一下 import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist import time

def main(): rclpy.init()

node = Node('circle_navigation')

publisher = node.create_publisher(Twist, '/cmd_vel', 10)

node.get_logger().info("=== 开始绕圈导航 ===")


time.sleep(2.0)

try:

    movements = [
        {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
        {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
        {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
        {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
        {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
        {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
        {"name": "前进3秒回到起点", "linear": 0.2, "angular": 0.0, "duration": 3.0},
    ]
    
    for i, move in enumerate(movements):
        node.get_logger().info(f"动作 {i+1}/{len(movements)}: {move['name']}")
        
        twist = Twist()
        twist.linear.x = move['linear']
        twist.angular.z = move['angular']
        publisher.publish(twist)
        
        time.sleep(move['duration'])
    
    node.get_logger().info("=== 绕圈导航完成 ===")
    
finally:
    stop_twist = Twist()
    publisher.publish(stop_twist)
    node.get_logger().info("机器人已停止")
    node.get_logger().info("很多没明白用AI写的呜呜呜")
    
    node.destroy_node()
    rclpy.shutdown()

if name == 'main': main()

################################################################################################################## import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist import time

def main(): rclpy.init()

node = Node('t_shape_movement')
publisher = node.create_publisher(Twist, '/cmd_vel', 10)

node.get_logger().info("=== 开始T字形运动控制 ===")
time.sleep(2.0)

cycle_count = 0

try:
    while True:
        cycle_count += 1
        node.get_logger().info(f"=== 第 {cycle_count} 次T字形循环开始 ===")
        
        # 1. 从起点到中心点（竖线）- 面向前
        node.get_logger().info("竖线: 向前移动到中心点")
        twist = Twist()
        twist.linear.x = 0.15
        publisher.publish(twist)
        time.sleep(2.0)
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.5)
        
        # 2. 左横线：左转90度前进然后返回
        node.get_logger().info("左横线: 左转90度")
        twist = Twist()
        twist.angular.z = 0.5
        publisher.publish(twist)
        time.sleep(3.14)  # 90度，现在面向左
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.3)
        
        node.get_logger().info("左横线: 向左前进")
        twist = Twist()
        twist.linear.x = 0.15
        publisher.publish(twist)
        time.sleep(1.5)  # 左横线前进
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.5)
        
        node.get_logger().info("左横线: 后退回中心")
        twist = Twist()
        twist.linear.x = -0.15
        publisher.publish(twist)
        time.sleep(1.5)  # 后退到中心，仍面向左
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.3)
        
        # 3. 右横线：右转180度前进然后返回
        node.get_logger().info("右横线: 右转180度")
        twist = Twist()
        twist.angular.z = -0.5
        publisher.publish(twist)
        time.sleep(6.28)  # 180度，从面向左转到面向右
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.3)
        
        node.get_logger().info("右横线: 向右前进")
        twist = Twist()
        twist.linear.x = 0.15
        publisher.publish(twist)
        time.sleep(1.5)  # 右横线前进
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.5)
        
        node.get_logger().info("右横线: 后退回中心")
        twist = Twist()
        twist.linear.x = -0.15
        publisher.publish(twist)
        time.sleep(1.5)  # 后退到中心，仍面向右
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.3)
        
        # 4. 回到起点：左转90度后退
        node.get_logger().info("回到起点: 左转90度")
        twist = Twist()
        twist.angular.z = 0.5
        publisher.publish(twist)
        time.sleep(3.14)  # 90度，从面向右转到面向前
        twist = Twist()
        publisher.publish(twist)
        time.sleep(0.3)
        
        node.get_logger().info("回到起点: 向后移动")
        twist = Twist()
        twist.linear.x = -0.15
        publisher.publish(twist)
        time.sleep(2.0)  # 后退到起点
        
        node.get_logger().info(f"=== 第 {cycle_count} 次T字形完成 ===")
        node.get_logger().info("等待3秒后开始下一次循环...")
        twist = Twist()
        publisher.publish(twist)
        time.sleep(3.0)
        
except KeyboardInterrupt:
    node.get_logger().info("程序被用户中断")
except Exception as e:
    node.get_logger().error(f"发生错误: {e}")
finally:
    twist = Twist()
    publisher.publish(twist)
    node.get_logger().info("机器人已停止")
    node.destroy_node()
    rclpy.shutdown()

if name == 'main': main()