[rospy] ctrl-cで綺麗に終わるようにする

ワーニングメッセージ

ROSのノードをctrl-cで終了させた時、そのノードと通信している他のノードで次のようなメッセージが出ることがあった。

.sh

[WARN] [1542224645.663897]: Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.

これはPythonでノードを動かしている場合、ctrl-cが裏で動いているスレッドを途中で止めてしまう事に原因があるように思われる。下記のプログラムテンプレートでは省略しているがsubscribeでコールバックがある場合もその処理は別スレッドなので同様の問題が発生する。
これを解決するにはctrl-cの処理をrospyにまかせずに自前で綺麗に書けば良い。

プログラムテンプレート

the_node.py

# !/usr/bin/env python

import signal
import rospy

cont = True

def handler(signal, frame):
    global cont
    cont = False

def the_node():
    global cont

    rospy.init_node('the_node', disable_signals = True)

    rate = rospy.Rate(40)
    while cont:
        ~~~
        rate.sleep()

    rospy.signal_shutdown('finish')
    rospy.spin()

if __name__ == '__main__':
    signal.signal(signal.SIGINT, handler)
    the_node()

• 開始時:
  init_nodeでdisable_signals = Trueとしてrospyにctrl-cの処理をさせないようにする。
• 終了時:
  signal_shutdownでrospyにshutdownを司令してspinでshutdownするまで待つ。
• 全般:
  contはグローバル変数で別スレッドからもちゃんと読めるように各defでglobal contと宣言する必要がある。

シリアル通信

特にシリアル通信をしている場合、ROSをctrl-cでshutdownさせると通信途中で処理が中断してしまう確率が高い。そんなケースでもこの方法は有効だ。シリアル通信している部分を

.py

try:
    sp.write(~)
    sp.flush()
except KeyboardInterrupt:
    cont = False

みたいにし、rospy.signal_shutdown('finish')の前に

.py

sp.close()

すれば綺麗にシリアルポートを終了でき、次にちゃんとopenできる。