2011/08/22(月)rosserial_arduinoをmbedへ移植

2011/08/23 02:40 ROSnucho
こないだまでちょっとRTMを触ってたんですが、世界的にはROSの方が流行ってそうということで浮気してます。ちなみに勉強する上でこちらが参考になりました。
そしてふらふら探してみるとRTnoのROS版みたいなものでrosserialというROSノード?があるんですね。

例によって反射的にmbedに移植してしまいましたので下に置いておきます。
まぁ自分以外にこれを求めている人がいるか甚だ疑問があるところですが……。

プログラム

rosserial_mbed
rosserial_arduinoに付いてきてたexamplesごと移植したコードです。まず試したいときはこちらをインポートしてください。
ただし全てのexampleが完全に動作かどうかはチェックできてないのでご注意を。。。
なお、初期状態ではHelloWorldのexampleが実行されるようになってます。

rosserial_mbed_lib
examplesだけ取り除いたものです。
ライブラリとして使いたいときはこちらをご利用ください。

使い方

基本的にはrosserial_arduinoのチュートリアルを参考にすればオッケーだと思います。
ここではとりあえず、一番基本的なところのHelloWorldの使い方だけ載っけておきます。

HelloWorld

コンソール上で
roscore
とroscoreを実行して
rosrun rosserial_python serial_node.py /dev/ttyACM0
とmbedの繋がっているポートを指定してserial_node.pyを実行します。
うまく繋がっていれば
rostopic echo chatter
とすると、mbedからstringのデータが送られてきます。

その他は本家のチュートリアルとソースを照らし合わせながらやってみてください。
これを利用して自作ロボットをROSの枠組みで動かすとか楽しそうですよね。

2011/08/04(木)ミニ・リノセロス組み立てたった

2011/08/04 05:13 工作nucho
IMG_20110803_212910
大人の科学Vol.30に続いて別冊大人の科学にはテオ・ヤンセンのミニ・リノセロスが付録で付いてきます.
こういった続き物の企画にまんまと乗せられて買うのは僕のライフワークのようなものですね.一も二もなく購入しました.

組み立て

昨日みた夢の話なみにどうでもいいであろう途中経過の写真たちです.

IMG_20110803_214112
部品点数は142個...バリ取りが大変そうだな,と思いました.

IMG_20110803_234328 IMG_20110804_004813 IMG_20110804_010457
部品点数はミニビーストのときよりも増えているものの,作り方がシンプルになったのか私にとっては今回の方がむしろ作りやすかったです.机は相変わらず汚いです.


IMG_20110804_033341
そして,完成です.
ミニビーストが風車だったのに対し,ミニリノセロスはシロッコファンが頭についています.
シロッコファンは追い風でも向かい風でも回転する方向は同じであり,ひいてはこのミニリノセロスも歩く向きは常に同じです.

リノセロスはサイ(Rhinoceros)ということらしいですが,ミニチュアだとクマムシに近いかなという印象を持ちますね.そんなことないですね.

2011/08/01(月)Javaでシリアルポートの詳細な名前取得

2011/08/01 19:29 Program::Javanucho
Java上でシリアルポートに接続された任意のデバイスのポート名を取得して,自動で接続するということがしたかった時のメモ.
システムの色んな情報を受け取れるらしいWMIをJavaにラップしているjWMIをeclipseで使う,という話です.

使用手順

  1. パッケージエクスプローラ上の任意のJavaプロジェクトを右クリック
  2. 「新規>パッケージ」を選択
  3. 名前に「com.citumpe.ctpTools」と入力
  4. ここからダウンロードしてきたフォルダの中の「jWMI.java」をさっき作ったパッケージの中に放り込む
  5. 何か341行目が化けてるので「OSRecoveryConfiguracion」みたいな感じで直す
あとはプログラムの頭で
import com.citumpe.ctpTools.jWMI;
とインポートし,
try {
	// COMポートの詳細な名前を教えてもらう
	String name = jWMI.getWMIValue(
			"Select Caption from Win32_SerialPort", "Caption");
	System.out.println(name);
	} catch (Exception e) {
	// TODO 自動生成された catch ブロック
	e.printStackTrace();
}
という風に使ったら,
通信ポート (COM1)
Silicon Labs CP210x USB to UART Bridge (COM33)
URG Series USB Device Driver (COM31)
てな感じで返ってきた…んですけど,モノによって出てこないものがあります.
確認しているものだとmbed(mbed Serial Port)とft232シリアル変換IC(USB Serial Port)は取得できなかったです.何故だ….

2011/8/26追記

name = jWMI.getWMIValue("Select Caption from Win32_PnPEntity", "Caption");
としたら,今まででてこなかったものも表示されました.
必要ない情報も多くなってしまいますけれど.

参考

シリアルポートの名前取得

2011/07/29(金)RTnoをmbedに移植した

2011/07/29 22:20 RTMimportnucho
現在は作者さまの方でmbedに対応していらっしゃるので,そちらを使用した方が良いと思われます.

