はじめてのROS入門とハンズオン 〜2+1 DoF ロボットアームでおえかき〜

# はじめに

本稿は新人エンジニアの教育向けに作成したRobotics研修のうち，ROSに関するトピックを抜粋・再編集したものです．ロボティクス分野が専門でないエンジニアを想定読者とし，ROSの概説からはじめてロボットアームを用いたお絵かきハンズオンまでを掲載しました．「ロボットに興味があって勉強してみたいけど何からやっていいか分からない」「ROSをおすすめされた何が嬉しいのかピンと来ない」そんな方々の一助になれば幸いです．

本稿は大きく2章構成です．1つめの章では，ROSの概説（ROSとはなにか？何が嬉しいのか？）について述べます．2つめの章では，ROS対応のロボットアームを通じて，ROSを使ったプログラミングのハンズオンを掲載しています．結構な長文になってしまったので，Qiita画面右の目次で「いま何の話を読んでいるのか」を確認しながら読み進めてみてください．

## 免責事項

本稿の内容は筆者(@B-SKY-Lab)の見解や知見に基づいて提供されています．内容の正確性や信頼性には万全を期しておりますが，これを利用して生じた損害や不利益については全て自己責任において判断の上でご利用ください．


# ROS概説

## ROSとは？

ROSとは**R**obot **O**perating **S**ystem の頭文字をとった単語です．その名の通り，「ロボット向けの**メタOS**」を謳うシステム[[1]](http://wiki.ros.org/ja/ROS/Introduction)で，単なるライブラリには留まらないフレームワークとご理解ください．例えばROSを用いたプログラミングではPythonとC++を用いることができますが，Pythonにおけるnumpyのように「importしてメソッド叩くと便利計算ができる」だけのライブラリではなく，ロボット全体のシステムを構築する上で様々な機能を提供しています．

## ROSの種別

2024年6月現在，ROSには大きく2種類のバージョンが存在します．

### ROS
単に「ROS」とだけ呼ばれるものは初代のROSを指しています．これは主に**研究用途**向けに開発されたものであり，商用システムは視野に入れていませんでした．区別のために「ROS 1」と書かれることもあり，近々サポート切れも見えてきつつあります．

### ROS 2
ROS 1が想定外の普及を遂げてしまい，商用ロボットシステムでもこれを使いたいニーズが増えてきました．しかしその設計思想は商用を想定しておらず，品質保証などの観点から苦しい部分も見えてきます．そこで**商用・産業用途**も想定して新しく作られたのが「ROS 2」です．同じROSの名前を冠していますが，**大幅な変更がたくさん**あるようです．私も全く追えておらず，早く学ばねばな...と思っています．

本稿ではROS 1のみを用いています．

## ROSの恩恵

ROSを使ってロボット開発を行うことで様々な恩恵がありますが，その中の代表的なもの4個を列挙します．

- 分散処理とプロセス間通信
  - publish / subscribe と呼ばれるプロセス間通信の基盤を提供することで、ロボットに必要な機能を複数のノード（≅プロセス）に分散できる
- 開発ツール群
  - 分散させた機能に対し、各ノードの連結や通信内容の可視化
  - ノードの一括起動
  - ログ記録
- エコシステム
  - 統合されたドキュメントや大規模コミュニティ
- 潤沢なライブラリ
  - 移動，マニピュレーション，知覚など様々なライブラリ・パッケージが利用可能

次節からこれらについて詳しくみていきましょう．

### 分散処理の嬉しさ

ROSはpublish / subscribeと呼ばれるプロセス間通信の基盤を提供し，ロボットに必要な機能を複数のノードに分散させることができます．ITエンジニア的には「プロセス」と読み替えていただいてもよいです．

具体例を用いて，この分散処理の嬉しさを考えてみましょう．ここではよくある掃除ロボットのように対向二輪型のロボを，ジョイスティックで操縦するシチュエーションを考えます．

この子をラジコンにする．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/566713bf-9fa4-7128-b766-05959ed37451.png)

