VS-RC003を搭載した"KUMACO"をROSで動かす(後編)「KUMACOを画像処理から制御する」

#概要

VS-RC003というVstone社のロボット用制御ボードを搭載したロボットをROSと接続して利用するお話を投稿させていただきます。
課題として、"KUMACO(クマコ)"という名前のぬいぐるみ型のホビーロボットを用いて、"いないいないバァ(顔が見えたら喜ぶ)"させたいと思います。

完成形はこんな感じです。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ぬいぐるみロボット、KUMACOに目が付いて、息子と”いないいないバァ”で遊べるように進化しました！<br><br>枯れたVS-RC003プラットフォームの二足ロボも、ROS化すれば命が宿ります(*⁰▿⁰*) <a href="https://t.co/EdMui2YzxN">pic.twitter.com/EdMui2YzxN</a></p>&mdash; みっちー (@Dream_Drive) <a href="https://twitter.com/Dream_Drive/status/1205626987265421312?ref_src=twsrc%5Etfw">December 13, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

[前回](https://qiita.com/Dream_Drive/items/4a28119d1275f6e3dcad)は、VS-RC003をROSから仮想コントローラーのコマンドで、制御することが出来ました。
今回は、ROSを使って、カメラ画像からKUMACOの行動を制御したいと思います。ついに、"いないいないバァ"が完成です。

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i7-9750H |
| Ubuntu | 18.04 |
| ROS | Melodic |

# 仕様

まず、"いないいないバァ"を実行するにあたって、大雑把に仕様を決めます。

- 顔が見えたら、「やったぁ！」のモーションを実行する
- 顔が見えないときは、アイドリングモーションを実行する

という設計で行きたいと思います。

# 画像処理の準備

顔を判別するためには、カメラが必要ですので、まずは画像処理に必要なパッケージ(カメラ画像のパッケージとOpenCVのパッケージ)をインストールします。

~~~bash:必要なパッケージのインストール
$ sudo apt-get install ros-melodic-opencv-apps 
$ sudo apt-get install ros-melodic-usb-cam
~~~

# 画像処理(顔認識)のノードの起動

先程インストールしたopencv_appsの中には、今回まさに使えそうな顔検出(face_detectio)ノードが含まれますので、横着してそのまま使ってしまいましょう。

~~~bash:カメラノードの起動とOpenCV顔検出ノードの起動
$ roscore
$ rosrun uvc_camera uvc_camera_node
$ roslaunch opencv_apps face_detection.launch image:=/image_raw
~~~

立ち上げると、顔検出のプレビュー画面が表示されると同時に、検出した顔と目の座標が配信されます。
<img width="800" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/231185/266e4eaf-2de6-75a7-51af-56c261f8fc29.png">

ちなみに、画像上に２つ以上の顔があった場合は、それぞれの顔の座標を返します。
ただし、顔の判別まではできないので、今回はとりあえず「顔が１つ以上あるかどうか？」というところに着眼したいと思います。

画像処理に関して、他人の褌だけで完了してしまったような感じですが(^^;A
これもある意味ROSの醍醐味かなと思います。

あとはface_detectionノードの結果を用いて、前回紹介したros2vsrc/vsrc_connectノードに司令を送るノードを作れば良さそうですね！
顔の有無によって、ボタンの状態を変化させるようなノードを作りましょう、あと一息！

# 顔検出状況ノードから、コントローラのボタンの状態を変化させるノードを作る

いきなりですが、結論から書くと、下記のようなノードになります。

~~~C++:face2happy.cpp(顔を検出すると、R3が押されるノード)
#include "ros/ros.h"
#include "ros/time.h"

#include "opencv_apps/FaceArrayStamped.h"
#include "ros2vsrc/VsrcControl.h"

double face_x = 0;
double face_y = 0;

void face_callback(const opencv_apps::FaceArrayStamped::ConstPtr& facearraystamped)
{
  face_x = 0; // 未検出のときは0
  face_y = 0; // 未検出のときは0

  if ( facearraystamped->faces.size() > 0 ){
    face_x = facearraystamped->faces[0].face.x;
    face_y = facearraystamped->faces[0].face.y;
  }
}

int main(int argc, char **argv){
  ros::init(argc, argv, "face2happy");  // ノードの初期化
  ros::NodeHandle nh; // ノードハンドラ

  ros::Subscriber sub_face;  // サブスクライバの作成
  sub_face = nh.subscribe("/face_detection/faces", 10, face_callback);

  ros2vsrc::VsrcControl vsrcctl;  // パブリッシャの登録
  ros::Publisher vsrc_pub = nh.advertise<ros2vsrc::VsrcControl>("vsrc_control",10);

  ros::Rate loop_rate(10);  // 制御周期10Hz
  
  while(ros::ok()) {
    vsrcctl.w_torque =	1;  // VS-RC003トルクオン

    if ((face_x == 0)&&(face_y == 0)){        // 顔を未検出のときR3を離す
      ROS_INFO("Not found face (R3 = OFF)");
      vsrcctl.b_R3 = 0;
    }
    else{                                     // 顔を検出したときR3を押す
      ROS_INFO("found face !!! (R3 =  ON) | Face = (%f , %f)",face_x,face_y);
      vsrcctl.b_R3 = 1;
    }

    vsrc_pub.publish(vsrcctl);			// コントローラーの値を送信
    ros::spinOnce();                // コールバック関数を呼ぶ
    loop_rate.sleep();
  }

  return 0;
}
~~~

/face_detection/facesをSubscribeし、検出された顔の座標を取り込みます。
1つ以上の顔が検出されると、1つ目の顔の座標をface_x,face_yに代入します。

つまり、未検出の場合は顔のface_x,face_y座標が(0,0)になるため、顔の座標が(0,0)以外のときはKUMACOが誰かの顔を認識したということになります。
この真偽をもとに、メインループ内では顔が検出されればR3ボタンをTrueにし、検出されなければR3ボタンをFalseにします。

こちら、face2happyのノードに必要なファイルをパッケージにして、[GitHub](https://github.com/dreamdrive/kumaco_control)に公開していますので、ご参考まで。
さくっとインストールする場合は、下記の通りターミナルで入力していただければOKです。

~~~
$ cd ~/catkin_ws/src
$ git clone https://github.com/dreamdrive/kumaco_control.git
$ cd ..
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash
~~~

さて、ここまで準備できたら、さっそく実行しましょう。
画像処理のノードは先程立ち上げましたので、今回作成したkumaco_controlノードと、[前回](https://qiita.com/Dream_Drive/items/4a28119d1275f6e3dcad)使えるようになった、ros2vsrc/vsrc_connectも起動しまいましょう。

~~~bash:KUMACOの制御ノードと、"いないいないバァ"ノードの起動
$ rosrun ros2vsrc vsrc_connect _serialdev:=/dev/ttyUSB0
$ rosrun kumaco_control face2happy
~~~

こちらを実行すると、冒頭のTwitterの動画の通り、KUMACOが動作します！
ノードのつながりは下記の通りとなります。一直線ですね。

<img width="800" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/231185/c8f04c21-7941-e811-0d11-e628421806a3.png">

"いないいないバァ"する、ちょっと賢くなったKUMACOの完成です！

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">ぬいぐるみロボット、KUMACOに目が付いて、息子と”いないいないバァ”で遊べるように進化しました！<br><br>枯れたVS-RC003プラットフォームの二足ロボも、ROS化すれば命が宿ります(*⁰▿⁰*) <a href="https://t.co/EdMui2YzxN">pic.twitter.com/EdMui2YzxN</a></p>&mdash; みっちー (@Dream_Drive) <a href="https://twitter.com/Dream_Drive/status/1205626987265421312?ref_src=twsrc%5Etfw">December 13, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

ちなみに、[GitHub](https://github.com/dreamdrive/kumaco_control)の方には、上記ノードをすべて起動するkumaco_face.launchも用意しています。

~~~bash:いないいないバァに必要なノード一括起動
$ roslaunch kumaco_face.launch device:=/dev/video0 serialdev:=/dev/ttyUSB0
~~~

カメラデバイスと、シリアルポートを選べるようにしています。

以上で、ROSからVS-RC003を搭載した一般的なホビーロボットをコントロールすることが出来ました。
VS-RC003のユーザーで、もし興味を持った方がいらっしゃいましたら、是非ROSの世界でも二足歩行ロボットを動かしましょう！

# 参考
- [ROSのOpenCV_AppsとUSBカメラを使って始める画像処理](https://qiita.com/nnn112358/items/c8119857a85077de41db)

# 一連の記事へのリンク

- [VS-RC003を搭載した"KUMACO"をROSで動かす(前編)「PCとKUMACOを繋ぐ話」](https://qiita.com/Dream_Drive/items/15b09fc0d6581f9f4192)
- [VS-RC003を搭載した"KUMACO"をROSで動かす(中編)「VS-RC003を制御するROSノード」](https://qiita.com/Dream_Drive/items/4a28119d1275f6e3dcad)
- [VS-RC003を搭載した"KUMACO"をROSで動かす(後編)「KUMACOを画像処理から制御する」](https://qiita.com/Dream_Drive/items/0216af27f0c9394f5513)