OGIMOノート

6歳の娘と、4歳の息子(脳性麻痺)を持った父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具の製作記録、勉強記録を残していきます

【ROS】Nintendo LABO(ニンテンドーラボ) Toy-Con+ESP8266を使ってROS対応二輪ロボットを動かしてみた

完成したもの

出来たものはこちら

youtu.be
f:id:motokiinfinity8:20180524220642j:plain

Nintendo LABOのバイクToy-Conを使って、ロボットを操作できる様にしてみた。
アクセスハンドルをひねるとロボットが前に動き出し、ハンドルを左右に傾けるとロボットも左右に移動する様な使い勝手。
ハンドル加減と曲がり具合はまだ調整中ですが、体験価値としては面白いかと。
娘も楽しそうに遊んでくれてました!(息子にはまだ早いので、息子向けにはToy-Conの見直しから)

ちなみに、ROS上で動かしているので、将来的には自立移動等と組み合わせたりも出来そう♪


作成動機

前回記事
ogimotokin.hatenablog.com
の続きで、「さーて! GW中にencorderからodometryを作成して、いよいよnavigationと結合するか~」と意気込んでいたのですが、
Nintendo LABO発売!
によってすべての計画が崩れてしまいました!
これは面白い!

あっという間に、娘のお手て(マーカー)を追いかけるロボットが出来ました(^-^)
これはただのダンボールで遊ぶおもちゃじゃないですね。子供の成功体験(作った!出来た!動いた!)を積ませて、「上手にできた!楽しい! もっと色々作りたい!」と好奇心を高めさせてくれる素敵な工作教材です(^-^)
ただ簡単に作るだけじゃなく、しっかり原理も学べるという。子供向けに「IRカメラ」とか「振動の早さ(周波数)を変える」とかをかんたんに説明していたりして、これはすごい!
さすが任天堂! 本当に子供心を掴むのがうまいですね!

気が付けばGWも終わり、すっかり計画破綻。
しかし、この体験価値を生かせないだろうか?と考え、「いっそ、娘の作ったToy-CONで実際のロボットも動いたら面白んでないか?」という事で早速トライしてみました。

システム構成(案)

思いついたのは以下の案
f:id:motokiinfinity8:20180524222806p:plain


Arduino対応無線モジュールesp8266(ESP-WROOM-02)マイコンをROS対応して、Nintendo Switch Joy-CONのジャイロセンサ情報をキャプチャーするという手段。Nintendo LABO側はトイコンガレージを使って、2つのジャイロセンサーの角度に応じて赤外線送信命令を送信するプログラムを作成してやればよいので、これからNintendo LABOをROS対応したと言えなくはない。

という訳で、まずはesp8266のROS対応から。
これは Wifi経由でrosserialノードを使う事ができる様だ。下記サイトを参考にしてプログラムを作成してみた。

GitHub - agnunez/espros: ROS serial for ESP8266 over WiFi

serverPortは11411を使用。clientからserverへの接続はesp8266サンプルプログラムと同様。加えて

  ros::NodeHandle nh;
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();

を追加するイメージ。その後はrosserialと同じ使い方でpub/sub定義をすれば良い。(完成コードは後述)

課題

順調に制作を進めていたのだが、本家のNintendo LABOの開発が進まず。一番のブロッカーは娘と一緒に遊ぶ時間があわず通常キット5個が完成していない状況。元々娘と一緒に遊ぶ約束で購入したため、私が勝手にトイコンガレージを触ろうとすると
「わたしがまだやってないのに、先にやらんといてーーーー!!」
という始末。これは困った…

システム構成案(改)

という訳で、娘の決済がおりず、やむなくシステム変更。
f:id:motokiinfinity8:20180524222808p:plain

Conの代わりにジャイロセンサ×2を購入して、Toy-Conの段ボールに張り付ける。1つはハンドル部、1つは胴体部。

SODIAL(R) GY-521 MPU-60503軸ジャイロ+3軸加速度センサ
Amazon CAPTCHA

