概要
VS-RC003というVstone社のロボット用制御ボードを搭載したロボットをROSと接続して利用するお話を投稿させていただきます。
課題として、"KUMACO(クマコ)"という名前のぬいぐるみ型のホビーロボットを用いて、"いないいないバァ(顔が見えたら喜ぶ)"させたいと思います。
完成形はこんな感じです。
ぬいぐるみロボット、KUMACOに目が付いて、息子と”いないいないバァ”で遊べるように進化しました！
— みっちー (@Dream_Drive) December 13, 2019
枯れたVS-RC003プラットフォームの二足ロボも、ROS化すれば命が宿ります(⁰▿⁰) pic.twitter.com/EdMui2YzxN
前回は、VS-RC003をROSから仮想コントローラーのコマンドで、制御することが出来ました。
今回は、ROSを使って、カメラ画像からKUMACOの行動を制御したいと思います。ついに、"いないいないバァ"が完成です。
環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i7-9750H |
| Ubuntu | 18.04 |
| ROS | Melodic |
仕様
まず、"いないいないバァ"を実行するにあたって、大雑把に仕様を決めます。
- 顔が見えたら、「やったぁ！」のモーションを実行する
- 顔が見えないときは、アイドリングモーションを実行する
という設計で行きたいと思います。
画像処理の準備
顔を判別するためには、カメラが必要ですので、まずは画像処理に必要なパッケージ(カメラ画像のパッケージとOpenCVのパッケージ)をインストールします。
$ sudo apt-get install ros-melodic-opencv-apps
$ sudo apt-get install ros-melodic-usb-cam
画像処理(顔認識)のノードの起動
先程インストールしたopencv_appsの中には、今回まさに使えそうな顔検出(face_detectio)ノードが含まれますので、横着してそのまま使ってしまいましょう。
$ roscore
$ rosrun uvc_camera uvc_camera_node
$ roslaunch opencv_apps face_detection.launch image:=/image_raw
立ち上げると、顔検出のプレビュー画面が表示されると同時に、検出した顔と目の座標が配信されます。
ちなみに、画像上に２つ以上の顔があった場合は、それぞれの顔の座標を返します。
ただし、顔の判別まではできないので、今回はとりあえず「顔が１つ以上あるかどうか？」というところに着眼したいと思います。
画像処理に関して、他人の褌だけで完了してしまったような感じですが(^^;A
これもある意味ROSの醍醐味かなと思います。
あとはface_detectionノードの結果を用いて、前回紹介したros2vsrc/vsrc_connectノードに司令を送るノードを作れば良さそうですね！
顔の有無によって、ボタンの状態を変化させるようなノードを作りましょう、あと一息！
顔検出状況ノードから、コントローラのボタンの状態を変化させるノードを作る
いきなりですが、結論から書くと、下記のようなノードになります。
#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に公開していますので、ご参考まで。
さくっとインストールする場合は、下記の通りターミナルで入力していただければ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ノードと、前回使えるようになった、ros2vsrc/vsrc_connectも起動しまいましょう。
$ rosrun ros2vsrc vsrc_connect _serialdev:=/dev/ttyUSB0
$ rosrun kumaco_control face2happy
こちらを実行すると、冒頭のTwitterの動画の通り、KUMACOが動作します！
ノードのつながりは下記の通りとなります。一直線ですね。
"いないいないバァ"する、ちょっと賢くなったKUMACOの完成です！
ぬいぐるみロボット、KUMACOに目が付いて、息子と”いないいないバァ”で遊べるように進化しました！
— みっちー (@Dream_Drive) December 13, 2019
枯れたVS-RC003プラットフォームの二足ロボも、ROS化すれば命が宿ります(⁰▿⁰) pic.twitter.com/EdMui2YzxN
ちなみに、GitHubの方には、上記ノードをすべて起動するkumaco_face.launchも用意しています。
$ roslaunch kumaco_face.launch device:=/dev/video0 serialdev:=/dev/ttyUSB0
カメラデバイスと、シリアルポートを選べるようにしています。
以上で、ROSからVS-RC003を搭載した一般的なホビーロボットをコントロールすることが出来ました。
VS-RC003のユーザーで、もし興味を持った方がいらっしゃいましたら、是非ROSの世界でも二足歩行ロボットを動かしましょう！