SPRESENSEでrviz2上のUDRFモデルを動かす

最近、ROS2を触り始めました。microROS を使って SPRESENSE でプログラミングが出来るようになったのですが、ロボットを作るのがなかなか手が動かない。そこでモチベーションをあげるために、ホストPC上のシミュレーションモデルをSPRESENSEで動かせるか試してみました。

URDFのモデルを準備する

URDFとはPC上でロボットをモデリングするためのXMLベースの記述言語のことです。ロボットを作るのは時間がかかります。一方で制御ソフトウェアの開発も時間がかかってしまいます。ロボットを作ってから制御ソフト開発となると完成までかなり時間がかかってしまいます。そこで、ROSではロボットをシミュレーションする環境が用意されました。

それだけでなく、ロボット完成後もロボットの動きと連動してシミュレーションされたロボットを動かすこともできますので、ロボットの挙動がどのようになっているか把握しやすくなっています。

URDFについての詳細は次の資料が役に立つと思いますので参照してください。

・urdf_tutorial: Learning URDF Step by Step
・MathWorks:URDF入門
・マニピュレータ型ロボットのURDF作成
・移動型ロボットのURDF作成
・ROS練習用のロボットを作る

urdf_tutorial をビルドする

urdf を ros2 で使えるようにするため、urdf_tutorial というプロジェクトをダウンロードしコンパイルします。

ターミナルから次のコマンドでダウンロード＆ビルドができます。（すでにROS2 humbleの環境をインストール済であることが前提です）

~$ source /opt/ros/humble/setup.bash
~$ mkdir -p ros2_ws/src
~/ros2_ws$ cd ros2_ws
~/ros2_ws$ git clone -b ros2 https://github.com/ros/urdf_tutorial.git src/urdf_tutorial
~/ros2_ws$ colcon build --packages-select urdf_tutorial
~/ros2_ws$ source ./install/setup.bash
~/ros2_ws$ ros2 launch urdf_tutorial display.launch.py model:=./src/urdf_tutorial/urdf/07-physics.urdf

最後の一行は、./src/urdf_tutorial/urdf/070phisics.urdf と言う URDF で記述されたロボットのモデルを rviz2 で立ち上げるコマンドです。次の画面が出てきたら正常に urdf_tutorial がビルドできています。

このモデルはちょっと複雑なので、もう少し単純化したモデルがほしいところです。でも、URDFをゼロから作るのは大変なので、アールティ社さんの作られた教育用ロボットアーム「CRAME+」のモデルを使わせていただきました。

・ROSを使用したCRANE+の動かし方　その8

こちらのロボットは関節が４つ、アーム（手を含む）が５つで構成されたモデルです。関節を「joint」,アームを「link」と呼びます。動かすときはjointを操作します。

10-arm.urdf

<?xml version="1.0"?>
<robot name="crane">
 
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.0227" rpy="0 0 0" />
      <geometry>
    <box size="0.0499 0.032 0.0454" />
      </geometry>
      <material name="red">
    <color rgba=".8 0 0 1" />
      </material>
    </visual>
  </link>
 
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0.0145 0 0.0454" rpy="0 0 0" />
    <axis xyz="0 0 1" />
    <limit lower="-2.61" upper="2.61" effort="0.5" velocity="6.0" />
  </joint>
 
  <link name="link1">
    <visual>
      <origin xyz="0 0 0.013" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.026" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0 0.026" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
  </link>
 
  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.026" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <limit lower="-1.75" upper="1.75" effort="0.5" velocity="6.0" />
  </joint>
 
  <link name="link2">
    <visual>
      <origin xyz="0 0 0" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0 0.0415" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.083" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0 0.083" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
  </link>
 
  <joint name="joint3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.083" rpy="0 0 0" />
    <axis xyz="0 -1 0" />
    <limit lower="-2.55" upper="2.55" effort="0.5" velocity="6.0" />
  </joint>
 
  <link name="link3">
    <visual>
      <origin xyz="0 0 0" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0 0.04675" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.0935" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0 0.0935" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
  </link>
 
  <joint name="joint4" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0 0 0.0935" rpy="0 0 0" />
    <axis xyz="0 -1 0" />
    <limit lower="-1.80" upper="1.80" effort="0.5" velocity="6.0" />
  </joint>
 
  <link name="link4">
    <visual>
      <origin xyz="0 0 0" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0 0.0227" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.061" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0.03295 0.065727241" rpy="-0.52359878 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.019" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 0.0377 0.09665" rpy="0 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.0454" />
      </geometry>
      <material name="red" />
    </visual>
  </link>
 
  <joint name="joint5" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0 -0.0145 0.045" rpy="0 0 0" />
    <axis xyz="-1 0 0" />
    <limit lower="-0.69" upper="0.69" effort="0.5" velocity="6.0" />
  </joint>
 
  <link name="link5">
    <visual>
      <origin xyz="0 0 0" rpy="0 1.57079632675 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 -0.013 0" rpy="0 0 0" />
      <geometry>
        <box size="0.0475 0.026 0.024" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 -0.0267 0.023" rpy="0 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.070" />
      </geometry>
      <material name="red" />
    </visual>
 
    <visual>
      <origin xyz="0 -0.0166252378 0.07545" rpy="-0.52359878 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.040299" />
      </geometry>
      <material name="red" />
    </visual>
  </link>
 </robot>

