環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
GUIのスライダーでURDFを記述して表示したRviz上のロボットを動かすこというサンプルを作りました。RvizはROSの一部で本来はロボットの状態を可視化するものです。つまりROSノードからの操作でRvizを動かせなくてはなりません。
Rviz上のロボットが動く経緯
イメージ的には以下のような経路でRviz上のロボットが表示されて、動きます。
- urdfの中のfixedなjoint(=linkの関係=tf)については/robot_state_publisherがurdfを呼んだ後に/tf_staticというトピックでpublishします。
- RvizGUIは/joint_statesをpublishします（一番左のuser_nodeという部分に該当します）。これは各関節の名前と角度の組のリストです。これを/robot_state_publisherがsubscribeしてurdfと照合して、その結果を/tfに投げます。ただしこの機能は1軸の関節にしか使えません。
- 2軸以上のjointの場合は/robot_state_publisherが対応していないので、直接/tfにpublishする必要があります。
- Rvizは/tf、/tf_staticの情報をもとにurdfで指定された図形を付けて表示します。
urdfのjointのtypeに従ってまとめると
| type | 軸数 | 動作 |
|---|---|---|
| fixed | 0 | /robot_state_publisheが/tf_staticをpublish |
| revolute | 1 | /robot_state_publisheに/joint_statesを投げて駆動する |
| continuous | 1 | ↑に同じ |
| prismatic | 1 | ↑に同じ |
| planar | 3 | 直接/tfに投げることで動く |
| floating | 6 | ↑に同じ |
つまりRvizはロボットの関節などの関係を理解しているのではなくただtfにしかがって表示をしているだけなのです。
user_nodeを作れば任意の動きをさせるROSノードを作ることができます。
joint_stateを投げるROSノード
サンプルコードを以下に示します。urdfはROS講座15で使ったjointが2つあるアームロボットを使います。typeがrevolveの2つのjoint(body2_joint、body3_joint)がありますが、body2_joint=x、body3_joint=-2xと正負が逆の角度を指定してxをどんどん増加させていくプログラムを作ります。
今回はRvizの表示テストように以下のようなノードを作りましたが、自作のロボットを作った時にはそのロボットの実際の動作に応じたjoint_statesをpublishすれば実際のロボットの動きをRvizに反映できます。
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <string>
#include <math.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "vis_joint_publisher");
ros::NodeHandle nh;
ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok())
{
sensor_msgs::JointState js0;
js0.header.stamp = ros::Time::now();
js0.name.resize(2);
js0.name[0] = "body2_joint";
js0.name[1] = "body3_joint";
js0.position.resize(2);
js0.position[0] = -1.0 * (float)count / 40.0;
js0.position[1] = 2.0 * (float)count / 40.0;
joint_pub.publish(js0);
count++;
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
sensor_msgs::JointState型を使います。jointstate型はnameとpositionの2つの配列になっていて、それぞれにjointの名前と値を入れます。このjointの名前はURDFの<joint>に記述した名前です。
<launch>
<arg name="model" default="$(find vis_lecture)/urdf/simple_body4.urdf"/>
<arg name="rvizconfig" default="$(find vis_lecture)/rviz/joint_display.rviz" />
<arg name="length0" default="1.0" />
<node name="vis_joint_publisher" pkg="vis_lecture" type="vis_joint_publisher"/>
<param name="robot_description" command="$(find xacro)/xacro $(arg model) length:=$(arg length0) --inorder" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
実行
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
roslaunch vis_lecture joint_display.launch
このように自動的にロボットの表示が動きます。