Update README.md

README.md

@@ -1,4 +1,4 @@
-
+作业提交里的t_shape代码有错，这里更新了一下
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
@@ -53,7 +53,6 @@ if __name__ == '__main__':
     main()
 
 ##################################################################################################################
-
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
@@ -62,16 +61,10 @@ import time
 def main():
     rclpy.init()
     
-
     node = Node('t_shape_movement')
-    
-
     publisher = node.create_publisher(Twist, '/cmd_vel', 10)
     
     node.get_logger().info("=== 开始T字形运动控制 ===")
-    node.get_logger().info("机器人将循环执行T字形路径")
-    
-
     time.sleep(2.0)
     
     cycle_count = 0
@@ -79,106 +72,104 @@ def main():
     try:
         while True:
             cycle_count += 1
+            node.get_logger().info(f"=== 第 {cycle_count} 次T字形循环开始 ===")
             
-            # T字向上移动
-            node.get_logger().info("T字竖线: 向前移动")
+            # 1. 从起点到中心点（竖线）- 面向前
+            node.get_logger().info("竖线: 向前移动到中心点")
             twist = Twist()
-            twist.linear.x = 0.15  # 前进速度
+            twist.linear.x = 0.15
             publisher.publish(twist)
-            time.sleep(2.5)  # 前进2.5秒
-            
-            # 停止一下
-            node.get_logger().info("暂停")
+            time.sleep(2.0)
             twist = Twist()
             publisher.publish(twist)
             time.sleep(0.5)
             
-            # T字左转并前进
-            node.get_logger().info("T字横线左半: 左转")
+            # 2. 左横线：左转90度前进然后返回
+            node.get_logger().info("左横线: 左转90度")
             twist = Twist()
-            twist.angular.z = 0.4  # 左转速度
+            twist.angular.z = 0.5
             publisher.publish(twist)
-            time.sleep(1.2)  # 左转1.2秒（约45度）
-            
-            node.get_logger().info("暂停")
+            time.sleep(3.14)  # 90度，现在面向左
             twist = Twist()
             publisher.publish(twist)
             time.sleep(0.3)
             
-            node.get_logger().info("T字横线左半: 向前移动")
+            node.get_logger().info("左横线: 向左前进")
             twist = Twist()
             twist.linear.x = 0.15
             publisher.publish(twist)
-            time.sleep(2.0)  # 前进2秒
-            
-            node.get_logger().info("暂停")
+            time.sleep(1.5)  # 左横线前进
             twist = Twist()
             publisher.publish(twist)
             time.sleep(0.5)
             
-            # 回到T字中心点
-            node.get_logger().info("回到T字中心: 向后移动")
+            node.get_logger().info("左横线: 后退回中心")
             twist = Twist()
-            twist.linear.x = -0.15  # 后退速度
+            twist.linear.x = -0.15
             publisher.publish(twist)
-            time.sleep(2.0)  # 后退2秒
-            
-            node.get_logger().info("暂停")
+            time.sleep(1.5)  # 后退到中心，仍面向左
             twist = Twist()
             publisher.publish(twist)
             time.sleep(0.3)
             
-            # T字右转并前进
-            node.get_logger().info("T字横线右半: 右转")
+            # 3. 右横线：右转180度前进然后返回
+            node.get_logger().info("右横线: 右转180度")
             twist = Twist()
-            twist.angular.z = -0.4  # 右转速度
+            twist.angular.z = -0.5
             publisher.publish(twist)
-            time.sleep(2.4)  # 右转2.4秒（约90度）
-            
-            node.get_logger().info("暂停")
+            time.sleep(6.28)  # 180度，从面向左转到面向右
             twist = Twist()
             publisher.publish(twist)
             time.sleep(0.3)
             
-            node.get_logger().info("T字横线右半: 向前移动")
+            node.get_logger().info("右横线: 向右前进")
             twist = Twist()
             twist.linear.x = 0.15
             publisher.publish(twist)
-            time.sleep(2.0)  # 前进2秒
-            
-            node.get_logger().info("暂停")
+            time.sleep(1.5)  # 右横线前进
             twist = Twist()
             publisher.publish(twist)
             time.sleep(0.5)
             
-            # 回到起点准备下一次循环
-            node.get_logger().info("回到起点: 向后移动")
+            node.get_logger().info("右横线: 后退回中心")
             twist = Twist()
             twist.linear.x = -0.15
             publisher.publish(twist)
-            time.sleep(2.0)  # 后退2秒
+            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("回到起点: 左转")
+            node.get_logger().info("回到起点: 向后移动")
             twist = Twist()
-            twist.angular.z = 0.4
+            twist.linear.x = -0.15
             publisher.publish(twist)
-            time.sleep(1.2)  # 左转1.2秒
+            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()