へっぽこ元ロボコニスト

ロボコンに燃え尽きた自分が日々の出来事を書くだけのブログです

2019/10/7(Raspberry PiのROSからpigpioを使ったSPI通信でMPU9250を動かして,Madgwick Filterから姿勢を計算する)

くそ長タイトル

前回はMPU9250の値を取りました

mozyanari.hatenablog.com

MPU9250の値を取るだけでは,重力加速度しか分からないので姿勢を計算していきます
ただ,修論もあるし手軽にROSのパッケージがあるMadgwick Filterを使います

wiki.ros.org

ドローンなら,ラズパイとMPU9250で姿勢が取得出来て,PWMをだしてアンプを動かして飛ばせるのではと思います

環境

前回と同じです Raspberry Pi 3 B
OS:Ubuntu Mate
ROS:Kinetic
9軸センサ:MPU9250

プログラム

github.com pigpioを使ったSPI通信とMPU9250の値の取得は前回の記事を参照してください

センサデータ変換

double acc_x = mpu9250.get_acc_x()/16384.0*9.81;
double gyr_x = mpu9250.get_gyr_x()/130.072*0.01745;

今回は,加速度と角速度だけを使います
imu/data_rawというデータをimu_filter_nodeに与えます
なので,imu/data_rawを自分で作る必要があります

センサデータをimu_filter_nodeが求めている単位に変換して与えます 何度か実験してみた所,
加速度は,m/s2
角加速度は,rad/s2
が必要ということが分かりました
式の意味は前回の記事を参考にしてください

imu/data_rawの作成

sensor_msgs::Imu Imu;
Imu.header.frame_id ="imu";
Imu.header.stamp = ros::Time::now();
Imu.linear_acceleration.x = acc_x;
Imu.linear_acceleration.y = acc_y;
Imu.linear_acceleration.z = acc_z;

Imu.angular_velocity.x = gyr_x;
Imu.angular_velocity.y = gyr_y;
Imu.angular_velocity.z = gyr_z;

もしかしたら,分散の場所に値を入れればMadgwick Filterが上手いこと何か処理してくれるかもしれませんが,ここでは値を入れてpublishするだけです

Madgwick Filterの追加

github.com

ソースにgit cloneで持ってきてコンパイルします
ただ,ラズパイだとフリーズする可能性があります(一度フリーズしました)
なので,makeするときは

catkin_make -j1 -l1

でしましょう

同時に起動するlaunchファイル

github.com

MPU9250から加速度,角加速度を取得するノード,Madgwick Filterノードを同時に起動するlaunchです

この時,パラメータのuse_magをfalhseにします
これで地磁気を使わずに計算します

また,パラメータのreverse_tfはtrueにします これによって,imuリンクからodomリンクを生成します (これがないと,rvizで表示した時に基準となるtfがないために姿勢が変化しているか分かりません)

実験結果

youtu.be

起動するとなぜかodomリンクからyaw軸に90度回転したところから始まります
yaw軸は地磁気がないと補正は難しいようです ですが,十分使える精度だと思います

まとめ

今日はまとめで一日使ってしまった
明日からは研究しよ

2019/10/7(Raspberry PiのROSからpigpioを使ったSPI通信でMPU9250を動かす)

夏シンポが終わってひと段落

前回まででGPIOでLチカをしてみたり,サーボを動かしてみたり,シリアル通信をしてみました mozyanari.hatenablog.com mozyanari.hatenablog.com mozyanari.hatenablog.com

今回は,Raspberry PiのROSからSPI通信でMPU9250を動かしてみます
正直これはマイコンでやればいいんじゃないと思うかもしれませんが,ラズパイでやりたい理由は以下の二点です

  1. すべてをRaspberry PiのROSで処理させて,ROSパッケージの恩恵を受けたい
  2. ロボットに乗るように出来るだけ小さく作りたい

センサを取得したり,オドメトリとのセンサフュージョンをしようと思うときには結局マイコンからラズパイにデータを送る必要があります
例えば,これをやりたいとき wiki.ros.org

また,STMでセンサデータを受信しようとした時,少ないピンの表面実装のパッケージを使えば小さくできなくもないですが,今考えている移動ロボットはLRFが載ってますし結構キツキツなスペースです

なので,出来るだけROS上でセンサデータを取得したいです

今回の目標はMPU9250の加速度と角加速度をSPIで取得して,重力加速度を観測します

環境

Raspberry Pi 3 B
OS:Ubuntu Mate
ROS:Kinetic
9軸センサ:MPU9250

プログラム

github.com さすがにプログラムが長くなってきたので,適宜抜粋しながら解説します

pigpioの初期化

int SPI_MPU9250::pigpio_initialize(){
  pigpio_check = pigpio_start(NULL,NULL);
  if(pigpio_check < 0){
      ROS_ERROR("pigpio Initialize Error");
      throw;
  }else{
      ROS_INFO("pigpio initialize OK");
      return pigpio_check;
  }
}

ここではpigpioの初期化をしています
以前の記事とは異なるのは,初期化が失敗するとthrowをしてノードを落としていることです
また,クラス内変数にpigpio_checkを持たせておくことで他の場所でのpigpioの初期化の結果を使えるようにしています

pigpioのSPIの使い方

int SPI_MPU9250::spi_initialize(){
  //spi config
  unsigned int spi_channel = 0;          //0ch
  unsigned int baud = 500000;            //500kHz
  
  //spi_flags setting
  unsigned int spi_flags = 0;
  spi_flags = spi_flags | 0b00;            //select mm
  spi_flags = spi_flags | (0b000<<2);   //select p0-p3 
  spi_flags = spi_flags | (0b000<<5);   //select u0-u3
  spi_flags = spi_flags | (0b0<<8);     //select A
  spi_flags = spi_flags | (0b0<<9);     //select W
  spi_flags = spi_flags | (0b0001<<10); //select n
  spi_flags = spi_flags | (0b0<<14);    //select T
  spi_flags = spi_flags | (0b0<<15);    //select R
  spi_flags = spi_flags | (0b1000<<16); //select b
  //ROS_INFO("%d",spi_flags);

  //spi open
  spi_handle = spi_open(pigpio_check,spi_channel,baud,spi_flags);
  if(spi_handle < 0){
      ROS_ERROR("SPI Initialize Error");
      throw;
  }else{
    ROS_INFO("SPI initialize OK");
      return spi_handle;
  } 
}

ここではspiの初期化をします
pigpioのSPIの設定は以下のページにあります abyz.me.uk
第一引数には,pigpioの初期化の結果,第二引数にはどのSPIを使うか,第三引数はSPIの通信速度,第四引数には,SPIのその他の設定を入れます

今回は,第二引数は0にして,MISOがGPIO9,MOSIがGPIO10,SCLKがGPIO11,CEがGPIO8のSPIを使います
第三引数は500Khzにしました

最も設定が多くて面倒だった第四引数の設定について説明します
ここには22bit必要で自分はunsighd intを用意しました

  • 0~1bitはSPIモードを設定します

os.mbed.com 立ち上がりでデータを読むかなどを設定するところですが,基本的に00を書き込んでおけばいいと思います

  • 2~4bitはCEがアクティブLowかアクティブHighかを設定します
    ただ,今回はCEピンが動いていない気がしたので,スレーブセレクトは自分でしました

  • 5~7bitはCEをチップセレクト(スレーブセレクト)として予約するかどうか
    予約していますが,自分で動かしているので一つ前の設定も含めてここは少し謎です

  • 8bitはメインSPIを使うか,auxiliary SPIを使うかです
    auxiliary SPIはメインSPIのピンが何かしらの理由で埋まっていれば使えばいいと思いますが,特殊なSPIな気がします

  • 9bitは何線でつないでいるかです
    SPIはチップセレクト(スレーブセレクト)を無視した3線と,チップセレクト(スレーブセレクト)を含んだ4線で通信します
    今回は4線で設定しています
    この設定はメインSPIでしか行えませんので,auxiliary SPIでは ただ,ここも4線で設定していますが結局チップセレクト(スレーブセレクト)は自分で動かしているので設定として間違っているかもしれません

  • 10~13bitはいまいち理解できていないので,調べた物を引用させていただきます

    nnnnの4ビットはMOSIからMISOに切り替わるバイト数を指定する。当然最大15バイトということになる。これはAが1のとき、つまり3本線の場合だけ使える、と書いてある。ということはMOSIとMISOを多重化するということなんだろうか。