センサーの使い方はこちらのサイトを参考にした
こじ研(ESP センシング編)
https://www.ei.tohoku.ac.jp/xkozima/lab/espTutorial2.html

2つのジャイロセンサーとはI2Cで並列接続。区別できる様に片側のAD0ピンは電源PU抵抗をつけてHにしておく。
胴体部をAD0ピンをPUしてI2Cアドレス0x69、ハンドル部は未実装にしてI2Cアドレス0x68とする。
x,y,z方向の加速度情報を読み出し(読み出しは2byte×2、リトルエンディアンである事に注意)、分解能で割って加速度(G)に変換する。その後、加速度からセンサー対地角度を求める。あとは、ハンドル部(toy_acc)と胴体部(base_acc)との差分をもとにハンドルのひねり量を算出する。

そう、、、お気づきの通り、ここでもう Nintendo LABOは関係なくなってしまった(笑)

作成コード

①esp8266側 toycon_ros.ino

#include <ESP8266WiFi.h>
#include <ros.h>
#include <time.h>
#include <Wire.h>
#include <geometry_msgs/Twist.h>

#define MPU6050_TOY_ADDR 0x68
#define MPU6050_BASE_ADDR 0x69
#define MPU6050_AX  0x3B
#define MPU6050_AY  0x3D
#define MPU6050_AZ  0x3F
#define MPU6050_GX  0x43
#define MPU6050_GY  0x45
#define MPU6050_GZ  0x47

float base_acc_x, base_acc_y, base_acc_z;
float toy_acc_x,  toy_acc_y,  toy_acc_z;
float base_acc_angle_x, base_acc_angle_y, base_acc_angle_z;
float toy_acc_angle_x, toy_acc_angle_y, toy_acc_angle_z;

const char* ssid     = "********************";
const char* password = "******************";
IPAddress server(192,168,11,3);      // Set the rosserial socket ROSCORE SERVER IP address
const uint16_t serverPort = 11411;    // Set the rosserial socket server port

void setupWiFi() {                    // connect to ROS server as as a client
  if(DEBUG){
    Serial.print("Connecting to ");
    Serial.println(ssid);
    WiFi.begin(ssid, password);
  }
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
  }
  if(DEBUG){
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
  }
}

// ROS nodes //
ros::NodeHandle nh;
geometry_msgs::Twist cmdvel_msg; 
ros::Publisher pub_cmdvel("/toycon/cmd_vel", &cmdvel_msg); 
 
