經過這麼多天的鋪陳，今天真的要控制無人機啦！

控制無人機飛行

首先，前往套件的 src 目錄建立控制程式。這裡示範可以讓無人機飛出一個正方形軌跡的程式 square_move.py，主要是利用發布 ROS 話題來指揮無人機，寫法和之前介紹過 Day 6 寫節點以及 Day 11 發布者節點概念大同小異。

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import time

from std_msgs.msg import Empty
from geometry_msgs.msg import Twist

class MoveSquare(object):
    def __init__(self):
        self.rate = rospy.Rate(10)

        # 設定發布者
        self.takoff_pub = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=10)
        self.land_pub = rospy.Publisher('/ardrone/land', Empty, queue_size=10)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        rospy.sleep(2) # 等待 publishers 完全連線

        self.takoff_msg = Empty()
        self.land_msg = Empty()
        self.cmd_msg = Twist()

    def takoff(self):
        while not rospy.is_shutdown():
            connections = self.takoff_pub.get_num_connections()
            if connections > 0:
                rospy.loginfo("Taking off...")
                self.takoff_pub.publish(self.takoff_msg)
                self.rate.sleep()
                break

        rospy.sleep(2) # 等待執行完成

    def land(self):
        while not rospy.is_shutdown():
            connections = self.land_pub.get_num_connections()
            if connections > 0:
                rospy.loginfo("Landing...")
                self.land_pub.publish(self.land_msg)
                self.rate.sleep()
                break

    def move_forward(self, duration, speed):
        start_time = int(round(time.time()))
        self.cmd_msg.linear.x = speed
        
        while not rospy.is_shutdown():
            rospy.loginfo("Moving forward...")
            connections = self.cmd_vel_pub.get_num_connections()
            if connections > 0:
                if time.time() - start_time < duration:
                    self.cmd_vel_pub.publish(self.cmd_msg)
                else:
                    break
                self.rate.sleep()
        
        # 停止無人機運動
        self.cmd_msg.linear.x = 0.0
        self.cmd_vel_pub.publish(self.cmd_msg)

        rospy.sleep(2) # 等待執行完成

    def turn_right(self, duration, speed):
        start_time = int(round(time.time()))
        self.cmd_msg.angular.z = -1 * speed

        while not rospy.is_shutdown():
            rospy.loginfo("Turning right...")
            connections = self.cmd_vel_pub.get_num_connections()
            if connections > 0:
                if int(round(time.time())) - start_time < duration:
                    self.cmd_vel_pub.publish(self.cmd_msg)
                else:
                    break
                self.rate.sleep()
        
        # 停止無人機運動
        self.cmd_msg.angular.z = 0.0
        self.cmd_vel_pub.publish(self.cmd_msg)

        rospy.sleep(2) # 等待執行完成

    def move_upward(self, duration, speed):
        start_time = int(round(time.time()))
        self.cmd_msg.linear.z = speed
        
        while not rospy.is_shutdown():
            rospy.loginfo("Moving upward...")
            connections = self.cmd_vel_pub.get_num_connections()
            if connections > 0:
                if time.time() - start_time < duration:
                    self.cmd_vel_pub.publish(self.cmd_msg)
                else:
                    break
                self.rate.sleep()
        
        # 停止無人機運動
        self.cmd_msg.linear.z = 0.0
        self.cmd_vel_pub.publish(self.cmd_msg)

        rospy.sleep(2) # 等待執行完成

if __name__ == "__main__":
    rospy.init_node("move_square", anonymous=True)

    move_square = MoveSquare()

    try:
        move_square.takoff() # 起飛
        move_square.move_upward(duration=3, speed=0.5) # 上升

        for i in range(3):
            move_square.move_forward(duration=3, speed=1) # 向前飛行
            move_square.turn_right(duration=2, speed=1.2) # 向右旋轉

        move_square.move_forward(duration=3, speed=1) # 向前飛行
        move_square.land() # 降落

        rospy.loginfo("Flight completed.")
    except rospy.ROSInterruptException:
        pass

和之前的改動主要是改成類別的寫法。

1. 在 __init__() 設定會共享的發布者（Publisher）以及指定要發布的話題，我這裡將會用到話題分成三個：/ardrone/takeoff （負責發佈起飛指令）、/ardrone/land （負責發佈降落指令）以及 /cmd_vel （負責發佈控制無人機速度的訊息）

2. get_num_connections() 是 publisher 物件可以使用的一個方法，用來檢查是否已與訂閱者（Subscriber）建立連線。會需要檢查，是因為我們沒辦法確定系統中每一個程序是不是按照順序執行，因此需要避免連線還沒建立好發布者就急著發布訊息，導致指令無法正確執行。

控制程式轉換成可執行檔

程式寫好之後，也順便將 python 撰寫的控制程式轉換成可執行檔，以便之後能直接使用 rosrun 執行。

$ chmod +x square_move.py

到這邊還先別急著執行！還記得前面幾篇介紹怎麼把模型匯入 Gazebo 中的啟動檔、世界檔的內容嗎？那時並沒有把 ardrone_gazebo 的無人機模型放進去，因此還需要再進行調整。另外，還有一些系統設定的眉眉角角也最好在執行程式前先準備好，因此下一篇才會正式在 Gazebo 中讓無人機非起來。