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

class TShapeControl(Node):
    def __init__(self):
        super().__init__('t_shape_control')
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)

    def move(self, linear=0.0, angular=0.0, duration=2.0):
        twist = Twist()
        twist.linear.x = linear
        twist.angular.z = angular
        end_time = time.time() + duration
        while time.time() < end_time:
            self.publisher.publish(twist)
            time.sleep(0.1)

    def run(self):
        while rclpy.ok():
            self.move(0.2, 0.0, 3.0)   # 前进
            self.move(0.0, 0.5, 2.0)   # 左转
            self.move(0.2, 0.0, 3.0)   # 前进
            self.move(-0.2, 0.0, 3.0)  # 回退
            self.move(0.0, -0.5, 2.0)  # 右转
            self.move(0.2, 0.0, 3.0)   # 前进

def main(args=None):
    rclpy.init(args=args)
    node = TShapeControl()
    node.run()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()