RTnoはArduino上でRTコンポーネントを開発するためのライブラリです.
手軽なIOとしてRTコンポーネントを作ることができるのはとても良いのですが,如何せんArduinoだと気が利いた玩具くらいが限界だと感じるのでmbedに移植してみました.

移植とはいっても綺麗にクラス分けされていたので,作業としては通信周りとあとはちょこちょこくらいでさくっと終了です.

プログラム

元々のRTnoと同じくRTnoProxyを利用します.
ただ私の場合はインストーラ形式で提供されているRTnoProxyが動かなかったので,ソースからコンパイルしたものを使用しました.
勘違いで古いバージョンを使ってただけでした(・ω<)-☆

雛形

RTno_Template
雛形です.
これを色々と書き換えてRTコンポーネントにすることができるのです.

IO制御のサンプル

RTno_digitalInOut
ポート仕様
名称I/Oデータタイプ意味
ledINTimedLongSeqmbed上のLED制御
in0OUTTimedLongSeqp5~p10のポート入力を知る
out0OUTTimedLongSeqp15~p20のポート出力制御
使い方
大体想像通りの動きだと思うので,ここではledポートだけを使ってみます.
rtm-naming-serviceとRT System Builderはあらかじめ起動しているものとします.

rtc.conf内を
corba.nameservers:localhost
exec_cxt.periodic.rate:500.0
Test.RTnoProxy.config_file: RTnoProxy.conf
logger.enable:NO
RTnoProxy.conf内を
conf.default.comport:\\\\.\\COM34
conf.default.baudrate:115200
とします,COM番号など環境に依る部分は適宜書き換えてください.

そうしたら,RTnoProxyCompとDataPortTestCompを起動します.

digitalInOut1
RT System Builder上でRTnoProxyCompのledとDataPortTestCompのlongSeqOutを繋いで,どちらも状態をActiveにします.

digitalInOut2
次に画像のようにDataPortTestComp上で
L
4
1
0
1
0
と入力すると

IMG_20110729_204612
mbed上のLEDが光るという寸法です.
同じような感じでin0ポートでmbed上での入力が知れたり,out0ポートでmbed上のデジタル出力を制御できたりします.

モータ制御のサンプル

RTno_MotorControl
今度はエンコーダ付きモータであるRDO-29B54AふたつをSabertooth2X25で制御してみたサンプルです.
ポート仕様
名称I/Oデータタイプ意味
encorderOUTTimedLongSeqモータの現在のエンコーダ値を知る
positionINTimedLongSeqモータの入力値の位置まで動かす
回路図
RTno_MotorControl_schematic
読みにくくてすいません.
sabertooth2x25のDIPスイッチは2,3,5番をONにした状態です.
使い方
rtm-naming-serviceとRT System Builderはあらかじめ起動しているものとします.

rtc.conf内を
corba.nameservers:localhost
exec_cxt.periodic.rate:500.0
Test.RTnoProxy.config_file: RTnoProxy.conf
logger.enable:NO
RTnoProxy.conf内を
conf.default.comport:\\\\.\\COM34
conf.default.baudrate:115200
とします,COM番号など環境に依る部分は適宜書き換えてください.


RTnoProxyCompとDataPortTestCompを起動します.
MotorControl1
RT System Builder上でRTnoProxyCompのencoderとDataPortTestCompのlongSeqIn,RTnoProxyCompのpositionとDataPortTestCompのlongSeqOutを繋いで,どちらも状態をActiveにします.

MotorControl2
次に画像のようにDataPortTestComp上で
L
2
1000
-2000
と打ち込むとモータが動き出し,しばらくして目的の位置に達したら停止します.

MotorControl3
DataPortTestComp上でvと打ち込むと現在のエンコーダ値を見ることができて,大体指定の位置に動いたんだなということがわかります.
どうでしょう.これくらいのことができれば結構本格的なRTコンポーネントを使った機器まで作れそうな気がしますよね.

既知の問題点

  • ポートは配列しか扱えない
    • 手元のJapaninoでオリジナルのRTnoを使用してもそうなので,RTnoの元々の仕様なのかも……?違うようです.もう一回確認してみます
    • 解決してアップーデートしました.(8/1)
  • シリアル通信をバッファリングしてない
    • MODSERIALなどのバッファ付きの通信を使いたかったけど,うまいこと動かなかった.うーん.
    • 通信待ちでタイムアウトしない以外は問題なさそう…?
  • Arduino側の仕様に併せているためTimedDouble型を内部でfloat型として扱っている.精度が半分になって勿体無いなぁ.
    • RTnoProxyに手を入れるしか….

2011/07/09(土)QPToolkitでマーカの位置姿勢計測したメモ

2011/07/10 00:36 Programnucho
qptoolkit3d
QPToolkitとは最近すっかり社会的認知度が高まっているARを行うためのライブラリから位置計測部分を抜き出した位置計測フレームワークです.
このフレームワークを利用することで,ごく普通のWebカメラから紙に印刷したマーカまでの距離姿勢を簡単に計測することができるのです.

