Move as triangle (Not completed)

pub.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy # เรียก Libray จากภายนอก
from geometry_msgs.msg import Twist
import time

vel = Twist()  # สร้าง Object สำหรับเปลี่ยนความเร็วหุ่นยนต์

def main (): # ฟังก์ชั่นหลัก
    rospy.init_node('turtle_commander', anonymous=True) # Regis Node บน Master
    rate = rospy.Rate(10) # คุม Sampling time ของ Node นี้ (Hz)
    # สร้างตัวแปร pub สำหรับการ Publish ข้อมูล ไปยัง Topic /turtle1/cmd_vel
    pub = rospy.Publisher('/turtle1/cmd_vel', Twist,queue_size=10)
    
    timestart = time.time()
    while not rospy.is_shutdown():  # ลูปในการควบคุมหุ่นยนต์
        timenow = time.time()
        counter = timenow - timestart # สำหรับนับเวลา (หน่วย วินาที)
        # TODO
        # print("Turtle is running.", counter)

        if (counter < 2):
            vel.linear.x = 1
            print("CASE 1", counter)
        elif (counter >= 2 and counter < 4):
            vel.linear.x = 0
            vel.angular.z = 1
            print("CASE 2", counter)
        # elif (counter >= 4 and counter < 6):
        #     vel.linear.x = 1
        #     vel.angular.z = 0
        #     print("CASE 3")
        else:
            vel.linear.x = 0
            vel.angular.z = 0
            print("CASE FINAL", counter)
            break

        # END
        pub.publish(vel)
        rate.sleep()
        
if __name__ == '__main__':
    try:
        main ()
    except rospy.ROSInterruptException:
        pass