decafish.blog.ss-blog.jp

  • 14bitは送信時のMSBかLSBを設定します 0だとMSBから1だとLSBで通信します ただ,この設定はauxiliary SPIだけです

  • 15bitは受信時のMSBかLSBを設定します 0だとMSBから1だとLSBで通信します ただ,先ほどと同様この設定はauxiliary SPIだけです

  • 16~22bitは何ビットで通信するか設定する場所です MPU9250は1byte = 8bitずつ読むので0b1000=8で十分です
    ただ,最高26 = 64bit通信出来るので.マイコンと通信するときにはうれしいかもしれません
    (例えば,オドメトリを受け取るときとか)

ここまでつらつら書いてきましたが,この人のブログが一番参考になると思います decafish.blog.ss-blog.jp

spiのread関数

char SPI_MPU9250::pigpio_spi_read(char address){
  char read_data;
  char send_address = address | set_bit_1; 
  gpio_write(pigpio_check,CS_pin,0);
  spi_write(pigpio_check,spi_handle,&send_address,1);
  spi_read(pigpio_check,spi_handle,&read_data,1);
  gpio_write(pigpio_check,CS_pin,1);
  return read_data;
}

引数では読みだしたいアドレスを取得します
MPU9250でSPI通信を使ったリードを行うときにはアドレスの先頭bitを1にします(ここ大事)
そして,チップセレククトピンを下げて(アクティブLow),読みたいアドレスを書き込み,読み出します
最後にチップセレクトピンを上げて終了します
その結果をread_dataとして返します
ここでは特にエラー処理はしていないので,失敗したとしても分からないのでその点は注意してください (やろうと思えばできますが,加速度や角加速度を取得するたびチェックするのはどうかと思ってやめました)

spiのwrite関数

char SPI_MPU9250::pigpio_spi_write(char address, char write_data){
  char send_address = address;
  gpio_write(pigpio_check,CS_pin,0);

  char result1 = spi_write(pigpio_check,spi_handle,&address,1);
  char result2 = spi_write(pigpio_check,spi_handle,&write_data,1);
  gpio_write(pigpio_check,CS_pin,1);
  if(result1 < 1 | result2 < 1){
      ROS_ERROR("write error result = %d %d",(int)result1,(int)result2);
      throw;
  }else{
    ROS_INFO("MPU9250 OK");
    return 1;
  }
}

引数で書きたいアドレスとデータを取得します
readとは異なりアドレスには何も手を付けません 最初に,チップセレククトピンを下げてアドレスを送信して,その後データを送信します
最後にチップセレククトピンを上げます
書き込み関数の結果は書き込んだバイト数が返ってきます
つまり1byteの書き込みに成功すれば1が返ってくるはずなので,1以下が出れば失敗と判定してノードを落とします

MPU9250のチェック

SPI通信でMPU9250が存在するかチェックします

void SPI_MPU9250::who_am_i_check(){
  char read_data = pigpio_spi_read(who_am_i);
  if(read_data == 0x71){
      ROS_INFO("who am i OK");
  }else{
      ROS_ERROR("who am i Error read data = %d",(int)read_data);
    throw;
  }
}

ここでは先ほどのpigpio_spi_read関数を使ってMPU9250のwho am iが返ってくるかをチェックします 見つからなけばノードを落とします

MPU9250の起動

void SPI_MPU9250::mpu9250_power_on(){
  pigpio_spi_write(wake_up_1,wake_up_ward_1);
  pigpio_spi_write(wake_up_2,wake_up_ward_2);
}

MPU9250 の内部アドレス0x6Bに0x00 を書き込み,内部アドレス0x37に0x02 を書き込みます
ここら辺はMPU9250のマニュアル通りです

加速度,角加速度の取得

int16_t SPI_MPU9250::get_acc_x(){
  int acc_x_h = pigpio_spi_read(ACCEL_XOUT_H);
  int acc_x_l = pigpio_spi_read(ACCEL_XOUT_L);
  int16_t acc_x = ((acc_x_h << 8) | acc_x_l);
  return acc_x;
}

この関数では加速度,角加速度を取得します
リードしたデータはintで取得して,必ずint16t_tに格納します もし,intに入れると4byte = 32bitなので,シフト演算でまとめた時に負の値がおかしくなります

加速度の変換

double acc_x = mpu9250.get_acc_x()/16384.0*9.81;

mpu9250から取得した加速度はそのままだと使えません
なのでこれをm/s2に変換します mpu9250のデフォルトのレンジは,+-2gです
また,分解能は16bit,65536段階,つまり,-32768~32768です
(いや,0を考えると-32767~32768かな?)
例えば,16384という値が来れば 16384/16384 = 1g * 9.8 = 9.8m/s2となります

つまり,出てきた値を16384で割って,9.8をかければ加速度に変換できます
(今回はデフォルトのレンジが+-2gなのでこの計算で出来ます
もし,+-4gなら,1gの値が32768/4=8192なので,8192で割って,9.8をかけたら加速度になります)

角加速度の変換

double gyr_z = mpu9250.get_gyr_z()/130.072

これも同様に変換します
デフォルトが+-250dps = degree/secなので,32768/250 = 131.072の値で割ります
こうすることで,角加速度が出てきます

(ただ,これの単位は度なので,後でラジアンが必要になりました
その場合以下のようにしました

double gyr_z = mpu9250.get_gyr_z()/130.072*0.01745

rad = deg * (pi/180)なので,pi/180 = 0.01745を掛けました)

Raspberry PiとMPU9250の配線方法

f:id:masanari7430:20191007150636j:plain 図のICの名前は違いますが,この方法で結線しました

実験結果

youtu.be accを見ると9.8が出ていて,重力加速度が認識出来ています

まとめ

解説を書くのが長かった
最近,研究の方でも解説を書いているけど,これを書かないと1年後の自分が絶対覚えていないので仕方がないと思いながら書いています
でも,やっていて思ったのがこんなことしていてもいいのかなということです
最近,IPhoneのカメラだけでARを表示してさらにオクルージョンしてるのをみたり,Oculus Questがハンドトラッキングをカメラだけでしているのを見ると,この後の人生で自分が価値のあるものを作れるのかと思います
来年から働きますがこれでいいのかな とりあえずご飯食べてきます

2019/8/15(Raspberry PiのROSからpigpioを使ってGroovy-PIDを動かしてモータを動かす)

タイトルが長い

前回はGroovy-PIDのLEDをROSのpigpio経由で動かしました
mozyanari.hatenablog.com
mozyanari.hatenablog.com

次にRaspberry PiのROSからpigpioを使ってGroovy-PIDを動かしてモータを動かします
前回と同様の環境を使っているので,もしやっていないなら動作確認も含めてRaspberry PiのROSからpigpioを使ってGroovy-PIDを動かしてLチカしてみてください

環境

Raspberry Pi 3 B
OS:Ubuntu Mate
ROS:Kinetic

プログラム


今回のプログラムも前回と同様にgitで配布しているので,git cloneで取ってください
github.com

解説

#include <pigpiod_if2.h>
pi = pigpio_start(NULL,NULL);

説明するまでもなく初期化です
詳しい説明は以前の記事にあります
mozyanari.hatenablog.com



char m = 0x6d;
char s = 0x73;
char space = 0x20;
char zero = 0x30;
char one = 0x31;
char two = 0x32;
char three = 0x33;
char five = 0x35;  
char enter = 0x0D;