まず，比較対象として**非ROS環境**でこれをシンプルに作るとどうなるでしょうか？真っ先に思いつくのは，マイコンなどで**中央集権**的に処理を書いてみる構成です．図にするとこんな感じ．

![スクリーンショット 2024-06-03 2.06.57.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/153aedd9-a2fe-acd0-3ab3-c42252910a32.png)

操縦のインターフェースとしてジョイスティックを持ち，これの信号をマイコンが受け取ってタイヤの回転数を計算して，モータへ伝えます．

```python
# この関数の中身が一定周期で無限ループする
main_loop(){

    # ジョイスティックの値（スティックの傾きやボタン押下）を読み取る
    val_js = read_joystick()

    # 読み取ったジョイスティックの値に応じて，ロボットの制御入力
    # （例えば目標の並進速度と旋回角速度）を計算する
    cmd = gen_cmd_js(val_js)

    # 目標の並進速度と旋回角速度に基づいて，左右のモータの目標速度を計算する
    [vl, vr] = gen_vel_cmd(cmd)

    # 計算した目標速度でモータを回転させる
    set_motor_vel_left(vl)
    set_motor_vel_right(vr)
}
```

（注: 制御入力として並進速度と旋回の角速度を計算するのがピンと来ない場合は，「差動二輪　制御　運動学」「対向二輪　ロボット　制御　運動学」などでググって出てくる解説を読んでみてください．）

疑似コードの意味は上記の通りです．あくまでざっくりとしたイメージですが，言われたら「ふーん，まあそんな感じになりそうだね」と思っていただける内容かと思います．


それでは，これをROSを用いた分散処理にする場合を考えてみましょう．
`main_loop()`の中身を「機能ごとのノード」に分散させると，例えば次図のようになります．

![スクリーンショット 2024-06-03 2.22.48.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/090e6a31-5c8e-1a68-5c0d-902103088817.png)

1. ジョイスティックの読み取り
1. ロボの制御入力の生成
1. モータ回転数の制御

今回は3個に分けてみました．これらのノードはそれぞれ分指して独立に実行することができ，**トピック**と呼ばれる情報が間で飛び交うようになります．図中の橙の実線矢印がトピックを表し，トピックを送出することを`publish`，送出されたトピックを読み取ることを `subscribe` といいます．ROSはこの「ノードの分散と独立した実行，およびトピックをノード間でやりとりする」ための機能を提供してくれます．ちなみに，各ノードはPythonまたはC++でコーディングします．

ROSの枠組みで各機能を分散した場合の構成はなんとなくイメージできたかと思いますが，これだけでは「何が嬉しいのか」があまり伝わってきません．そこで，**ロボットにLiDARを追加搭載して障害物回避をさせたい**と言われた場合を考えてみます．

ROSの枠組みの上でシステムを構築すると，各機能（ノード）の結合を疎にできます．これによって，**機能追加・改修・再利用**が非常に容易になります．例えば「LiDARを載せたい」となったら，これを処理するノードを「ポン付けの感覚で」追加すればよいのです．

![スクリーンショット 2024-06-03 2.32.17.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/15c1fd7e-9565-acd5-4e37-032fb64103ce.png)

システムの構成図的にはこんな感じです．LiDARをロボに乗っけて，これに対応する読み取りノードを追加実装したような状態です．このとき嬉しいのが，LiDARとは関係のない「ジョイスティック読み取りノード」「モータ回転数制御ノード」あたりは，存在を忘れたまま実装作業ができます．彼らは独立して実行されるノードなので，LiDARと直接関係ない限りは頭から追い出したままで良いんですね．これが **「結合が疎」** と言われている部分で，ロボット開発に **ROSを使う最大の嬉しさ** とも言われています．

（注: 実際にこの構成を取ろうとすると制御入力の計算には少し手を入れる必要がありますが...）


