りさーちゃーのひよこ

2016年4月19日火曜日

2016年2月11日木曜日

Gazebo から ROS のプラグインを呼ぶ処理の仕組みについて書いてみた

2016年1月26日火曜日

Controller と HardwareInterface との間の処理の仕組みについて書いてみた

2016年1月9日土曜日

robot_state_publisher の仕組み

#はじめに
よく分からずに`robot_state_publisher`を使っていたのですが、どう動いているのちょっと気になったので備忘録です。

# 概要
`robot_state_publisher`ノードは、稼働ジョイントの現在角度を表す`/joint_states`トピックをsubscribeし、その状態を反映させた`TF`フレームをpublishする役割を持ちます。

# 参考文献
- [ROS Wiki](http://wiki.ros.org/robot_state_publisher)
- [Learning ROS for Robotics Programming: A Practical, Instructive, and Comprehensive Guide to Introduce Yourself to Ros, the Top-notch, Leading Robotics Framework](http://www.amazon.co.jp/Learning-ROS-Robotics-Programming-Comprehensive/dp/1782161449)

# `joint_state_publisher`ノードを利用した場合
- `rqt_graph`を見てみます。ほうほう、確かに`robot_state_publisher`は`joint_states`トピックをsubscribeしています。
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjHXMj7bosBz7w-X0d13lh30xIeVxT4vRJ66zLyN1H4UXkGbFQODCPWLYTd1Jrp1GgAcpIJk7KDNfgCuyoJeJLaayFVVr7P7WncHpgP7Bvubmb1Vmm_Xed7BxMaO6YGPubd2mapisAAjvs9/s1600/rqt_joint_state_publisher.png" imageanchor="1" style="clear: left; margin-bottom: 1em; margin-right: 1em;"><img border="0" width="50%" height="50%" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEjHXMj7bosBz7w-X0d13lh30xIeVxT4vRJ66zLyN1H4UXkGbFQODCPWLYTd1Jrp1GgAcpIJk7KDNfgCuyoJeJLaayFVVr7P7WncHpgP7Bvubmb1Vmm_Xed7BxMaO6YGPubd2mapisAAjvs9/s1600/rqt_joint_state_publisher.png" /></a>

- その先も見てみます。`robot_state_publisher`が`/tf`をpublishし、`rviz`がsubscribeしている様子が分かります。
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj7vZ0a1MCK1rI0yofQTPAUq8JwscZ3Gs3rEgadnknDrHQoMeitoDjiXVzqIMeqON5ZOIRVtYW6dAMJA6tE7Fhzf64NZX7yZoSGRPgPRGkcihZUigAmBePUJ5f77vISWoVKbNCj8JR8-yzO/s1600/rqt_joint_state_publisher_rviz.png" imageanchor="1" ><img border="0" width="50%" height="50%" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEj7vZ0a1MCK1rI0yofQTPAUq8JwscZ3Gs3rEgadnknDrHQoMeitoDjiXVzqIMeqON5ZOIRVtYW6dAMJA6tE7Fhzf64NZX7yZoSGRPgPRGkcihZUigAmBePUJ5f77vISWoVKbNCj8JR8-yzO/s1600/rqt_joint_state_publisher_rviz.png" /></a>

- `joint_state_publisher`は、`/robot_description`パラメータを機械的に解読し、稼働可能な`joint`一覧を表示します。ユーザはこれをグリグリいじって稼働可能なジョイント角度を自由に設定できます。

- ユーザが入力した値に従って`/joint_states`トピックをpublishします。内部的には、下記のように`/joint_states`がpublishされます。
- ジョイントが4つ(joint1～joint4)まである場合のコードのイメージ。あくまでイメージ。
<pre class="brush: cpp;">
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>

int main() {
 ~省略~
 ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
 sensor_msgs::JointState joint_state;
 ~省略~
 ユーザ入力時のコールバック {
 //update joint_state
 joint_state.header.stamp = ros::Time::now();

//稼働ジョイント数分領域確保
        joint_state.name.resize(ジョイント数);
        joint_state.position.resize(ジョイント数);

//稼働ジョイント現在角度設定。ユーザ入力に合わせてフレームIDと回転角度[rad]入力。
        joint_state.name[0] ="joint1";
        joint_state.position[0] = hoge0; // joint1フレームの回転角度
        //
        joint_state.name[1] ="joint2";
        joint_state.position[1] = hoge1; // joint2フレームの回転角度
        //
        joint_state.name[2] ="joint3";
        joint_state.position[2] = hoge2; // joint3フレームの回転角度
        //
        joint_state.name[3] ="joint4";
        joint_state.position[3] = hoge3; // joint4フレームの回転角度
        // 以下、ジョイント数分続く。固定フレームの場合、positonは0となる。

joint_pub.publish(joint_state);
    }
}

</pre>
- あとは、`robot_state_publisher`が良きに計らって`TF`フレームを発行してくれます。

# `gazebo`を利用した場合
- `rqt_graph`を見てみます。
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiE1O-C0_NiconmPGTYPMdyzTcxhY0tVkywYKAoWAiWKCwGIR2ufc9pigxxpMs_ovxG537w4SlhwdZIAd8GQavT7KL2I6UW1HIPqwhWnXjvIvOro43Bc9lNSjRW31rFYJsWjMtWR99roQY_/s1600/rqt_gazebo.png" imageanchor="1" ><img border="0" width="50%" height="50%" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEiE1O-C0_NiconmPGTYPMdyzTcxhY0tVkywYKAoWAiWKCwGIR2ufc9pigxxpMs_ovxG537w4SlhwdZIAd8GQavT7KL2I6UW1HIPqwhWnXjvIvOro43Bc9lNSjRW31rFYJsWjMtWR99roQY_/s1600/rqt_gazebo.png" /></a>

- その先も見てみます。先ほどと同様、`robot_state_publisher`が`/tf`をpublishし、`rviz`がsubscribeしている様子が分かります。
<a href="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEggtS7wggC42gzjFM_rJq5OBW9kNmWzPB0XmL49hDms5zoY5S_oplq4l8R5feXqDu3BmDMYwwshK3N2WS6iJB-tcmBAnfTSNk-2vydGnjFsqH0iIVzURIPakV7oAFpnxdTYATiXk-x9Ghso/s1600/rqt_gazebo_rviz.png" imageanchor="1" ><img border="0" src="https://blogger.googleusercontent.com/img/b/R29vZ2xl/AVvXsEggtS7wggC42gzjFM_rJq5OBW9kNmWzPB0XmL49hDms5zoY5S_oplq4l8R5feXqDu3BmDMYwwshK3N2WS6iJB-tcmBAnfTSNk-2vydGnjFsqH0iIVzURIPakV7oAFpnxdTYATiXk-x9Ghso/s1600/rqt_gazebo_rviz.png" /></a>
 - 大事なことは、`gazebo`が直接`/tf`をpublishしていないという点です。
 - これは、ロボットのジョイントに関する情報が`/joint_states`トピックに集約されていることを示しています。
- `gazebo`は`/robot_description`パラメータを機械的に解読しジョイントを割り出します。
- その中で、ユーザが明示的に指定した稼働ジョイントについて、`gazebo`内で回転角度等全部計算した上で、`/joint_states`トピックをpublishしてくれます。
- なお、`joint_state_publisher`の起動は不要です。
- 内部的には、下記のように`/joint_states`がpublishされているはずです。
- ジョイントが4つ(joint1～joint4)まである場合のコードのイメージ。あくまでイメージ。
<pre class="brush: cpp;">
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>

int main() {
 ~省略~
 ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
 sensor_msgs::JointState joint_state;
 ~省略~
 while (ros::ok()) {
 //update joint_state
 joint_state.header.stamp = ros::Time::now();

//稼働ジョイント数分領域確保
        joint_state.name.resize(ジョイント数);
        joint_state.position.resize(ジョイント数);

//稼働ジョイント現在角度設定。フレームIDと回転角度[rad]入力。
        joint_state.name[0] ="joint1";
        joint_state.position[0] = hoge0; // joint1フレームの回転角度
        //
        joint_state.name[1] ="joint2";
        joint_state.position[1] = hoge1; // joint2フレームの回転角度
        //
        joint_state.name[2] ="joint3";
        joint_state.position[2] = hoge2; // joint3フレームの回転角度
        //
        joint_state.name[3] ="joint4";
        joint_state.position[3] = hoge3; // joint4フレームの回転角度
        // 以下、ジョイント数分続く。固定フレームの場合、positonは0となる。

// gazebo内部の回転角度やオドメトリ計算。ジョイントの角度(hoge0 ~ hoge3)も更新。
        ~省略~

joint_pub.publish(joint_state);
 }
}
</pre>

# 終わりに
こんなのでよいのでしょうかね。無知よりはましになったかなと思います。

2016年1月6日水曜日

Bloggerに引越して来ました

#はじめに
利便性を考慮し，アメブロ時代の`りさーちゃーのたまご`から引っ越して，ブログ名を`りさーちゃーのひよこ`に改めます．
博士課程に進学したから（？），`たまご`から`ひよこ`への昇進（？）です．ひよっこであることに代わりはありません．

#なんでBlogger？
## 決定打
- アメブロが吐くMobableTypeのtextから移行すると楽
- Googleアカウントもっているから始めるの楽
- Simple is Best!

結局楽なのを選んだ．

## 惜しい点
- Markdown 編集が間接的になる
- スタイリッシュさに欠ける

tumblr辺りでかっちょよくとも思ったけど，前の記事が引き継ぎが大変そうだなー，と思ってBloggerに逃げた．
まぁ，間接的でもMarkdown使えるんでいいっす．

#手順
アメブロから直はできません．ちょっとめんどいです．
##アメブロ→FC2ブログに移行
<a href="http://diwao.com/ds-labo/2011/02/ameba-to-fc2.html">diwao日記 - アメブロから引っ越ししようとしたら思いの外大変だった</a> 
このお引越しがよく出来ている．
インポートすると，画像もFC2側のサーバに上げてくれて，リンク貼り直しまでしてくれる．

##FC2ブログでMovableType形式のtxtファイルを出力
<a href="https://woresite.jp/2007/02/10/180406.html">ヨレサイト-FC2ブログをエクスポートしてMovableTypeに引っ越す方法</a> 
今だとUTF-8で出力されるようで，余計な変換はいらないっぽい．多分．

##MovableType形式のtxtファイルを，Bloggerのxmlファイルに変換
<a href="http://syasuda.com/t/tools.html">syasudaのツール</a> 
これ探すの大変だった． 
<a href="http://s.jkunblog.com/article/151572274.html">JKun Blog - Bloggerブログのxmlファイルに関する形式変換方法</a> 
を見ながら，googleのコンバータ使ったりもしたんだが，何故か日付のフォーマットが合ってなかったりして泣きそうだった時にsyasudaさんのツールを見つけました．
感謝．

##Bloggerでxmlファイルをインポート
<a href="https://support.google.com/blogger/answer/41387?hl=ja">Googleヘルプ</a> 
画像のリンクはFC2のまま．もう疲れたのでこれでいいw

##Blogger側の調整
上記だけだと微妙に合わないところがある．
日付とか，記事にタイトルにコメントタイトルが入ってきちゃったり...
その辺気になるところを修正．

あと，やらなくてもいいのだけど，Markdownで書き直したり（<a href="http://qiita.com/her0m31/items/1804bdc251a647e0e9a8">BloggerでMarkdown書けるようにした。</a>），
プログラムコードはハイライトさせたり（<a href="http://p--q.blogspot.jp/2013/05/bloggersyntaxhighlighter1.html">P--Q - Blogger:SyntaxHighlighter(1)ブログにソースコードを綺麗に表示させる</a>）して
遊んでみた．結構疲れたけど，楽しかった．

Markdownエディタはこいつがいい感じ 
→<a href="http://qiita.com/shohei909/items/f33a6cace1c83bf36c8a">Windows, Mac, Linuxで使えるMarkdownエディタ、HarooPadを使う</a>

#おわりに
というわけで，無事引越ししました．
調整の際に昔のブログ眺めたりして，なかなかおもしろかった．留学した時とか，震災の時とか．当時こんなこと感じたんだなーとか．

とまぁそんなわけで（？），また気分がのったときにちょいちょい書いておこうかと思います．

2015年8月17日月曜日

rosbag を使おうとしたら clock トピックにpublish しろと怒られる

2015年8月7日金曜日

Ubuntu 14.04 上のROS (Indigo) でARToolKitを導入してみる

#はじめに
ふとしたことから`ARToolKit`を導入しようと思ったのですが，ネット上の情報が余りにも散乱している．（いつものことだが．．．）
チュートリアルがチュートリアルになっていない．
てなわけで，解説がないと忘れてしまう自分のためにメモを残しておくといういつものパターン．

#ARToolKitのインストール
本丸くんを入れる前に，いくつかやっておくべきことがあります．
<a href="http://wiki.ros.org/ar_tools" target="_blank" title="">ROS.org- ar_tools</a> を参考にしましたが，つまづいたので段階的に導入．

##ROS用のARToolKit のパッケージを探す

`roslocate`がなければ<a href="http://wiki.ros.org/roslocate" target="_blank" title="">インストール</a>
<pre class="brush: bash;">
$ roslocate info ar_tools
</pre>
出力は以下のようになります．
<pre class="brush: bash;">
- git:
 local-name: ar_tools
 uri: https://github.com/ar-tools/ar_tools.git
 version: master
</pre>

##上記パッケージをローカルにダウンロード

- `wstool`がなければ<a href="http://wiki.ros.org/wstool" target="_blank" title="">インストール</a>

- 初期化．`~/catin_ws/src/.rosinstall`が作成されます．
<pre class="brush: bash;">
$ wstool init ~/catin_ws/src
</pre>

- 取得
<pre class="brush: bash;">
$ cd ~/catin_ws
$ wstool set --target-workspace=src ar_tools --git https://github.com/ar-tools/ar_tools.git
</pre>

- 更新
<pre class="brush: bash;">
$ wstool update ar_tools --target-workspace=src ar_tools
</pre>

##パッケージのインストールとビルド
<pre class="brush: bash;">
$ rosdep install -i --from-paths ar_tools
$ cd ~/catin_ws
$ catkin_make
</pre>

#uvc_camera パッケージのインストール

サンプルとして`ar_pose`を使いたいので，
`~/catkin_ws/src/ar_tools/ar_pose/launch/README.md`
を覗いてみると， uvc_camera driver が必要とのこと．
チュートリアルを見てもさっぱりわからなかったので，ネットの海をさまよってこちらのサイトを見つける
　<a href="https://defendtheplanet.net/2014/11/05/using-ros-indigo-webcam-by-the-uvc_camera-usb-video-class-package/" target="_blank" title="">Using ROS Indigo with a Webcam by the uvc_camera (USB Video Class) package</a><

##パッケージの取得・インストール・ビルド

- 取得
<pre class="brush: bash;">
$ cd ~/catkin_ws/src/
$ git clone https://github.com/ktossell/camera_umd.git
</pre>

- インストール
<pre class="brush: bash;">
$ rosdep install camera_umd uvc_camera jpeg_streamer
</pre>

- ビルド
<pre class="brush: bash;">
$ cd ~/catkin_ws
$ catkin_make #build the workspace
</pre>

##依存モジュール追加
<pre class="brush: bash;">
$ sudo apt-get install libv4l-dev
</pre>

##動作確認
<pre class="brush: bash;">
$ roscd uvc_camera/launch/
$ roslaunch ./camera_node.launch
$ rosrun image_view image_view image:=image_raw
</pre>

#ar_poseの動作のための準備

##サンプル用launchファイルの編集

`~/catkin_ws/src/ar_tools/ar_pose/launch/ar_pose_single.launch`

を確認する．注意すべき点を挙げます．

- カメラの設定
<pre class="brush: xml;">
<node ns="camera" pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera" output="log">
</pre>
タグ内の
<pre class="brush: xml;">
<param name="device" type="string" value="/dev/video0" />
</pre>
を，認識されているカメラの名前に合わせること．

- マーカの設定
<pre class="brush: xml;">
<node name="ar_pose" pkg="ar_pose" type="ar_single" respawn="false" output="screen">
</pre>
タグ内の，
<pre class="brush: xml;">
<param name="marker_pattern" type="string" value="$(find ar_pose)/data/patt.hiro"/>
</pre>
を，認識対象のマーカに合わせること．

##カメラキャリブレーション

こちらを参照

→ <a href="http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration" target="_blank" title="">ROS.org - camera_calibrationTutorialsMonocularCalibration </a>

`~/catkin_ws/src/ar_tools/ar_pose/launch/ar_pose_single.launch`
では，キャリブレーションパラメータを yaml 形式で渡す必要があります．
まずはキャリブレーションせねば汗．

###キャリブレータの導入
<pre class="brush: bash;">
$ cd ~/catkin_ws
$ rosdep install camera_calibration
$ catkin_make
</pre>

###キャリブレータの起動
- まずはカメラが動いていることを確認．/image_raw がトピックでいるはず．
<pre class="brush: bash;">
$ rostopic list
</pre>

- 動いていなければ実行．
<pre class="brush: bash;">
$ roslaunch uvc_camera camera_node.launch
</pre>

- いよいよ起動
<pre class="brush: bash;">
$ rosrun camera_calibration cameracalibrator.py --size 11x7 --square 0.02 image:=/image_raw
</pre>

ここで，キャリブレーションボードはPTAMのボードを使います．
<a href="https://www.blogger.com/null"></a><a href="https://github.com/ethz-asl/ethzasl_ptam" target="_blank" title="">GitHub - ethz-asl/ethzasl_ptam</a>
の/ptam/calib_pattern.pdf　を使用しました．

11x7 はキャリブレーションボードの格子点の数，0.02[m] (=2cm) は正方形格子の一辺の長さです．

###キャリブレーションの実行
色々な角度で撮りまくる．勝手にキャプチャしてくれる．
40枚くらい撮り終わると，CALIBLATE ボタンが有効になるので，ポチる．

しばらく待つ．1, 2分かかることもある．応答がなくなりグレーアウトしても，待つ．

##キャリブレーションファイルを yaml 形式に変換

まだ終わりません汗．
こちらを参照
　→ <a href="http://wiki.ros.org/camera_calibration_parsers" target="_blank" title="">camera_calibration_parsers</a>

- キャリブレーションファイル保存
  - 計算が終わると SAVE ボタンが有効になるので，保存．
  - デフォルトで /tmp/calibrationdata.tar.gz  に保存される．
  - 解凍して，中身の ost.txt を ~/catin_ws/src/camera_umd/uvc_camera に移動．

- ファイル変換
<pre class="brush: bash;">
$ mv ost.txt ost.ini
$ rosrun camera_calibration_parsers convert ost.ini camera_calibration.yaml
</pre>
 - この名前にしておけば，
`~/catin_ws/src/ar_tools/ar_pose/launch/ar_pose_single.launch`
内の，
<pre class="brush: xml;">
 <param name="camera_info_url" type="string" value="file://$(find uvc_camera)/camera_calibration.yaml" />
</pre>
との整合性がそのまま取れます．

#ar_poseの動作確認

##実行
<pre class="brush: bash;">$ roslaunch ar_pose ar_pose_single.launch
</pre>

<a class="detailOn" href="http://blog-imgs-86.fc2.com/m/o/r/morimoridiary254/blog_import_568b66f11875b.png" id="i13388897181"><img alt="artoolkit" src="http://blog-imgs-86.fc2.com/m/o/r/morimoridiary254/blog_import_568b66f11875b.png" id="1438945100222" ratio="1.238390092879257" style="border: currentColor; height: 323px; width: 400px;" /></a> 
ようやくここまで来た...

キャリブレーション yaml ファイルがないと，ここで怒られちゃうので，ちゃんとキャリブレーションしておきましょう．

#おわりに
どんだけやらせんだよｗ　超大変だったし！Wiki のチュートリアルがチュートリアルになってない！ｗ
まぁ，動いたからよしとしましょう．勉強になりました．

登録: 投稿 (Atom)