モータを動かすコマンドは以下のページにあります
www.omiya-giken.com
今回使用するのは
「m」 「 」(space) 「0」「」(改行)
:オープンループモード
「s」 「 」(space) 「0」「 」(space) 「0」「」(改行)
:モータを止める
「s」 「 」(space) 「0」「 」(space) 「5」「0」「0」「」(改行)
:500の力でモータを回す
この3つが主です
ただ,これをasciiコードで送るのでそれに対応した値を変数として保持しています



int serial_flag = serial_open(pi,"/dev/ttyACM0",57600,0);

ここでUSB接続したシリアルポートを開いています

第一引数

pigpioを初期化した時に返ってきた値を使用しています

第二引数

/dev/ttyACM0を指定していますが,他に接続している物があったり,OSのバージョンによっては違うものが出てくるかもしれませんがその時はここを適宜変更してください

第三引数

Groovy-PIDを動かすのに必要なボーレートは57600なのでここはこの値で固定です


serial_write(pi,serial_flag,&m,1);
serial_write(pi,serial_flag,&space,1);
serial_write(pi,serial_flag,&zero,1);
serial_write(pi,serial_flag,&enter,1);

この部分でシリアル通信でオープンループ設定をしています

第一引数

pigpioを初期化した時に返ってきた値を使用しています

第二引数

シリアル通信を初期化した時の値を使用しています
第一,第二と初期化の時に返ってきた値を使うことで,もしどちらかの初期化が失敗したら動かないようになっています

第三引数

送信したいデータのアドレスを渡しています


//if no [time_sleep], this node don't work
time_sleep(0.1);

オープンループ設定をした後にtime_sleep(0.1)がありますが,これを入れないと自分は動きませんでした
このGroovy PIDは送ったコマンドを適宜コールバックして返してくれています
また,エンター入力した後はTeratermで表示を綺麗にするための改行などの3コマンドを送っています
なので,オープンループ設定をした後にすぐに速度指令を送ると,PICはシングルスレッドなので改行コマンドを送信している時に速度指令を受け取ることになり動かないのではと考えています


serial_write(pi,serial_flag,&s,1);
serial_write(pi,serial_flag,&space,1);
serial_write(pi,serial_flag,&zero,1);
serial_write(pi,serial_flag,&space,1);
serial_write(pi,serial_flag,&five,1);
serial_write(pi,serial_flag,&zero,1);
serial_write(pi,serial_flag,&zero,1);
serial_write(pi,serial_flag,&enter,1);

この部分で速度指令をしています
この例では
「s」 「 」(space) 「0」「 」(space) 「5」「0」「0」「」(改行)
:500の力でモータを回す
を実行しています

実行

sudo pigpiod

まずは絶対にこれを実行します

roscore

ROSではこれが必ず必要です


f:id:masanari7430:20190727165240j:plain
Groovy-PIDのリセットスイッチを押す

rosrun groovypid Groovy_moter_test

これでモータを動かすことができます

そういえばこの記事でいきなりシリアル通信してるからそれに関して少し書きたいな

2019/8/15(Raspberry PiのROSからpigpioを使ってGroovy-PIDを動かしてLチカしてみる)

お盆は実家でSLAMの勉強

前回の記事です
mozyanari.hatenablog.com

前回はTeratermからGroovy-PIDを動かしました
このままだとロボットでは使えないので,Raspberry PiのROSからpigpioを使ってGroovy-PID上のLEDを動かしてみます

つなぎ方や動作確認,raspberry piにROSを入れることはすでに終わっている前提とします
ただ前回はGroovy IoTからPCにつなぎましたが,今回はGroovy IoTからraspberry piをUSB接続します

環境

Raspberry Pi 3 B
OS:Ubuntu Mate
ROS:Kinetic

プログラム


今回のプログラムはgitで配布しているので,git cloneで取ってください
github.com

解説

#include <pigpiod_if2.h>
pi = pigpio_start(NULL,NULL);

説明するまでもなくpigpioの初期化です
詳しい説明は以前の記事にあります
mozyanari.hatenablog.com


char l = 0x6c;
char space = 0x20;
char zero = 0x30;
char one = 0x31;
char enter = 0x0D;

Groovy PIDはTeratermで送信するasciiコードで動いています
つまり,前回やったTeratermでLチカさせたコマンドは
「l」 「 」(space) 「0」「」(改行)= 「0x6c」「0x20」「0x30」「0x0D」
を送ればいいことになります
ここではそのコマンドを定義しています
また,シリアル通信は8bitで送受信を行うのでcharにしといたほうが安全です


int serial_flag = serial_open(pi,"/dev/ttyACM0",57600,0);

ここでUSB接続したシリアルポートを開いています

第一引数

pigpioを初期化した時に返ってきた値を使用しています

第二引数

/dev/ttyACM0を指定していますが,他に接続している物があったり,OSのバージョンによっては違うものが出てくるかもしれませんがその時はここを適宜変更してください

第三引数

Groovy-PIDを動かすのに必要なボーレートは57600なのでここはこの値で固定です


serial_write(pi,serial_flag,&l,1);
serial_write(pi,serial_flag,&space,1);
serial_write(pi,serial_flag,&zero,1);
serial_write(pi,serial_flag,&enter,1);

この部分でシリアル通信でデータを送信しています

第一引数

pigpioを初期化した時に返ってきた値を使用しています

第二引数

シリアル通信を初期化した時の値を使用しています
第一,第二と初期化の時に返ってきた値を使うことで,もしどちらかの初期化が失敗したら動かないようになっています

第三引数

送信したいデータのアドレスを渡しています


実行

sudo pigpiod

まずは絶対にこれを実行します

roscore

ROSではこれが必ず必要です


f:id:masanari7430:20190727165240j:plain
Groovy-PIDのリセットスイッチを押す

rosrun groovypid Groovy_LED_test

これでLEDがチカチカします

2019/7/29(TeratermでGroovy-PIDとGroovy IoTを動かしてみる)

ブログのアクセス数見てるとちゃんとみんな休日休んでることが分かる

大宮技研から「Groovy-PID」「Groovy IoT」を入手したので動かしてみました

大宮技研さんのHP
www.omiya-giken.com

Groovy IoT

f:id:masanari7430:20190727161719j:plain
www.switch-science.com


今回はTeratermからボード上のLEDを動かしてみました

つなぎ方

つなぎ方は以下のページを参考にしました
www.omiya-giken.com

  • Groovy-PIDとGroovy IoT間

f:id:masanari7430:20190727162623j:plain
4線のケーブルで接続します
ただ,ここで注意しないといけないのは,Groovy IoTには画像の左からI2C,UART,GPIO1,GPIO2の接続端子があり,UARTを使用します
(自分はここで間違ってGPIO2につないでいて小一時間動かなくて苦労しました)

  • Groovy IoTとPC間

f:id:masanari7430:20190727163938j:plain
microUSBでPCと接続します

Teratermとの接続してLEDを光らせる

動かし方は以下のページを参考にしました
www.omiya-giken.com

USB接続するとGroovy PIDの上のLEDが光ります
f:id:masanari7430:20190727170301j:plain
Teratermを開いて接続されたCOMボードを選択します
f:id:masanari7430:20190727165736p:plain
ボーレートは57600,データは8bit,パリティなし,ストップbitは1にします
f:id:masanari7430:20190727165745p:plain
このあと,Groovy PIDのリセットスイッチのJ4を押します
f:id:masanari7430:20190727165240j:plain
すると,コマンドを打てる画面が出てきます
f:id:masanari7430:20190727170043p:plain
「l」 「 」(space) 「0」を入力してエンターで送信します
f:id:masanari7430:20190727170057p:plain
すると最初は光っていたLEDが消えます
f:id:masanari7430:20190727170254j:plain

これでとりあえず動くことが確認できたので終了です

2019/7/25(Rapberry Pi 3 + Ununtu MateでGPIOのUART(シリアル)を有効にする)

今回の記事は本当に合っているか分からないので,間違っていたら指摘お願いします

ROSでpigpioを使ってGPIOのUARTを動かそうとしたときにデフォルトの設定だとGPIOがbluetoothに使用されているらしく色々いじる必要がありました