もしこれを，最初に比較対象として出した非ROS環境で行おうとしたらどうなるでしょう？
中央集権的に実装されている場合には，**他との競合に気を遣いながら**メインループを頑張って編集しなくてはなりません．LiDARを1個だけならまだマシですが，実際には開発途中でセンサやアクチュエータがぽんぽん生えてきたり入れ替わったりということはザラにあり，その度にこの辛い編集が生じていては効率もガタ落ちです．

また，この図では全ての処理が1個のループに収まっており，「（ロボ自体の）制御周期とLiDARの情報取得の周期が完全に同じになる」というある種の制約も生じます．LiDARの点群が数十万点を超える大規模なものになったとき，果たして制御周期への悪影響を避けられるでしょうか？点群取得が重たすぎて実行周期が伸びたりしませんか？高画質なカメラも繋ぎたいとなったら？悩みは尽きません．（このあたりの通信や制御周期の悩みを良い感じにしてくれる話はPublish/Subscribeの詳細の節でまた紹介します．）

![スクリーンショット 2024-06-03 2.40.23.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/650d460f-a6a7-81f2-daaa-3ec1f909081c.png)

もちろんこの構成は極端なものなので，実際に中央集権的に実装する際にも適切なモジュール分割を行うことである程度は効率を落とさずにいられるでしょう．分散処理も頑張ればフルスクラッチできるかも知れません．それも1つの解ですし，私が学生時代に所属していた研究室では実際にそうして組まれたロボットもいました．しかし，開発するロボットの中身が大規模になればなるほどそのしんどみは増していきますし，例えば世界に向けてそのシステムを公開しあうような規模になると，ROSのように「基盤になる枠組み」に乗っかっていた方が何かと好都合です．世界中の皆がオレオレ実装を公開しあうよりもずっと効率的ですし，使い回しもしやすい．LiDARメーカーも「ROS対応するようにIFを作れば良い」となっていた方が製品を売りやすいことでしょう．ROS 1が普及したのにはこのような背景もある（と思い）ます．


### Publish / Subscribeの詳細

ROSにおけるプロセス間通信であるPublish / Subscribeは，「**Publish側が主導権**を持ち，**Subscribe側はコールバック**で駆動」します．すなわち，Publish側は一定周期で（Subscribe側の都合は一切感知せず一方的に）トピックを送出し続ける動きをします．

![スクリーンショット 2024-06-03 3.03.30.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/5cd7e87c-6175-e00b-03d9-f7f814473ce3.png)

イメージはこんな感じ．UDPの感覚ですね．この図ではジョイスティック読み取りノードが，その読み取り結果を5ミリ秒おきにPublishする様子を示しています．このときジョイスティック読み取りノードは「自分が送出した情報をどのノードが読み取るか」「どのくらいの周期で送って欲しいと思っているか」などは感知しません．淡々と5ミリ秒おきに情報を送出し続けます．

この仕組みは，読み取りや送出の周期が異なる複数のハードウェアを同時に扱いたいロボットシステムにとって非常に嬉しいものです．前節でもロボの制御周期とセンサの読み取り周期の扱いが難しい話を挙げましたが，publish / subscribeのモデルで扱うと取り回しが比較的良くなります．

![スクリーンショット 2024-06-03 3.11.56.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/2e666c22-e463-c443-d0c0-b5eff289f2bf.png)

図のように「ジョイスティック読み取りの結果は5ミリ秒，LiDARの読み取りは100ミリ秒ごとに最新の情報が送出されるが，ロボの制御周期は10ミリ秒で回したいと思っている」とします．ROSの枠組みの上ではこれを簡単に実装でき，制御入力の計算ノードは「ジョイスティックとLiDARの値はコールバックで最新のものを取得しつつ，10ミリ秒の毎ループではその時点の最新の情報を使う」という挙動をとります．こうすることで，ロボットの制御周期を保ちつつ様々なハードウェアを柔軟に扱えるのが嬉しいポイントです．


### 開発ツール群

本節ではROSを使ってロボット開発を行う上でよく利用する便利ツールを挙げます．詳細は必要に応じて調べるような，インデック的にお読みください．