void setup() {
  Serial.begin(115200);
  setupWiFi();
  delay(2000);
// Ros objects constructors   
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();
  nh.advertise(pub_cmdvel);

  //  i2c as a master
  Wire.begin();
  Wire.beginTransmission(MPU6050_BASE_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  Wire.beginTransmission(MPU6050_TOY_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();
}

void loop() {
  if (nh.connected()) {
    //  send MPU6050_BASE_ADDR
    //  send start address
    Wire.beginTransmission(MPU6050_BASE_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_BASE_ADDR, 14);

    //  get 14bytes
    short int AccX, AccY, AccZ;
    AccX = Wire.read() << 8;  AccX |= Wire.read();
    AccY = Wire.read() << 8;  AccY |= Wire.read();
    AccZ = Wire.read() << 8;  AccZ |= Wire.read();

    //  X/Y方向を反転
    AccX = AccX * -1.0;
    AccY = AccY * -1.0;

    // 取得した加速度値を分解能で割って加速度(G)に変換する
    base_acc_x = AccX / 16384.0; //FS_SEL_0 16,384 LSB / g
    base_acc_y = AccY / 16384.0;
    base_acc_z = AccZ / 16384.0;

    // 加速度からセンサ対地角を求める
    base_acc_angle_x = atan2(base_acc_x, base_acc_z) * 360 / 2.0 / PI;
    base_acc_angle_y = atan2(base_acc_y, base_acc_z) * 360 / 2.0 / PI;
    base_acc_angle_z = atan2(base_acc_x, base_acc_y) * 360 / 2.0 / PI;


    //  send MPU6050_TOY_ADDR
    //  send start address
    Wire.beginTransmission(MPU6050_TOY_ADDR);
    Wire.write(MPU6050_AX);
    Wire.endTransmission();  
    Wire.requestFrom(MPU6050_TOY_ADDR, 14);

    //  get 14bytes
    AccX = Wire.read() << 8;  AccX |= Wire.read();
    AccY = Wire.read() << 8;  AccY |= Wire.read();
    AccZ = Wire.read() << 8;  AccZ |= Wire.read();

    // 取得した加速度値を分解能で割って加速度(G)に変換する
    toy_acc_x = AccX / 16384.0; //FS_SEL_0 16,384 LSB / g
    toy_acc_y = AccY / 16384.0;
    toy_acc_z = AccZ / 16384.0;

    // 加速度からセンサ対地角を求める
    toy_acc_angle_x = atan2(toy_acc_x, toy_acc_z) * 360 / 2.0 / PI;
    toy_acc_angle_y = atan2(toy_acc_y, toy_acc_z) * 360 / 2.0 / PI;
    toy_acc_angle_z = atan2(toy_acc_x, toy_acc_y) * 360 / 2.0 / PI;

    // cmd_vel判定① : linear.x
    // TOY y角とBASE y角に対してTOY y角が90°以上になった場合、Baseとの差分角分の前進命令を送る
    float max_linear = 0.5;
    float start_angle = 92;
    float max_angle = 145;
    float diff_angle;
    diff_angle = toy_acc_angle_y - base_acc_angle_y;
    if(diff_angle < 0 )  diff_angle = 360 + diff_angle;
    
    if(diff_angle  < start_angle || base_acc_angle_y > 90 || base_acc_angle_y < -90){
      cmdvel_msg.linear.x = 0.0;
    }else if(diff_angle  > max_angle){
      cmdvel_msg.linear.x = max_linear;
    }else{
      cmdvel_msg.linear.x = max_linear * ( diff_angle - start_angle) / (max_angle - start_angle); 
    }

    // cmd_vel判定② :angular.z
    // BASE x角に応じて、左右方向の命令を送る
    if(cmdvel_msg.linear.x == 0.0){
      cmdvel_msg.angular.z = 0.0;
    }else if(base_acc_angle_x >= 35){
      cmdvel_msg.angular.z =0.5 * cmdvel_msg.linear.x /  max_linear;
      cmdvel_msg.linear.x = 0.0;  
    }else if(base_acc_angle_x <= -35){
      cmdvel_msg.angular.z =-0.5 * cmdvel_msg.linear.x /  max_linear;
      cmdvel_msg.linear.x = 0.0;  
    }else if(abs(base_acc_angle_x) < 35){
      cmdvel_msg.angular.z = base_acc_angle_x / 35.0;
    }

    // cmd_vel publish
    pub_cmdvel.publish( &cmdvel_msg );  
  } else {
    Serial.println("Not Connected");
  }
  nh.spinOnce();
  // Loop aprox. every  
  delay(50);  // milliseconds
}

raspberry pi側 aruduono_namual.launch

<?xml version="1.0"?>
<launch>
    <!-- Start joycon -->
    <node pkg="joy" name="joycon" type="joy_node"/>

    <!-- Start alphabot_manual node -->
    <node pkg="alphabot" name="AlphaBotManual" type="AlphaBotManual"  output="screen"/>

    <!-- Set to your serial port of esp8266 -->
    <arg name="toycon_serial_port" default="tcp"/>
    <arg name="toycon_baud_rate" default="57600"/>
    <node pkg="rosserial_python" type="serial_node.py" name="toycon_node" output="screen">
        <param name="port" type="string" value="$(arg toycon_serial_port)"/>
        <param name="baud" type="int" value="$(arg toycon_baud_rate)"/>
    </node>
</launch>
||<</span>

** 最後に
何気に、ESP8266をrosserialで動かす事が出来たのが一番の成果ではなかろうか。