正直何をして動くようになったのか分からないのですが,自分がやったことをつらつらと並べていきます

1

ubuntu-mate.community

このページが検索結果に一番マッチしていたので,やってみました
ですがこれの最後にrebootした後,sudo pigpioをするのを忘れていてpigpioからシリアルを開こうとしても当たり前ですが開きませんでした
これで動いた人がいたら教えてほしいです

追記
qiita.com
シリアルポートの権限付与の部分でこれだけで動いているので1のやり方だけで動く可能性があります

2

www.ingenious.jp
このページによると
miniUARTがGPIO
UART0がBluetooth
につながっているらしく,Bluetoothを停止してUART0をGPIOにつなぐ,もしくはminiUARTとUART0を入れ替える必要があります
実際に/devの中でls- lを実行するとシリアルの部分で
serial0→/dev/ttyS0
serial1→/dev/ttyAMA0
となっていると思います
これを入れ替えていきます

このページの手順に沿ってやってみました
Linuxのシリアルコンソールを無効化
この人との環境が少し違うので内容が少し違いました
なので,/boot/cmdline.txtを以下のように変更しました
修正前
f:id:masanari7430:20190725124621p:plain
修正後
f:id:masanari7430:20190725124649p:plain

②シリアル通信にUART0を使用するように変更
bluetoothは生かしたままにしたかったので
bのBluetoothとの通信にminiUARTを使用する方法を使用しました
なので,/boot/config.txtに
dtoverlay=pi3-miniuart-bt
core_freq=250
を追加しました
修正後
f:id:masanari7430:20190725125048p:plain

こうすると,miniUARTとUART0が入れ替わります
これを確認するには
/devの中でls -lを実行すると確認ができます

f:id:masanari7430:20190725125531p:plain

miniUART =/dev/ttyS0
UART0=/dev/ttyAMA0
らしく,画像の真ん中に表示されているように
serial0→/dev/ttyAMA0
serial1→/dev/ttyS0
となればOKです

正直どれがUARTを使えるようになったか分からないので,やるときは自己責任でお願いします

2019/7/25(raspberry piのROS上でpigpioを使ってGPIOからサーボを動かしてみる)

最近のお気に入りのyoutuberは夕闇に誘いし漆黒の天使達
www.youtube.com


前回の記事です
mozyanari.hatenablog.com

Lチカしてから1か月,NorthStarで遊び過ぎてこっちを忘れてました
NorthStarでやったことは,可能ならRosCon JPのLTで発表したいと思っているのでここでは書きませんが面白いものが出来たと思っています
(でもRosConのLT,企業の人がガチでやってるから学生が出来るか心配)

前回はROSにpigpioを導入してLチカをしました
次はPWMを出してサーボを動かしたいと思います

Lチカが出来てpigpioが正常に動いている前提で進めます

ですがサーボはたった一行で終わります

set_servo_pulsewidth

これを使うだけです

また,pigpioを使ったプログラムはgitにすべて挙げていますので好きなようにクローンして使ってください
github.com

プログラム


servo.cpp


(前回書いた記事とは,プログラム作法が全く違いますが,前回のようにクラスまで使うまでもないと感じたのでこのように書いています)

解説

#include <pigpiod_if2.h>
pi = pigpio_start(NULL,NULL);

前回もありましたがこの二行は必須です
ヘッダーと初期化を行っています

set_servo_pulsewidth(pi,26,1500);

abyz.me.uk
今回新しく出てきたのはここだけ
マイコンでサーボのPWMよりも簡単

第一引数

初期化の返り値piを使っているので初期化を失敗すると動きません
見ているとどの関数にもこれが必須な気がします

第二引数

ピン番号を選びます
26なのでGPIO26(ラズパイのピン番号では37)を制御します
(pigpioのいいところとしてGPIOピンのどのピンからでもPWMを出せます
これは本当に便利だし,小さいロボットならマイコンがいらない)

第三引数

パルスの幅を選択します
最小500~最大2500を入れてください
0を入れるとPWMがストップします
リファレンスによると1500程度がサーボ中心の角度らしいです

あとはROSを理解していれば分かると思います
(分からなかったらコメントで質問ください)

CMakeLists.txtの編集

add_executable(servo src/drvo.cpp)
target_link_libraries(servo ${catkin_LIBRARIES})

これはいつも通り必要な部分で

target_link_libraries(servo pigpiod_if2)

これを絶対に忘れないように!!

ROSの実行

ラズパイのGPIO26にサーボをつないで,デーモンをコマンドプロンプト上で実行します

sudo pigpiod

そしてrosrunで上のプログラムを実行するとサーボが1秒間隔でシャキシャキ動くと思います
arduinoよりも安定しているのか,感覚ですがサーボ特有の「ジー」っていう音が小さいように感じました

検証

え?
ラズパイが本当に安定したPWM出せてるの?
本当に50Hzの周波数?
だってラズパイってマイコンみたいにリアルタイム制御向いてないんじゃない?

自分もそう思ったのでロジアナで簡単ですが確認しました

第三引数を1500にしたとき
set_servo_pulsewidth(pi,26,1500);
f:id:masanari7430:20190725113550p:plain
第三引数を1000にしたとき
set_servo_pulsewidth(pi,26,1000);
f:id:masanari7430:20190725113650p:plain


安定していました
他の制御が入るとどうなるかわかりませんが,単体で使用する分には十分な波形だと思います

使っていて思うこと

pigpioとROSが連携しているので今までロボットを動かすのに必要だと考えていたマイコンを減らせるのではと考えています
今まで考えていたのは
ROSが入ったPC➔マイコン➔センサ・モータドライバ
だったのが
ROSが入ったラズパイ➔センサ・モータドライバ
とダイレクトに出来るのではと思います

しかもpigpioはラズパイのすべてのピンがPWM化できるのでそこらのマイコンよりは数が多いです
ただ,どれだけ同時にPWMをちゃんと出力できるか,割り込みもちゃんと使用できるか,ROSの他のノードが重い処理をするとどのような影響がピンに出てくるか,等いろいろ検討することがまだあると思います

2019/6/27(raspberry piのROS上でpigpioを使ってLチカしてみる)

早く社会人になって,自分の金でデバイスを買いたい

学部4年の頃から研究でROSを使っていて常に困っていることがありました
それはROSからLEDやモータなどを動かせないということです

金を積んでDynamixelを買えば高性能なサーボを使えますし,実験ではturtlebotを代機として使えばいいとは思います
また,rosserial_arduinoを使えばPCからarduinoをノードとして操作できます
www.besttechnology.co.jp
www.turtlebot.com


ただ,学生ロボコンをしていた自分としては中華サーボのトルクが小さい物を動かして遊んでみたいですし,エンコーダの生値を読んでモータにPID制御をかけて発振させて遊んでみたいのです
また,arduinoしか動かせないのはスペック的につらいです

そこで目標としては,ROSが乗ったデバイスからLEDやモータを動かします
(ただROS2が出てきて組み込みマイコンにそもそもROS2が乗るのですぐオワコンになりそう)
今回はその最初としてRaspberry Pi上のROSからIOピンを使ってLチカをします
そのためには様々な方法が現在あります

  • WiringPi

tool-lab.com

  • RPi.GPIO

qiita.com
ですが,自分は出来るだけPWMもあって,SPIやI2Cなどの操作もしたい
さらに,ROSから動かしたいというのがあったのでpigpioを使いました

pigpioのインストール

インストールは以下のページを参考にしました
karaage.hatenadiary.jp

sudo apt-get update
sudo apt-get install pigpio

ROS上でcppプログラムを書く

書き方は以下のページを参考にしました
botalab.tech
またリファレンスはここです
https://abyz.me.uk/rpi/pigpio/pdif2.htmlabyz.me.ukabyz.me.uk



まずは適当にパッケージを作って,cppファイルを書きます
gpioという名前にしました

#include <ros/ros.h>
#include <pigpiod_if2.h>

#include <std_msgs/Bool.h>

class gpio{
  //construct
  public:
  gpio();
  