カメラからマーカまでの3次元上の距離を計測するところまでは値を見るだけで簡単にできたのですが,姿勢を計測するのに結構手間取ってしまいました.
今回はそのことについて,もしかしたら超常識的なことなのかもしれないメモ書きです.

姿勢計測を行う

マーカの姿勢を取得するには回転行列+並進ベクトルで表されているOpenGLの行列というものを扱えばいいようです.
併進ベクトルはカメラからマーカまでの距離として,回転成分はなんぞや,と思って調べてみるとARマーカの回転成分はx-y-zオイラー角回転行列で示されているという文章を見つけました.

これは三次元の回転行列をx,y,z軸の順で回転させたもので,この形式の回転行列であるということがわかれば一応角度を取り出すことができます.
ここから先の計算に関してはこちらの下の方にそのものずばりが書いてありました.手で計算した後に見つけたって言う….

ここまでわかればプログラムに落とすことができますね.
processingで書いたものを以下に示してみます.プロトタイピングってことでとっても縦長です.すいません.

プログラム

QPServer上で「Name」「OpenGL model view matrix~」の二つにチェックを入れた状態で動かしてください.
画面上に表示された3Dがカメラ上のマーカにあわせて傾いたり近づいたりするはずです.16cm四方のマーカならそのままで大丈夫ですが,それ以外のサイズだとマーカのサイズに合わせて表示位置や立体のサイズ変えないといけないかもしれません.
あとマーカを見つけたときだけ表示というようにしているせいか凄いちらつきます.画面から離れて部屋を明るくしてこのプログラムを利用してください.
import processing.net.*; 
Client myClient;
double phi,theta,psi;
double X,Y,Z;
double[][] matrix = new double[4][4];

void setup() {
  myClient = new Client(this,"192.168.145.1",55555); // IPとポート番号
  size(480, 480, P3D);
  frameRate(30);
}

void draw() {
  background(0);

  if (myClient.available() > 0) {
    String dataIn = myClient.readStringUntil('\n');

    if ( dataIn != null ) {      
      // "Num="を含むデータ列かどうかをチェックし,データ数を取得
      if ( dataIn.indexOf("Num=")==0 ) {       
        int num = int(split(trim(dataIn),' ' )[1]);
        // num個のマーカのデータを取得する
        for (int i=0; i<num; i++) {  
          String marker_info = myClient.readStringUntil('\n');      
          if ( marker_info!= null ) {
            // 1行分のデータを切り分けて欲しいデータを取得する
            String[] data = split(trim(marker_info),',');

            // マーカの名前と2次元座標
            String name = data[0];
            for(int m=0;m<4;m++)
            {
              for(int n=0;n<4;n++)
              {
                matrix[n][m] = Double.parseDouble(data[1+m*4+n]);
              }
            }
            //傾斜角度を求める
            if (matrix[0][2] > 0.998)
            {
              phi = Math.atan2(matrix[1][0],matrix[1][1]);
              theta = Math.PI/2.0;
              psi = 0;
            }
            else if (matrix[0][2] < -0.998)
            {
              phi = Math.atan2(matrix[1][0],matrix[1][1]);
              theta = -Math.PI/2.0;
              psi = 0;
            }
            else
            {
              phi = Math.atan2(-matrix[1][2],matrix[2][2]);
              theta = Math.asin(matrix[0][2]);
              psi = Math.atan2(-matrix[0][1],matrix[0][0]);
            }

            //併進ベクトルを代入
            X=matrix[0][3];
            Y=matrix[1][3];
            Z=matrix[2][3];

            println("X:"+X+" Y:"+Y+" Z:"+Z);
            println("phi:"+Math.toDegrees(phi)+" theta"+Math.toDegrees(theta)+" psi:"+Math.toDegrees(psi));
            println();

            //マーカの位置にあわせて立体を表示
            translate(width/2+(int)X, height/2+(int)Y,-(int)Z+600);    
            //軸の向き的にy軸に180度,z軸に90度回転させて表示.あとなんか-をかける.
            rotateX(-(float)phi);
            rotateY(-((float)theta+(float)Math.PI));
            rotateZ(-((float)psi+(float)Math.PI/2));

            //マーカ平面ぽいものを描画
            stroke(0);
            fill(255, 255, 255);
            pushMatrix();
            translate(-50, -50, 0);
            rect(0,0,100,100);
            popMatrix();
            //X軸の棒を描画
            stroke(0);
            fill(255, 0, 0);
            pushMatrix();
            translate(-50, 0, 0);
            box(100, 5, 5);
            popMatrix();
            //Y軸の棒を描画
            stroke(0);
            fill(0, 255, 0);
            pushMatrix();
            translate(0, -50, 0);
            box(5, 100, 5);
            popMatrix();
            //Z軸の棒を描画
            stroke(0);
            fill(0, 0, 255);
            pushMatrix();
            translate(0, 0, 50);
            box(5, 5, 100);
            popMatrix();

          }        
        } 

      }
    }
    myClient.clear();
  }
}

参考

  1. QPToolkit:Webカメラを使った簡単位置計測
  2. 回転行列からオイラー角のパラメータ抽出を行う
  3. 1.変換(transformations)