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> # 終わりに こんなのでよいのでしょうかね。無知よりはましになったかなと思います。