Help us understand the problem. What is going on with this article?

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

概要

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

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

前回は、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)ノードが含まれますので、横着してそのまま使ってしまいましょう。

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

立ち上げると、顔検出のプレビュー画面が表示されると同時に、検出した顔と目の座標が配信されます。

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

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

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

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

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

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に公開していますので、ご参考まで。
さくっとインストールする場合は、下記の通りターミナルで入力していただければ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も起動しまいましょう。

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

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

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

ちなみに、GitHubの方には、上記ノードをすべて起動するkumaco_face.launchも用意しています。

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

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

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

参考

一連の記事へのリンク

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした