day2_assignment_pub.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy     # เรียก external library ชื่อ rospy
from geometry_msgs.msg import Twist # เรีียก lib Twist จาก package geometry_msgs.msg
import time      # เรียก lib ชื่อ time

vel = Twist()    # สร้าง object ชื่อ vel จาก Class Twist

# def คือการประกาศฟัังก์ชั่นใน Python
def main ():
    # Code ชุดนี้อยู่ใน main ฟังก์ชั่น จะต้องวรรค 4 ช่อง
    rospy.init_node('turtle_commander', anonymous=False) # ประกาศโหนดใหม่ชื่อ turtle_commander
    rate = rospy.Rate(10) # ความถี่ในการ Publish คือ 10 Hz 100ms
    # สร้าง obj ชื่อ pub ไว้ publish ข้อมูลไปยัง topic /turtle1/cmd_vel
    # โดยชนิดข้อมูลคือ Twist 
    pub = rospy.Publisher('/turtle1/cmd_vel', Twist,queue_size=10)
    
    timestart = time.time() # สร้างตัวแปรเพื่อเก็บเวลา เริ่มต้น

    while not rospy.is_shutdown(): # ทำซ้ำจนกว่าเงื่อนไขจะผิด โดย is_shutdown จะเป็น จริง เมื่อเรากด ctrl+c
        timenow = time.time() # สร้างตัวแปรเพื่อเก็บเวลา ณ ปัจจุบัน
        counter = timenow - timestart # เวลานับตั้งแต่เริ่มรัน (หน่วย ms)
        # TODO
        print("Turtle is running. ", counter) # แสดงข้อความบนจอ

        if (counter >= 0 and counter < 5):
            vel.linear.x  = 0.2
            vel.angular.z = 0.0
        elif (counter >= 5 and counter < 12):
            vel.linear.x  = 0.0
            vel.angular.z = 0.2
        else:
            vel.linear.x  = 0.0
            vel.angular.z = 0.0
            break


        # END
        pub.publish(vel) # ทำการ publish ข้อมูลใน vel เข้าระบบ
        rate.sleep()     # หน่วงเวลาเพื่อคุมจังหวะในการ Publish
    

if __name__ == '__main__':
    try:
        main ()
    except rospy.ROSInterruptException:
        pass