ROS2解決服務嵌套、回調嵌套死鎖問題

以前做開發都是用ros1+c++開發，最近入手深度強化學習開始了ros2+Python的開發之旅，下面是記錄一次死鎖的解決過程。由於話題、定時器、服務、動作、參數服務器等方式都是通過回調函數觸發，所以我們通過定時器嵌套服務的方式舉例。

背景介紹

死鎖問題，對於熟悉C++多線程開發使用mutex的朋友肯定不陌生，死鎖問題是程序中比較頭禿的bug之一，往往是鎖設置的不合理導致連環鎖，最後鎖死程序啥也幹不了。ros2還是一往ros1的操作，其中一個必要重要的東西就是回調函數，話題、定時器、服務、動作、參數服務器都會產生回調函數，默認情況下程序是以單線程工作的，回調函數通過相應的事件觸發過後按照時間順序形成一個隊列，然後這個線程就取出第一個回調進行處理，這個回調結束過後取下一個回調進行處理，依次反覆。這個過程看上去好像並沒有什麼大問題，但是想象如果我要實現一個功能定時呼叫一個服務，得到服務的執行結果。好比我們每天早上的鬧鐘都要在固定的時間響，這個可以比作定時器的功能，然後他調用的服務就是把我們叫醒，如果我們關了鬧鐘繼續睡這個服務給我們返回值就是叫醒操作失敗，如果成功起牀就表示叫醒操作成功。但是對於程序來說，特別對於單線程程序來說由於一次只能處理一個回調，這裏注意服務是雙向回調的，對於服務的客戶端由於我們需要得到結果所以也會有回調函數，當定時器達到時間產生了一個回調過後開始處理這個回調，回調裏面對呼叫一個服務，我們等待服務完成打印服務的結果，這個時候問題就來了，由於當前的時間回調函數沒有執行完，所以程序會一直等待呼叫的服務完成，但是得到服務的結果需要另外一個回調來得到結果，但是當前又無法結束目前的時間回調函數這個時候死鎖就來了，程序就一直卡死。
問題就是出現在服務的回調函數無法被執行，回調的狀態就無法得到更新。有問題就得解決，編程語法千千萬，解決方案也是千千萬，首先我們想到的就是多線程，給這個服務回調開一個新的線程執行回調函數，這樣就可以了，這確實是一個解決辦法，C++裏面的爲了提高性能我們一般都是採用多線程回調，在Python裏面經過實驗確實也可以解決問題，但是這樣做無疑不是一個很好的辦法，而且和我們人的思維方式也不太一樣，如果換做一個人來思考這個問題，我們遇到需要等待的問題往往是先放下當前的活轉而去幹別的事，對任務進行調整，例如在蒸麪包的時候我們不用一直在那守着，我們一般是回去乾點別的事，這就設計到一個新的概念叫做協程。很多語言都有協程的操作，Python的協程經過不斷的完善現在已經比較的方便了，C++在C++20中也提出了協程。下面就是講如何通過協程來解決這個死鎖的問題。

基本概念

對於ros2在Python的回調實現有兩個比較重要的概念，執行器(executors)和回調組(callback groups) ,詳細的內榮可以查看官網鏈接，簡單來說所謂的執行器可以理解爲一個線程，就是真正執行這個回調的東西，而回調組就是對組的回調進行管理，對這些發生的回調的執行順序進行管理。
關於什麼是協程可以參考我的上一篇博客, 裏面對什麼是協程做了一個感性的講解。

代碼實現

爲了避免死鎖我們用async將時間回調函數聲明爲一個異步的函數表示這個函數是可以中斷跳出的，然後通過異步呼叫服務的方式調用服務，用await等待返回結果。具體實現如下。
服務的服務器端我們就用一個簡單的做加法的例子

from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node

class MinimalService(Node):
    def __init__(self):
        super().__init__('minimal_service')
        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.add_two_ints_callback)

    def add_two_ints_callback(self, request, response):
        response.sum = request.a + request.b
        self.get_logger().info('Incoming request\na: %d b: %d' % (request.a, request.b))
        return response

def main(args=None):
    rclpy.init(args=args)
    minimal_service = MinimalService()
    rclpy.spin(minimal_service)
    rclpy.shutdown()


if __name__ == '__main__':
    main()

接下來是在定時器回調裏面調用這個服務

import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
from example_interfaces.srv import AddTwoInts

def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('minimal_client')
    cb_group = ReentrantCallbackGroup() # 這個類型的回調組運行一旦產生回調就執行
    cli = node.create_client(AddTwoInts, 'add_two_ints', callback_group=cb_group)

    async def call_service():
        nonlocal cli, node
        req = AddTwoInts.Request()
        req.a = 41
        req.b = 1
        future = cli.call_async(req)

        try:
            result = await future
        except Exception as e:
            node.get_logger().info('Service call failed %r' % (e,))
        else:
            node.get_logger().info(
                'Result of add_two_ints: for %d + %d = %d' %
                (req.a, req.b, result.sum))
 
    while not cli.wait_for_service(timeout_sec=1.5):
        node.get_logger().info('service not available, waiting again...')

    timer = node.create_timer(1.0, call_service, callback_group=cb_group)

    try:
        rclpy.spin(node)
    except KeyboardInterrupt :
        print('ctrl + c exit')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

編譯運行就可以看到結果，每隔一秒請求做一次加法。

總結

死鎖的關鍵點就在調用服務的結果狀態得不到更新，可以通過多線程和協程的方式讓狀態得到更新，但是協程無疑在這種情況下是一個比較優的解決辦法，多線程往往會帶來資源競爭和狀態混亂的風險，協程是由用戶設計的任務的調度這種方式會比多進程安全一點。
ROS2現在已經不斷完善，相比ROS1還是非常不錯的，後面會不斷分享一些實現並行、併發和ROS2的小技巧和心得。