- catkin : ノードを書いた際のビルドツールです．
- rviz : トピックの可視化に使います．LIDAR, IMU, カメラ画像, 環境地図など大抵のもの表示できて便利です．
- rosbag : トピックの記録に使います．飛び交うtopicを時刻情報含めて丸っと記録．実験するときはとりあえずこれで全ログ取っておきましょう．
- roslaunch : ノードを複数扱うときに，一括起動や起動時パラメータの設定に使います．実質必須の機能です．
- Gazebo : よくROSとセットになる物理シミュレータ．ROS 2になってから[名前や利権でバタバタ](https://qiita.com/porizou1/items/b97e7a5e282a6a749a7d)しています．


### エコシステム

統合されたドキュメントや大規模コミュニティもROSを使う嬉しさの一つです．ユーザが多く，その知見が集約されていることはツール選定の重要な要素足り得ます．

- [ROS Wiki (ROS.org)](http://wiki.ros.org/ja)
  - ROSのインストール、チュートリアル、各パッケージのドキュメントは原則ここに記載されています．とりあえずチュートリアルをやってみるのがおすすめです．

- [ROS Answers](https://answers.ros.org/questions/)
  - ROS専用stack overflowのようなサイトです．大抵のトラブルはここに知見が残されています．エラーメッセージでググると高確率でこの中の記事に辿り着くでしょう．



### 潤沢なライブラリ

ロボットの自律移動，アームのマニピュレーション，画像認識による知覚，点群処理など様々なライブラリやパッケージが利用可能です．ROSのノードが疎結合であるゆえに，世界中の研究者・開発者が公開している資産を簡単に再利用できる嬉しさがあります．巨人の肩に乗りましょう．


# 2+1 DoFロボットアームを動かす

前章ではROSの概説とROSを使う嬉しさについて述べました．本章では，ROS対応のオープンソースロボットアームである[BSL-Plotter](https://github.com/kim-xps12/bsl-plotter)の制御を通じて，実際にROSを使う体験をしていただきます．

## ロボットアームとは？

日本語版Wikipediaによると，以下のように述べられています（[元ページ](https://ja.wikipedia.org/wiki/%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E3%82%A2%E3%83%BC%E3%83%A0)）．

> ロボットアームは人間の腕に似た働きをするメカニカルアームの一種であり、通常はプログラミング可能である。ロボットアームが機械装置全体ということもあればもっと複雑なロボットの一部ということもある。（多関節ロボットのような）回転動作や橋渡しのための（直線的な）動作をするようにそういったマニピュレーターの接続部はジョイントでつながれている。[1][2]連鎖機構を形作っているとマニピュレーターの接続部は見なせる。マニピュレーターの連鎖機構の終端部は効果器といわれ、人の手に似ている。（出典：Wikipedia）

要約すると，

- 人間の腕を模したロボット
- 形状や構成はいろいろ
- 手に相当する何かが先端に付いている

ということになります．

## 使用するロボットアーム

本稿で扱うロボットアームは，人間の肩と肘に相当する関節，および手先の機構としてペンの把持機構とそれを上げ下げする手首の関節を持っています．CADのスクショはこんな感じです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/4f6a7d93-3724-b7ab-e7e4-b7ec446d3f1f.png)

このロボットアームはメカのモデル，BOM，制御のソースコードの全てをMITライセンスで公開している，オープンソースのロボットアームです．私が「授業で2リンクのロボットアーム習ったきりで動かしたことないな」と思い立って設計・公開しました．

BSL-Plotter GitHubリンク: https://github.com/kim-xps12/bsl-plotter

ぜひ本稿ハンズオン以外でもご活用ください．

## 順運動学と逆運動学

ロボットアームを制御するために必要な概念として，恐らく最も基本的な考え方である「順運動学」「逆運動学」について紹介します．

### 順運動学(Forward Kinematics)

順運動学とは **「それぞれの関節の角度から，手先の位置や向きを求めるモデル」** を指します．イメージしやすいように書き下すと「1個めの関節が30deg, 2個めが45degだから，三角関数で計算していくと...手先は(x, y)=(3, 14)にあるね！」のような計算を意味しています．一番根本の関節から順番に三角関数を計算していけば良いので，基本的には解析解が求まります．

順運動学は基礎となる考え方であるためここで紹介していますが，この計算そのものは本稿の制御では使いません．実際に使われるのは次節の逆運動学の方です．


### 逆運動学(Inverse Kinematics)

逆運動学とは **「手先の位置や向きを実現するための，関節角度を求めるモデル」** を指します．こちらも書き下すと「手先を(x, y)=(3, 14)にするには…1個の関節めが30deg, 2個めが45degになっていれば良さそうだね！」という内容を求める計算を意味します．ロボットアームの構造によっては，解析解が求まらないことも多々あり，その場合は数値計算で解くことになります．

今回使用するBSL-PLotterの構造であれば解析解を得ることができ，ロボットアームの構造を次図のように定めるとき，


![スクリーンショット 2024-06-03 4.27.25.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1490597/6aec0394-4d9c-8085-ed50-a5f2b49dc3d2.png)

次式で与えられます．

```math
\begin{align}

\theta_1 &= \arctan\left(\frac{y}{x}\right) - \arccos\left(\frac{{L_1}^2-{L_2}^2+(x^2+y^2)}{2L_1L_2}\right) \\

\theta_2 &= \pi - \arccos\left(\frac{{L_1}^2+{L_2}^2-(x^2+y^2)}{2L_1L_2}\right)

\end{align} 
```

ここで， 
- $L_1$: $j_1$〜$j_2$の間の長さ
- $L_2$: $j_2$〜$p$の間の長さ
- $\theta_1$: $x$軸と$j_1$-$j_2$を結ぶ直線が成す角度
- $\theta_2$: $j_1$-$j_2$を結ぶ直線と，$j_2$-$p$を結ぶ直線が成す角度

とします．（j3はペンの上げ下げに用いる関節であるため一旦無視しています）

実際に手先を所望の位置へ動かす制御のためには，この逆運動学を計算すれば良いことになります．


導出などの詳しい解説は @Ninagawa123 さんの記事が詳しいです．気になる方はぜひご覧ください（[リンク](https://qiita.com/Ninagawa123/items/4ae058d819de1d5b698a)）．

## ロボットアームを制御してみる

### 事前準備

事前準備として，[READMEのSoftwareの節以降](https://github.com/kim-xps12/bsl-plotter?tab=readme-ov-file#software)の手順を参考に開発環境を構築してください．ホストOSとしてUbuntu 20.04を，その上でdockerとdocker composeを利用できることを想定しています．

ROSの環境構築について詳細な手順を読んでみたい方は，[公式wikiのROS noeticのインストール](https://wiki.ros.org/Installation/Ubuntu)をご参照ください．

Apple Silicon系のMacをお使いの方は，こちらの記事でUbuntu仮想マシンの構築を紹介しているのでぜひご利用ください．
[Apple Silicon MacでもROSでgazeboとUSBデバイスを全部使いたい！](https://qiita.com/B-SKY-Lab/items/c519e5bc9a37b3805fd0)

### 演習1 水平な線を1回描く

#### 制御に用いる式

逆運動学を用いて直線を1回だけ描くことを考えます．逆運動学の入力は，手先の**目標位置**，すなわち点の座標を与えます．これを用いて線を描くにはどうすれば良いでしょうか？

解き方は色々ありますが，今回は微小に動かした点を一定時間ごとに指定します．手先の目標位置 $p$を 時間の関数 $p(t)$とする，つまり$(x, y)$をそれぞれ媒介変数 $t$ で表せば良いことになります．

```math
\begin{align}

x(t) &= {\rm hogehoge} \\
y(t) &= {\rm piyopiyo}

\end{align}
```

水平な線を引くなら，$x$成分を適当に増やし$y$成分は定数にしておけば良いので，例えば
```math
\begin{align}

x(t) &= at + b \\
y(t) &= c 

\end{align}
```

となります．ただし$a, b, cは$適当な定数です．

#### 実装する

これを実装してロボットアームを動かしてみましょう． `catkin_ws/src/plotter_controller/src/lesson.py`の `target_line(t)`へ実装することでロボットの制御に利用することができます．$a, b, c$は以下のようにしてください．

```math
\begin{align}

x(t) &= 20t + 200 \\
y(t) &= 50 

\end{align}
```
また，可動範囲を超えて動作することを防ぐために，以下の条件を満たすように実装してください．

```math
\begin{align}

0 \leq t \leq 7 &: x(t) = 20t + 200 \\
t > 7 &: x(t) = 340 

\end{align}
```

ファイルの編集には以下の方法があります．いずれもdockerコンテナ内で実行してください．

- vimを使う場合
    ```bash
    vim ~/catkin_ws/src/plotter_controller/src/lesson.py
    ```
- nanoを使う場合
    ```bash
    sudo apt install nano
    nano ~/catkin_ws/src/plotter_controller/src/lesson.py
    ```

ちなみに，想定解は以下のコードです
```python
def target_line(t):
	if(t<=7):
		x = 20*t + 200
	else: 
		x = 340
	y = 50
	return x, y
```


#### 書いた制御を実行する

前節の内容をコーディングできたら，いよいよ実行してみましょう．本稿ではrvizを用いた可視化で動作を確認します．

まずはdockerコンテナの内側でtmuxを起動します

```bash
tmux
```

tmuxが起動したら`[ctrl+b] ＋%`でペインを縦分割して増やします（[参考](https://qiita.com/nmrmsys/items/03f97f5eabec18a3a18b)）．

片方のペインで以下を実行し，rvizを起動してロボットアームを描画します．
```bash
roslaunch bsl_plotter_description display.launch
```

`[ctrl+b]+矢印`で隣のペインに移動します．
以下のコマンドを実行し，先ほど書いた制御を実行します．
```bash
rosrun plotter_controller lesson.py
```

ロボットアームが動いて，横に腕が伸びていく様子が見られれば成功です！

終了するときは，rvizは[x]ボタン，terminalで実行中の制御は[ctrl+c]または[ctrl+\]で停止できます．


### 演習2 水平な線を往復して描く

制御に用いる式のうち，$x$成分を一次式で増加＆そのまま打ち止めではなく，三角関数で揺らすことで「水平な線を往復して描き続ける」動作を実現できます．具体的には，以下のように変更します．

```math
\begin{align}

x(t) &= 100 \sin\left( \frac{2\pi}{5}t \right) + 200 \\
y(t) &= 50 

\end{align}
```

この式であれば，三角関数の振動の範囲を可動域の内側に収めているためif文での制限も不要です．以下のように実装すればよいことになります．

```python
def target_line(t):
	x = 100*np.sin( (2*np.pi/5)*t )+200
	y = 50
	return x, y
```

実行手順の大部分は前節と同様であるため省略します．少しスクロールを戻して読み返し，ぜひ水平線を繰り返し描き続ける様子を眺めてください．


### 演習3 キー操作で線を描く

演習1, 2を通して，ロボットアームを制御してみる感覚はなんとなく体験していただけたかと思います．最後に，ROS公式チュートリアルの内容を参考にして，「キー操作で線を描く」を実装してみましょう．進め方は以下の流れを推奨しています．

1. [ROSチュートリアル](https://wiki.ros.org/ja/ROS/Tutorials/WritingPublisherSubscriber%28python%29)を見て、文字列のpub/subを動かす
1. Publisher: キーボードから入力した結果を送出するように修正
1. Subscriber: 受信したメッセージを基にアームを動かすように修正

これの回答は読んだ方への宿題とします（時間ができたら回答を載せたブランチを公開します，しばらくお待ちください🙏）

# おわりに

いかがでしたか？本稿ではROSの概説と簡易なハンズオンをご紹介しました．読んだ方のROS導入の一助になれれば幸いです．

それでは．

# 参考

[1] ROS wiki イントロダクション（ http://wiki.ros.org/ja/ROS/Introduction ）