  private:
  int pi;
  
  void cb_LED(const std_msgs::Bool::ConstPtr &data);
  ros::NodeHandle nh;
  ros::Subscriber sub_led;
};

gpio::gpio(){
  pi = pigpio_start(NULL,NULL);

  sub_led = nh.subscribe("/led", 5, &gpio::cb_LED, this);
}

void gpio::cb_LED(const std_msgs::Bool::ConstPtr &data){
  if(data->data == 1){
    gpio_write(pi,26,1);
  }else{
    gpio_write(pi,26,0);
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "gpio");
  gpio gpio;
  ros::spin();
  return 0;
}
解説
#include <pigpiod_if2.h>

ヘッダーファイルがこれです
ネットで検索しているとpythonで書かれている物ではpigpio.hをインクルードしていたりしていてるのですが何が違うかいまいち分かりません

pi = pigpio_start(NULL,NULL);

ここで初期化します
初期化に成功すると0以上の整数値が返ってきます
ローカルで動かすのでNULLを引数として入れていますが,リファレンスを見るとIPアドレスとポートを指定できるのでもしかしたら別のPCにこのノードを立てて動かせるかも?

gpio_write(pi,26,1);

第一引数
piを入れている場所に負の値が入ると動きません
ここに初期化の時に帰ってきた値を使うことで初期化失敗しているときには動きません

第二引数
ここでピン番号を選びます
今回は26なのでGPIO26(ラズパイのピン番号では37)を制御します

第三引数
ここではGPIOのHighかLowかを選びます
1だとHighで電圧をかけます

あとはROSを理解していれば分かると思います
(分からなかったらコメントで質問ください)

CMakeLists.txtの編集

大体,ノードを作るとCMakeに

add_executable(gpio src/gpio.cpp)
target_link_libraries(gpio ${catkin_LIBRARIES})

を書くと思います
ただ今回は

target_link_libraries(gpio pigpiod_if2)

をさらに追加する必要があります
(これが一番大事)
これを書いておかないとコンパイルが通りません

ROSの実行

ラズパイのGPIO26にLEDと抵抗をつないで,デーモンをコマンドプロンプト上で実行します

sudo pigpiod

そして,gpioノードを実行して,/ledにtrueを送るとLEDが光ります

これでROS上でラズパイのGPIOを制御することが出来ました

追記2019/8/25

この内容に関して瀬戸内ROS勉強会で「ROS搭載ラズパイでLチカしてみる」で発表しました
ros.xrea.jp

2019/6/14(ハードウェアの干渉でNorthStarのキャリブレーションが上手くいかない)

夜の本気ダンスをメトロックで見てからはまった
www.youtube.com


以前の記事です
mozyanari.hatenablog.com

NorthStarをキャリブレーションをしようとしたのですが,どれだけ値を変更しても右と左がずれていました
まあ見えているしいいかなと思っていたのですが,NorthStarを人に見せる機会があるので,このままだと体験してくれる人の感動が半減しないかと思いました

そこで,どこかハード的にいけない場所があるかなと思ってCADを見ていました
myhub.autodesk360.com

そしたら,LCDを固定する部分と本体が干渉していました
f:id:masanari7430:20190614154045p:plain
おそらくこれが原因でLCDの角度が変わっていたのでしょう

作り直します

追記
削って干渉をなくしました

2019/6/4(estimatePoseSingleMarkersのtvecとrvecからマーカ位置を計算する)

就活終わったー
後は研究して卒業するだけや

前回の記事です
mozyanari.hatenablog.com
カメラ位置を計算することは出来ていましたが,自分が欲しいのはマーカの位置です
カメラ位置を計算できるということは,マーカ位置も取得できるはずなので検索しましたが全く記事が出てこなかったため色んな記事を参照しました
stackoverflow.com
www.learnopencv.com
www.learnopencv.com

最終的には回転行列からマーカの回転角度を取得するということで動きました

  • プログラム
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using OpenCVForUnity;
using UnityEngine.SceneManagement;

public class Opencv_test : MonoBehaviour {

    //カメラの番号
    private int camera_number = 0;

    private VideoCapture cam;
    public Mat cam_frame;

    private Texture2D cam_Texture;

    //Arucoの設定
    public ArUcoDictionary dictionaryId = ArUcoDictionary.DICT_4X4_50;
    private Dictionary ar_dict;

    //検出アルゴリズムの設定
    private DetectorParameters detect_param;

    //マーカの位置
    private Mat rvecs;
    private Mat tvecs;
    //送信するマーカ位置
    private Vector3 position;

    public GameObject camera;
    public GameObject marker;



    // Use this for initialization
    void Start () {
        cam = new VideoCapture(camera_number);
        //cam.Set(CaptureProperty.ConvertRgb, 3);
        cam.set(16, 3);
        cam_frame = new Mat();

        ar_dict = Aruco.getPredefinedDictionary((int)dictionaryId);
        detect_param = DetectorParameters.create();

        //plopIdはopenCVの2系時に名前で定義されていたが廃止された.3系以降は番号でこれを取得する
        cam_Texture = new Texture2D((int)cam.get(3), (int)cam.get(4), TextureFormat.RGB24, false);

        this.GetComponent<Renderer>().material.mainTexture = cam_Texture;

        tvecs = new Mat();
        rvecs = new Mat();
        position = new Vector3(0, 0, 0);
    }