こちらの URDF モデルをコピーして ~/ros2/src/urdf_tutorial/urdf/10-arm.urdf として保存します。次のコマンドで起動してみるとアームなロボットのモデルが表示されます。

~/ros2_ws$ ros2 launch urdf_tutorial display.launch.py model:=./src/urdf_tutorial/urdf/10-arm.urdf

URDF を制御するためトピックを調べる

トピックがどうなるか見てみましょう。最初は urdf を立ち上げる前の状態です。

~/ros2_ws$ ros2 topic list
/parameter_events
/rosout

コンソールで次のコマンドを打って URDF のモデルを立ち上げます。

~/ros2_ws$ ros2 launch urdf_tutorial display.launch.py model:=./src/urdf_tutorial/urdf/10-arm.urdf

別のコンソールを立ち上げて、トピックの様子を見てみましょう。

~$ source /opt/ros2/humble/setup.bash
~$ ros2 topic list
/joint_states
/parameter_events
/robot_description
/rosout
/tf
/tf_static

いくつか新しいトピックが現れました。この中の "joint_states"は、次のUI "Joint State Publisher" が生成しているトピックです。画面中のレバーを動かすと、URDFのモデルを動かすことができます。

ということは、SPRESENSE から "Joint State Publisher" が送るトピックの内容を送ればよいということが分かります。

Joint1 を動かすための情報を確認する

ここで、Joint1 を動かすために Joint1 がどうなっているか確認をしてみます。Joint1 の URDF の定義を見てみましょう。

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0.0145 0 0.0454" rpy="0 0 0" />
    <axis xyz="0 0 1" />
    <limit lower="-2.61" upper="2.61" effort="0.5" velocity="6.0" />
  </joint>

"parent link" は、接続元のアーム（link）の名前を指定しています。
"chile link" は、接続先のアーム（link）の名前を指定しています。
"origin" は、関節の場所と傾きを指定しています。
"axis" は、回転軸を指定しています。
"limit" は、関節の回転角の最大値・最小値、トルク、回転速度を指定します。
limit から Joint1 は、-2.61[rad]（-150度）から+2.61[rad]（+150度）の範囲で回転し、最大 0.5[N] のトルク、回転速度 6.0 [rad/sec] となっています。

ですので、Joint1 を動かすには、joint_stateトピックに回転する角度をメッセージとして投げてあげればよさそうです。

そうと分かれば、トピックの構造を理解して SPRESENSE から必要な情報をなげるだけです。 トピック joint_state の型は、"sensor_msgs/JointState.msg" で次のように定義されています。

Header header

string[] name
float64[] position
float64[] velocity
float64[] effort

このメッセージは複数のジョイントを指定できるようになっているため配列となっています。

"name" にはジョイント名を指定します。
"position" は、各ジョイントの回転角度を指定します。
"velocity" は、各ジョイントの回転速度を指定します。
"effort" は、各ジョイントの回転トルクを指定します。
Header 構造体の定義はこちらにありますので参照してください。

これで Joint1 を動かす方法については目処がついてきました。

SPRESENSE に Joint1 を動かすプログラムを実装する

組み込み向けのROS2ライブラリとして micro-ROS というライブラリが公開されています。SPRESENSE 用には次の github に Arduino IDE 環境向けのライブラリが用意されています。

サンプルや解説についてはここにあります。

これらを参考に、 Joint1 を動かすプログラムを SPRESENSE に実装してみました。

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <sensor_msgs/msg/joint_state.h>

static rcl_publisher_t publisher;
static rclc_executor_t executor;
static rclc_support_t support;
static rcl_allocator_t allocator;
static rcl_node_t node;
static rcl_timer_t timer;

static sensor_msgs__msg__JointState jstate;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}

void error_loop(){
  while(1){ digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(100); }
}

void setup() {
    
  set_microros_transports();  
  allocator = rcl_get_default_allocator();
   
  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));  

  // create node 
  RCCHECK(rclc_node_init_default(&node, "joint_state_publisher", "", &support));
  
  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
    "joint_states"));

  //sprintf(frame_id, "odom");
  static rosidl_runtime_c__String joint_name[5];
  static double positions[5];
  
  // Joint1  
  joint_name[0].data = "joint1";
  joint_name[0].size = 6;
  joint_name[0].capacity = 7;

  // Joint2
  joint_name[1].data = "joint2";
  joint_name[1].size = 6;
  joint_name[1].capacity = 7;

  // Joint3
  joint_name[2].data = "joint3";
  joint_name[2].size = 6;
  joint_name[2].capacity = 7;

  // Joint4
  joint_name[3].data = "joint4";
  joint_name[3].size = 6;
  joint_name[3].capacity = 7;

  // Joint5
  joint_name[4].data = "joint5";
  joint_name[4].size = 6;
  joint_name[4].capacity = 7;

  jstate.name.data = joint_name;
  jstate.name.size = 5;
  jstate.name.capacity = 5;
  jstate.position.data = positions;
  jstate.position.size = 5;
  jstate.position.capacity = 5;
}

