Skip to content

robot_state_publisher_principle

Jump to bottom Edit New page
Masaru Morita edited this page Oct 31, 2015 · 13 revisions

robot_state_publisher の仕組み

なんで突然こんなことを？

  • gazebo起動時にrviz観察した場合、ホイールのフレームが回転しました。
  • 一方、実機のbagファイル再生時 は、rvizで観察してもホイールフレームは見えているのに、(joint_state_publisher, robot_state_publisherの起動状態でも) 回転はしていませんでした。
  • なぜだ！？　と思い、調べてみました。推測含みますが、分かったことを記載しておきます。
  • まぁ、実動作には支障はないのですが、なんとなく笑。

概要

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

参考文献

joint_state_publisherノードを利用した場合

  • rqt_graphを見てみます。ほうほう、確かにrobot_state_publisherはjoint_statesトピックをsubscribeしています。 rqt_joint_state_publisher

  • その先も見てみます。robot_state_publisherが/tfをpublishし、rvizがsubscribeしている様子が分かります。 rqt_joint_state_publisher_rviz

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

  • ユーザが入力した値に従って/joint_statesトピックをpublishします。内部的には、下記のように/joint_statesがpublishされます。

  • コードのイメージ。あくまでイメージ。

#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] ="left_front_wheel";
        joint_state.position[0] = hoge0; // left_front_wheelフレームの回転角度
        //
        joint_state.name[1] ="right_front_wheel";
        joint_state.position[1] = hoge1; // right_front_wheelフレームの回転角度
        //
        joint_state.name[2] ="left_rear_wheel";
        joint_state.position[2] = hoge2; // left_rear_wheelフレームの回転角度
        //
        joint_state.name[3] ="right_rear_wheel";
        joint_state.position[3] = hoge3; // right_rear_wheelフレームの回転角度
        // 以下、ジョイント数分続く。固定フレームの場合、positonは0となる。

        joint_pub.publish(joint_state);
    }
}
  • あとは、robot_state_publisherが良きに計らってTFフレームを発行してくれます。

gazeboを利用した場合

  • rqt_graphを見てみます。 rqt_gazebo
  • その先も見てみます。先ほどと同様、robot_state_publisherが/tfをpublishし、rvizがsubscribeしている様子が分かります。 rqt_gazebo_rviz
    • 大事なことは、gazeboが直接/tfをpublishしていないという点です。
    • これは、ロボットのジョイントに関する情報が/joint_statesトピックに集約されていることを示しています。
  • gazeboは/robot_descriptionパラメータを機械的に解読しジョイントを割り出します。
  • その中で、ユーザが明示的に指定した稼働ジョイントについて、gazebo内で回転角度等全部計算した上で、/joint_statesトピックをpublishしてくれます。
  • なお、joint_state_publisherの起動は不要です。
  • 内部的には、下記のように/joint_statesがpublishされているはずです。
  • コードのイメージ。あくまでイメージ。
#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] ="left_front_wheel";
        joint_state.position[0] = hoge0; // left_front_wheelフレームの回転角度
        //
        joint_state.name[1] ="right_front_wheel";
        joint_state.position[1] = hoge1; // right_front_wheelフレームの回転角度
        //
        joint_state.name[2] ="left_rear_wheel";
        joint_state.position[2] = hoge2; // left_rear_wheelフレームの回転角度
        //
        joint_state.name[3] ="right_rear_wheel";
        joint_state.position[3] = hoge3; // right_rear_wheelフレームの回転角度
        // 以下、ジョイント数分続く。固定フレームの場合、positonは0となる。

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

        joint_pub.publish(joint_state);
    }
}
  • 補足
    • gazeboの場合にジョイント周りの制御を最上位で司るのは、gazebo_ros_controlというgazebo用プラグインです。
    • 更に、gazebo_ros_controlの配下にcontroller_spawnerというノードが必要です。目的に合わせて適切なコントローラをspawnします。
    • 例えば、joint_states周りの面倒を見てくれるコントローラは、joint_state_controllerというものらしいです。こいつをspawnしないと、下記のように回転するジョイントのTFフレームだけが発行されないという現象が起こりました。 rviz_gazebo_without_joint_state_controller
    • joint_state_controllerをspawnすると解決します。 rviz_gazebo_with_joint_state_controller

実機を利用した場合

  • ここまで来ればもう分かります。gazebo内部でやっているオドメトリと/joint_statesのpublishをドライバにやらせればよいと考えます。
  • パラメータから機械的に解読するのではなくて、自分で明示的に回転ジョイントを指定する必要があります。
  • third_robot_driverパッケージのthird_robot_driver_nodeで/odomトピックをpublishしている部分で、/joint_statesトピックもpublishすれば、おそらく実機で動かした時でもrviz上のホイールのTFフレームが回転するはずです。
  • コードのイメージ。あくまでイメージ。
#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] ="left_front_wheel";
        joint_state.position[0] = hoge0; // left_front_wheelフレームの回転角度
        //
        joint_state.name[1] ="right_front_wheel";
        joint_state.position[1] = hoge1; // right_front_wheelフレームの回転角度
        //
        joint_state.name[2] ="left_rear_wheel";
        joint_state.position[2] = hoge2; // left_rear_wheelフレームの回転角度
        //
        joint_state.name[3] ="right_rear_wheel";
        joint_state.position[3] = hoge3; // right_rear_wheelフレームの回転角度
        // 以下、ジョイント数分続く。固定フレームの場合、positonは0となる

        // エンコーダの値を取るなりなんなり。
        ~ third_robot_interface でやっている辺り。 ~

        // 回転角度やオドメトリ計算。ジョイントの角度(hoge0 ~ hoge3)も更新。
        ~ third_robot_interface でできそうな辺り。 ~

        joint_pub.publish(joint_state);
    }
}
Add a custom footer

#Contents

Clone this wiki locally