    // Update is called once per frame
    void Update () {
        List<Mat> marker_corner = new List<Mat>(OpenCVForUnity.CvType.CV_8UC1);
        Mat marker_ids = new Mat();
        List<Mat> reject_points = new List<Mat>(OpenCVForUnity.CvType.CV_8UC1);

        cam.read(cam_frame);

        Aruco.detectMarkers(cam_frame, ar_dict, marker_corner, marker_ids, detect_param, reject_points);

        //Debug.Log(marker_ids.size());
        if (marker_ids.total() > 0)
        {
            //マーカの描画
            Aruco.drawDetectedMarkers(cam_frame, marker_corner, marker_ids, new Scalar(0, 255, 0));

            //キャリブレーションデータの設定
            Mat camMatrix = new Mat(3, 3, CvType.CV_64FC1);
            camMatrix.put(0, 0, 548.82998077504431);
            camMatrix.put(0, 1, 0);
            camMatrix.put(0, 2, 332.15854329853039);
            camMatrix.put(1, 0, 0);
            camMatrix.put(1, 1, 549.82809802204554);
            camMatrix.put(1, 2, 236.24689239842121);
            camMatrix.put(2, 0, 0);
            camMatrix.put(2, 1, 0);
            camMatrix.put(2, 2, 1.0f);

            MatOfDouble distCoeffs = new MatOfDouble(-0.088668381409835462, 0.4802348866900254, -0.0026481695882058297, 0.0019686217165377452, -0.82389338772252729);



            Aruco.estimatePoseSingleMarkers(marker_corner, (float)0.125, camMatrix, distCoeffs, rvecs, tvecs);

            Aruco.drawAxis(cam_frame, camMatrix, distCoeffs, rvecs, tvecs, (float)0.1);


            Debug.Log("x=" + tvecs.get(0, 0)[0] + "y=" + tvecs.get(0, 0)[1] + "z=" + tvecs.get(0, 0)[2]);
            //Debug.Log("y=" + tvecs.get(0, 0)[1]);
            //Debug.Log("z=" + tvecs.get(0, 0)[2]);


            Debug.Log("roll=" + (rvecs.get(0, 0)[0] * 180 / Mathf.PI) + "pitch=" + rvecs.get(0, 0)[1] * 180 / Mathf.PI + "yaw=" + rvecs.get(0, 0)[2] * 180 / Mathf.PI);


            marker.transform.position = new Vector3(0, 0, 0);

            Mat Rt = new Mat();

            OpenCVForUnity.Calib3d.Rodrigues(rvecs, Rt);


            Mat T = new Mat(3, 1, CvType.CV_64FC1);
            T.put(0, 0, tvecs.get(0, 0)[0]);
            T.put(1, 0, tvecs.get(0, 0)[1]);
            T.put(2, 0, tvecs.get(0, 0)[2]);


            //Debug.Log(R);

            Mat R = Rt.t();


            Mat camera_position = Rt.t() * (-T);

            Debug.Log(camera_position);

            //回転角度の計算
            float roll = 0;
            float pitch = 0;
            float yaw = 0;

            float a = Mathf.Sqrt((float)(R.get(0, 0)[0] * R.get(0, 0)[0] + R.get(1, 0)[0] * R.get(1, 0)[0]));
            if (a > 1e-6)
            {
                roll = Mathf.Atan2((float)R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI;
                pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI;
                yaw = Mathf.Atan2((float)R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI;
            }
            else
            {
                roll = Mathf.Atan2(-(float)R.get(1, 2)[0], (float)R.get(1, 1)[0]) * 180 / Mathf.PI;
                pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI;
                yaw = 0;
            }
            
            

            camera.transform.position = new Vector3((float)tvecs.get(0, 0)[0] * 10, -(float)tvecs.get(0, 0)[1] * 10, (float)tvecs.get(0, 0)[2] * 10);


            camera.transform.rotation = Quaternion.Euler(roll, pitch, yaw);


        }

        Utils.matToTexture2D(cam_frame, cam_Texture);
    }

    public enum ArUcoDictionary
    {
        DICT_4X4_50 = Aruco.DICT_4X4_50,
        DICT_4X4_100 = Aruco.DICT_4X4_100,
        DICT_4X4_250 = Aruco.DICT_4X4_250,
        DICT_4X4_1000 = Aruco.DICT_4X4_1000,
        DICT_5X5_50 = Aruco.DICT_5X5_50,
        DICT_5X5_100 = Aruco.DICT_5X5_100,
        DICT_5X5_250 = Aruco.DICT_5X5_250,
        DICT_5X5_1000 = Aruco.DICT_5X5_1000,
        DICT_6X6_50 = Aruco.DICT_6X6_50,
        DICT_6X6_100 = Aruco.DICT_6X6_100,
        DICT_6X6_250 = Aruco.DICT_6X6_250,
        DICT_6X6_1000 = Aruco.DICT_6X6_1000,
        DICT_7X7_50 = Aruco.DICT_7X7_50,
        DICT_7X7_100 = Aruco.DICT_7X7_100,
        DICT_7X7_250 = Aruco.DICT_7X7_250,
        DICT_7X7_1000 = Aruco.DICT_7X7_1000,
        DICT_ARUCO_ORIGINAL = Aruco.DICT_ARUCO_ORIGINAL,
    }


    void OnDestroy()
    {
        if (cam_frame != null)
            cam_frame.Dispose();
    }


}

試行錯誤の跡が残ってるのでこのままコピペしても動かないので適宜参照してください

  • マーカ位置の計算

tvecとrvecを取得するまでは前回の記事と同じ流れです

camera.transform.position = new Vector3((float)tvecs.get(0, 0)[0] * 10, -(float)tvecs.get(0, 0)[1] * 10, (float)tvecs.get(0, 0)[2] * 10);

10倍してるのはデバックするときに動きが見やすいためにしています
単位はmです
前回と異なり,tvecsをそのまま使っています

  • マーカの角度計算
float roll = 0;
float pitch = 0;
float yaw = 0;

float a = Mathf.Sqrt((float)(R.get(0, 0)[0] * R.get(0, 0)[0] + R.get(1, 0)[0] * R.get(1, 0)[0]));
if (a > 1e-6)
{
    roll = Mathf.Atan2((float)R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI;
    pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI;
    yaw = Mathf.Atan2((float)R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI;
}else{
    roll = Mathf.Atan2(-(float)R.get(1, 2)[0], (float)R.get(1, 1)[0]) * 180 / Mathf.PI;
    pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI;
    yaw = 0;
}

この部分が最も時間がかかりました
rvecsをロドリゲス変換して転置した後の行列の中身を参考にしたページをもとに計算するとなんかうまいこと動くようになりました
なので,なぜこれが上手く動くか理解していません

動けば勝ちなのでこれでいいかなと思っています

2019/6/4(estimatePoseSingleMarkersのtvecとrvecからカメラ姿勢を計算する)

ココロオークションが最近の自分の流行
www.youtube.com


Arucoを使ってマーカの位置を取得しようとしています
Unityでマーカ位置にオブジェクトを置きたいのでちゃんとした位置結果を計算しないといけません
ネットで検索しても,detectMarkers→drawDetectedMarkersで描画するだけや,detectMarkers→tvecの値から直接位置計算をやっていて正解にたどり着けませんでした

OpencvにestimatePoseSingleMarkersと同様の結果を出すsolvePnPという関数があり,これで検索すれば出てくるかなと思って検索して見つけました
stackoverflow.com
これによって自分が出来たのはマーカを原点としてカメラの位置・姿勢がどこにあるかということです.

  • 環境

Unity 2018.2.21f1
OpenCV for Unity v2.3.2

  • プログラム
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using OpenCVForUnity;
using UnityEngine.SceneManagement;

public class Opencv_test : MonoBehaviour {

    //カメラの番号
    private int camera_number = 0;

    private VideoCapture cam;
    public Mat cam_frame;

    private Texture2D cam_Texture;

    //Arucoの設定
    public ArUcoDictionary dictionaryId = ArUcoDictionary.DICT_4X4_50;
    private Dictionary ar_dict;

    //検出アルゴリズムの設定
    private DetectorParameters detect_param;

    //マーカの位置
    private Mat rvecs;
    private Mat tvecs;
    //送信するマーカ位置
    private Vector3 position;

    public GameObject camera;
    public GameObject marker;



    // Use this for initialization
    void Start () {
        cam = new VideoCapture(camera_number);
        //cam.Set(CaptureProperty.ConvertRgb, 3);
        cam.set(16, 3);
        cam_frame = new Mat();

        ar_dict = Aruco.getPredefinedDictionary((int)dictionaryId);
        detect_param = DetectorParameters.create();

        //plopIdはopenCVの2系時に名前で定義されていたが廃止された.3系以降は番号でこれを取得する
        cam_Texture = new Texture2D((int)cam.get(3), (int)cam.get(4), TextureFormat.RGB24, false);

        this.GetComponent<Renderer>().material.mainTexture = cam_Texture;

        tvecs = new Mat();
        rvecs = new Mat();
        position = new Vector3(0, 0, 0);
    }

    // Update is called once per frame
    void Update () {
        List<Mat> marker_corner = new List<Mat>(OpenCVForUnity.CvType.CV_8UC1);
        Mat marker_ids = new Mat();
        List<Mat> reject_points = new List<Mat>(OpenCVForUnity.CvType.CV_8UC1);

        cam.read(cam_frame);

        Aruco.detectMarkers(cam_frame, ar_dict, marker_corner, marker_ids, detect_param, reject_points);

        //Debug.Log(marker_ids.size());
        if (marker_ids.total() > 0)
        {
            //マーカの描画
            Aruco.drawDetectedMarkers(cam_frame, marker_corner, marker_ids, new Scalar(0, 255, 0));

            //キャリブレーションデータの設定
            Mat camMatrix = new Mat(3, 3, CvType.CV_64FC1);
            camMatrix.put(0, 0, 548.82998077504431);
            camMatrix.put(0, 1, 0);
            camMatrix.put(0, 2, 332.15854329853039);
            camMatrix.put(1, 0, 0);
            camMatrix.put(1, 1, 549.82809802204554);
            camMatrix.put(1, 2, 236.24689239842121);
            camMatrix.put(2, 0, 0);
            camMatrix.put(2, 1, 0);
            camMatrix.put(2, 2, 1.0f);

            MatOfDouble distCoeffs = new MatOfDouble(-0.088668381409835462, 0.4802348866900254, -0.0026481695882058297, 0.0019686217165377452, -0.82389338772252729);



            Aruco.estimatePoseSingleMarkers(marker_corner, (float)0.125, camMatrix, distCoeffs, rvecs, tvecs);


            
            Debug.Log("x=" + tvecs.get(0, 0)[0]+ "y=" + tvecs.get(0, 0)[1] + "z=" + tvecs.get(0, 0)[2]);
            
            
            Debug.Log("roll=" + (rvecs.get(0, 0)[0] * 180 / Mathf.PI) + "pitch=" + rvecs.get(0, 0)[1] * 180 / Mathf.PI + "yaw=" + rvecs.get(0, 0)[2] * 180 / Mathf.PI);

            

            marker.transform.position = new Vector3(0, 0, 0);
            Mat Rt = new Mat();

            OpenCVForUnity.Calib3d.Rodrigues(rvecs,Rt);
            

            Mat T = new Mat(3, 1, CvType.CV_64FC1);
            T.put(0, 0, tvecs.get(0, 0)[0]);
            T.put(1, 0, tvecs.get(0, 0)[1]);
            T.put(2, 0, tvecs.get(0, 0)[2]);

            Mat R = Rt.t();

            Mat camera_position = Rt.t() * (-T);

            Debug.Log(camera_position);

            camera.transform.position = new Vector3((float)camera_position.get(0,0)[0]*10, (float)camera_position.get(2, 0)[0]*10, (float)camera_position.get(1, 0)[0]*10);

            float roll = Mathf.Atan2((float)-R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI;
            float pitch = Mathf.Asin((float)R.get(2, 0)[0]) * 180 / Mathf.PI;
            float yaw = Mathf.Atan2((float)-R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI;

            camera.transform.rotation = Quaternion.Euler(roll, yaw, pitch);


        }

        Utils.matToTexture2D(cam_frame, cam_Texture);
    }

    public enum ArUcoDictionary
    {
        DICT_4X4_50 = Aruco.DICT_4X4_50,
        DICT_4X4_100 = Aruco.DICT_4X4_100,
        DICT_4X4_250 = Aruco.DICT_4X4_250,
        DICT_4X4_1000 = Aruco.DICT_4X4_1000,
        DICT_5X5_50 = Aruco.DICT_5X5_50,
        DICT_5X5_100 = Aruco.DICT_5X5_100,
        DICT_5X5_250 = Aruco.DICT_5X5_250,
        DICT_5X5_1000 = Aruco.DICT_5X5_1000,
        DICT_6X6_50 = Aruco.DICT_6X6_50,
        DICT_6X6_100 = Aruco.DICT_6X6_100,
        DICT_6X6_250 = Aruco.DICT_6X6_250,
        DICT_6X6_1000 = Aruco.DICT_6X6_1000,
        DICT_7X7_50 = Aruco.DICT_7X7_50,
        DICT_7X7_100 = Aruco.DICT_7X7_100,
        DICT_7X7_250 = Aruco.DICT_7X7_250,
        DICT_7X7_1000 = Aruco.DICT_7X7_1000,
        DICT_ARUCO_ORIGINAL = Aruco.DICT_ARUCO_ORIGINAL,
    }

    void OnDestroy()
    {
        if (cam_frame != null)
            cam_frame.Dispose();
    }


}
  • ARマーカの認識
Aruco.detectMarkers(cam_frame, ar_dict, marker_corner, marker_ids, detect_param, reject_points);

cam_frameがカメラ画像でこれを入力することで,マーカの位置のmarker_cornerを取得します

  • ARマーカの位置の取得
Aruco.estimatePoseSingleMarkers(marker_corner, (float)0.125, camMatrix, distCoeffs, rvecs, tvecs);

マーカ位置のmarker_cornerとカメラパラメータのcamMatrixとdistCoeffsを入力します
カメラパラメータは事前に取得しておきます
mozyanari.hatenablog.com
(float)0.125はARマーカの実際の大きさをはかって入力しています
こうすることで,ARマーカとの相対的な位置関係を表す,tvecとrvecを取得出来ます

  • rvecとtvecからカメラ位置を計算

冒頭の参考文献をそのまま使っています
ロドリゲス変換

OpenCVForUnity.Calib3d.Rodrigues(rvecs,Rt);

カメラ位置の計算

Mat camera_position = Rt.t() * (-T);

カメラの回転角度計算

float roll = Mathf.Atan2((float)-R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI;
float pitch = Mathf.Asin((float)R.get(2, 0)[0]) * 180 / Mathf.PI;
float yaw = Mathf.Atan2((float)-R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI;

Unityに適した形で入力

camera.transform.rotation = Quaternion.Euler(roll, yaw, pitch);


これによってマーカの位置を原点として,カメラの位置・姿勢を取得できると思います

追記
マーカ位置を計算するVerも書きました
mozyanari.hatenablog.com

2019/5/3(ArucoUnityでCharuco boardを作ってカメラのキャリブレーションデータを取得する)

明日はMaker Fair Kyoto

前回の記事です
mozyanari.hatenablog.com

環境が作れたので,キャリブレーションボード(Charuco board)を作りキャリブレーションをします

normanderwan.github.io

Assets/Scenesの中のCreateMakers.unityを開きます
自分が好きなキャリブレーション用のボードを作ることができますが,手間なのでデフォルトのまま作ります
なので,そのままUnityの実行ボタンを押します
f:id:masanari7430:20190503173345p:plain


すると,Assets/Imagesフォルダが生成されて,キャリブレーション用のCharuco boardが作れます
f:id:masanari7430:20190503173251p:plain
使うのは
ArUcoUnity_ChArUcoBoard_Dict4x4_50_X_7_Y_5_SquareSize_200_MarkerSize_120.png
です

これを印刷して厚紙に貼り付けます
f:id:masanari7430:20190503174612j:plain


normanderwan.github.io


Assets/Scenesの中のCalibrateCamera.unityを開きます
f:id:masanari7430:20190503174802p:plain

まず指定するのは使うカメラです
Aruco WebcamWebcam Idを設定します
自分はノートPC内蔵カメラが0番,以降接続したUSBカメラの順番に1番...と続きます
f:id:masanari7430:20190503174908p:plain

Mono Aruco Camera Displayのチェックボックスを消します
キャリブレーション実行時にこれが起動しないようにするためらしいです
f:id:masanari7430:20190503175233p:plain


次は,ArucoCharucoBoardのArucoCharucoBoardを設定します
f:id:masanari7430:20190503175903p:plain
変更すべき場所は,Maker Side LengthとSquare Side Lengthです
Maker Side Lengthはマーカの大きさを,Square Side LengthはQRコードの一つのマスの大きさをm単位で指定します
なので,自分が印刷したCharuco boardを定規で測って入力してください
f:id:masanari7430:20190503181503j:plain
(こうすることで,印刷機が勝手にCharuco boardを拡大したり縮小したりして印刷しても大丈夫になっています)


最後に,PinholeCameraCalibrationのAruco Camera Parameters Controllerを設定します
Camera Parameterに,「自分が好きな名前.xml」を入力してください
キャリブレーションデータの名前を決める部分です
リファレンスには空欄でも勝手に名前を作るよと書いてありますが,空欄だとエラーで書き出してくれませんでした
f:id:masanari7430:20190503181936p:plain

設定が終わったらUnityの実行ボタンを押すとキャリブレーションが始まります
カメラにボードを映して「add Image」を押すとキャリブレーション用のデータを取ることができます
様々な角度で撮ってください(自撮りをあんまりしないのでリア充ならうまいことやれるんだろうなと思いながらやっていました)
これを10回程度繰り返して「Calibrate」を押すとキャリブレーションが実行されます
結果は,Assets/StreamingAssetsの中にxml形式で出力されます
「Reset」ボタンを押すと最初から始めることができます
f:id:masanari7430:20190503182739p:plain
(OpenCVではQRコードなしのキャリブレーション用ボードを用意しますが,今回の方法はQRコードがついています.これによってキャリブレーションボード全体が写っていなくてもキャリブレーションを行うことが出来るようです.ただ,QRコード全体が写っているほうが精度はもちろんいいです)

Assets/StreamingAssetsの中のxmlデータを確認します
estimatePoseSingleMarkersで必要なものは,Camera Matrices Value=cameraMatrix,DistCoeffsValues=distCoeffsです
読み方はおそらく今回のデータだと
cameraMatrix=
|578・・, 0  ,319・・|
| 0  ,581..,259・・|
| 0 , 0 , 1 |

distCoeffs=[0.0772,-0.2668,-0.0023,0.0024,-0.1549]

だと思います
f:id:masanari7430:20190503184548p:plain

これでカメラのキャリブレーションデータを取得することが出来ました
次はこのデータを使って実際にQRコードを認識して距離を取得します

2019/5/3(UnityでArucoUnityを実行できる環境を作る)

久しぶりに他人が作った英語のリファレンスを何時間も読んだ

前回の記事です
mozyanari.hatenablog.com

ArUcoのライブラリの中に,estimatePoseSingleMarkersという関数があります
マーカーの姿勢と位置を計測できる関数で,ロボットにマーカを置いてロボットまでの相対位置を取得しようと考えています
しかし,この関数を実行するには,①InputArray cameraMatrix ②InputArray distCoeffsが必要です
docs.opencv.org

①は3×3のカメラ行列で,②は今回は1×5行列のレンズ歪みパラメータです
②はリファレンスによると1×4や1×12のものもあるらしいです

つまり,事前にこれらのパラメータを取得しないといけません

ですので,Unity上でこれらのパラメータを取得します

これらのプログラムを一から書くのはとても面倒なので今回は以下のプログラムを使いました
normanderwan.github.io

このプログラムはかなり優秀で,キャリブレーション用のボードを自作できたり,魚眼カメラ,ステレオカメラのキャリブレーションもできます
ただ,今回は普通のwebカメラキャリブレーションを最短で行うことを目標にします

必要なもの

  • Unity 2018.2.21f1 (64-bit)
  • ArucoUnity(Commits on Jan 12, 2019のものを使いました)

github.com

  • ArucoUnityPlugin(ArucoUnityPlugin-v2.0.1-Windows.zipを使いました)

github.com

UnityでArucoUnityを実行できる環境を作る
原文
github.com

UnityでArucoUnityを開きます
ただ,昔のバージョンで作られたので
f:id:masanari7430:20190503170717p:plain
このような警告が出ますが,2018のバージョンで開いても大丈夫なのでこのままオープンします
開くとArucoUnityPluginが無いと警告されます

次に,警告されたArucoUnityPluginをUnityに入れます
f:id:masanari7430:20190503171030p:plain
ArucoUnityPluginの中のPluginsのフォルダを,UnityのAssets/ArucoUnityの中にドラッグ&ドロップで入れます

最後にこれを読み込ますためにUnityを再起動します
すると,警告がすべてなくなっていると思います

次は,キャリブレーション用マーカの作成です

2019/5/2(OpenCVでWebカメラの画像をモノクロにする)

GW中に高校の友達とご飯を食べに行ったときに,意外な進路をたどっていたので驚いた


前回の記事です
mozyanari.hatenablog.com

前回は自分の顔を判定する機能を実装しましたがOpenCVが動くのを確認しただけなので,次はWebカメラの画像をモノクロにしてUnity上で
表示します

今回参考にしたのは以下のページです

ssssota.hatenablog.com


using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using OpenCvSharp;

public class web_camera : MonoBehaviour {
    [SerializeField] private int m_WebCamIndex = 1;
    private Texture2D texture2D;
    private VideoCapture videoCapture;
    private Mat mat;
    private Mat grey;

	// Use this for initialization
	void Start () {
        videoCapture = new VideoCapture(m_WebCamIndex);
        texture2D = new Texture2D(videoCapture.FrameWidth, videoCapture.FrameHeight, TextureFormat.RGB24, false);
        GetComponent<Renderer>().material.mainTexture = texture2D;
        mat = new Mat();
        grey = new Mat();
	}
	
	// Update is called once per frame
	void Update () {
        videoCapture.Read(mat);
        Cv2.CvtColor(mat, grey, ColorConversionCodes.BGR2GRAY);
        texture2D.LoadImage(grey.ImEncode());
	}

    private void OnDestroy()
    {
        videoCapture.Dispose();
        videoCapture = null;
    }
}

startの部分

videoCapture = new VideoCapture(m_WebCamIndex);

どのWebカメラを起動するかをここで選択して,カメラに関する情報を変数videoCaptureで取得します
自分のノートPCは内臓カメラがあったので,ここの引数は1だと内蔵カメラ,2だとUSB接続したカメラになります

texture2D = new Texture2D(videoCapture.FrameWidth, videoCapture.FrameHeight, TextureFormat.RGB24, false);

カメラ画像に関する情報をtexture2Dで保持します
1つ目の引数で画像の横幅,2つ目で画像の縦幅を入力します
ここの引数をカメラの性能が分かっているなら決め打ちでもいいかもしれませんが,基本的にvideoCaptureから取得したほうが便利です
3つ目と4つ目はいまいち理解していません

GetComponent<Renderer>().material.mainTexture = texture2D;

これを実行することでUnity上でアタッチしているオブジェクトにtexture2Dを張り付けることを示すことができます
今回はPlaneのオブジェクトにアタッチする予定です

mat = new Mat();
grey = new Mat();

参考にしたページでは書いていませんでしたが,自分の場合これを書いていないとmatが常にnullで動きませんでした


upadateの部分

videoCapture.Read(mat);

videoCaptureにはwebカメラのデータが常に更新されて保存されているので,mat形式で書きだします

Cv2.CvtColor(mat, grey, ColorConversionCodes.BGR2GRAY);

この部分がOpenCVの機能を使った部分で,変数matに入っているカメラ画像をモノクロ化(グレースケール化)して変数greyに格納しています

texture2D.LoadImage(grey.ImEncode());

texture2Dはstartの部分でUnityのオブジェクトにアタッチされていることを示されているので,texture2Dに変数greyを入力するだけで勝手に更新されてUnity上で表示してくれます
ただ,greyはmat形式なのでImEncodeを使ってpngに変換してから入力します


OnDestroyの部分
この部分は参考にしたページには書いていましたが,自分の場合Unity上で実行した後この部分でエラーが発生したのでなくてもいいのかもしれません


これをUnityのPlaneにアタッチすれば,Plane上にwebカメラの画像が出てくるはずです

2019/4/29(UnityのOpenCVが動くかどうかを調べる)

大学生活で初めてGWを実家でダラダラ過ごしてる気がする

 

 前回の記事です

mozyanari.hatenablog.com

 

UnityでOpenCVが動く環境ができたのでとりあえず何か動かして,作った環境が正解か試します

以下のページのスクリプトをそのままコピーして動かしてみます

ssssota.hatenablog.com

 

  1. 名前がTestのスクリプト作ります
  2. 参考にしたページのプログラムをコピペして貼り付けます
  3. てきとうなオブジェクトにアタッチして,実行すればプロジェクト内のフォルダに画像を生成する

 

しかし,ここで問題になることがありました

var haarcascade = new CascadeClassifier("Assets/haarcascades/haarcascade_frontalface_default.xml");

ここで,分類器?を参照しているのですが,自分が入れた環境ではそんなもの存在しておらず自分で入れる必要がありました

 

OpenCVの公式のsourceファイルにありました

opencv.org

ここでは,自分のOpenCVSharpのバージョンが4.0.0なので同じバージョンのソースをダウンロードしました

 

分類器の場所は

opencv-4.0.0\opencv-4.0.0\data\haarcascades\haarcascade_frontalface_default.xml

にありました

 

これをUnityの好きな場所にドラッグ&ドロップで導入します

自分はScrptsフォルダの中に入れたので

var haarcascade = new CascadeClassifier("Assets/Scripts/haarcascade_frontalface_default.xml");

に変更しました

 

この状態で実行すると,カメラが起動して結果の写真がプロジェクト内フォルダに出てきました

 

とりあえずこれで自分が作った環境がちゃんと動くものということが分かったのでこれで今後勉強していきます