ROSにおけるシャットダウン処理の書き方

ROSのノードを動かした時、Ctrl-Cでそのノードを落としたいのに、落ちてくれない場合ありますよね。また落ちたとしても、下記の様なexceptionが発生したりするなどして、キレイに落ちてくれなかったりします。

terminate called after throwing an instance of 'boost::exception_detail::clone_impl >'
what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument
Aborted (core dumped)

これは[1]にもありますが、callbackやtimerが動いているのが問題です。そのため[1]に記載の様にシグナルハンドラを設定し、そのコールバック中でtimer[3]やsubscriber[2]を止め、さらにノードのshutdownを行なう必要があります（下記参照）。

signal(SIGINT, signal_handler);

void signal_hander(int signum)
{
/* 下記がtimerやsubscriber, nodeの停止用関数。実行するタイミングはプログラムに依存するので、必ずしもハンドラで実行すればよい訳ではない */
  timer.stop();        // timerの停止。この関数はtimerの中で呼んだ方がよいかもしれない。
  sub.shutdown(); // subscriberの停止
  ros::shutdown(); // ノードの停止。これを呼ぶとspinから抜ける。
}

上記終了処理に伴い、main関数中でspin関数を呼ぶ部分は下記の様に書く必要があります。spinOnceでなくspinを使ってしまうと、シグナルハンドラは呼ばれるものの、spin関数から抜けられないためにwhile文から抜けられません。終了処理を書きたい場合は、spinOnceを使うようにしましょう。そんなことはありません、ちゃんと抜けます。下記のcase1, 2のどちらでも構いません。

/* case 1 */
ros::spin();

/* case 2 */
ros::Rate rate(1);
while( false == ros::isShuttingDown() )
{
    ros::spinOnce();
    rate.sleep();
}

参考文献

[1]roscpp/Overview/Initialization and Shutdown - ROS Wiki
[2]roscpp: ros::Subscriber Class Reference
[3]roscpp: ros::Timer Class Reference