void loop() {
  static float position0 = 0.0;
  static bool increment0 = true;
  
  uint32_t current = micros();
  jstate.header.stamp.sec = current / 1000000;
  jstate.header.stamp.nanosec = current - jstate.header.stamp.sec*1000000;
  if (increment0) {
    position0 += 0.1;
    if (position0 >=  1.50) increment0 = false;
  } else {
    position0 -= 0.1;
    if (position0 <= -1.50) increment0 = true;
  }

  jstate.position.data[0] = position0;
  // publish topic
  RCSOFTCHECK(rcl_publish(&publisher, &jstate, NULL));
  delay(1000);
}

joint_state の配列を5つ確保しているのは、トピックに関節数分の情報を与えておかないと、rviz2 が URDF のモデルを正しく構築できないためです。

このプログラムでは Joint1 を1秒毎に 0.1[rad]（5.7度）回転させ±86度の間を行ったり来たりするプログラムになります。トルクや速度は特に指定をしなくても問題なく動きました。（恐らく指定したほうがよいと思いますが、、、）

display.launch.py の書き換え

SPRESENSE との接続実験をする前に、display.launch.py の書き換えが必要です。デフォルトのままでは、"Joint State Publisher" が立ち上がってしまい、"joint_state" トピックを専有してしまうため、SPRESENSE が出力する "joint_state" トピックが無効になってしまいます。

"Joint State Publisher" アプリを止めるのはさほど難しくありません。 ~/ros2_ws/src/urdf_tutorial/launch/display.launch.py の57行,58行をコメントアウトするだけです。

~/rosn2_ws/src/urdf_tutorial/launch/display.launch.py

 53     return LaunchDescription([
 54         gui_arg,
 55         model_arg,
 56         rviz_arg,
 57         # joint_state_publisher_node,
 58         # joint_state_publisher_gui_node,
 59         robot_state_publisher_node,
 60         rviz_node
 61     ])

編集したら、今一度 colcon でビルドし直します。

~/ros2_ws$ colcon build --packages-select urdf_tutorial
~/ros2_ws$ source install/setup.bash

これで rviz2 側の準備は完了です。

URDF のモデルを動かしてみる

いよいよ URDF のモデルを動かします。上記スケッチを書き込んだSPRESENSEをUSBを接続したまま、micro-ROS-agent をシリアル接続で立ち上げます。

~$ sudo docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble  serial　--dev /dev/ttyUSB0
[1659402842.881086] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1659402842.881813] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4

SPRESENSE に、先程の Arduino のスケッチを書き込みます。しばらくすると、micro-ROS-agent を立ち上げた コンソールにメッセージが追加して出てきます。これで、SPRESENSE とホストPCとの接続は完了です。

~$ sudo docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble  serial　--dev /dev/ttyUSB0
[1659402842.881086] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1659402842.881813] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1659348850.359973] info     | Root.cpp           | create_client            | create                 | client_key: 0x28E804DA, session_id: 0x81[1659348850.360103] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x28E804DA, address: 0
[1659348850.390805] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x28E804DA, participant_id: 0x000(1)
[1659348850.407067] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x28E804DA, topic_id: 0x000(2), participant_id: 0x000(1)
[1659348850.416714] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x28E804DA, publisher_id: 0x000(3), participant_id: 0x000(1)
[1659348850.428346] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x28E804DA, datawriter_id: 0x000(5), publisher_id: 0x000(3)

それが出来たら、別のコンソールを立ち上げて、URDF モデルを rviz2 で立ち上げます。

~/ros2_ws$  ros2 launch urdf_tutorial display.launch.py model:=./src/urdf_tutorial/urdf/10-arm.urdf

すると画面が現れて、ロボットが回転しはじめます。左ペインにあるTFのチェックボックスを外すと回転している様子がよく分かると思います。

他の関節（Joint）も同じような要領でコマンドを送れば、変更をすることができます。トルクや速度を指定してみるのも面白いでしょう。ぜひやってみてください。

作ってみた感想

一番つまったのは、Joint State Publisher を立ち上げると、SPRESENSE のトピックが無効になることでした。先に SPRESENSE を立ち上げているので、同じトピック名なら SPRESENSE のほうが優先されるのだろうと思ったのですが甘かった。トピック名が重なった場合の優先順位が ROS2 の中でどうなっているのかは、まだ謎です。当然のことですが、トピック名は被らないようにしないといけないですね。その他の部分はすんなりと行ったのですが、URDF の理解が浅いので苦労します。メカ屋ではないため、組み上がりのイメージができなかったり、物理シミュレーションのための inertia などは、どう設定してよいのか分からなかったり。まだまだ、ROS2 の冒険は続きそうです。