りさーちゃーのひよこ: 1月 2016

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>

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

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