Turtlebot3(ROS1)をAR Markerでナビゲーションしてみた

## はじめに

[株式会社Rist](https://www.rist.co.jp/)のロボットチームの原山です。

Ristは画像認識AIの会社ですが、その他にも色々とやっております。
その中にロボット事業もあり、私はその一員です。

## この記事について
会社では検証機として、Turtlebot3 BurgerとWaffle Piを買ってもらいました。
今回はWaffle Piの方を使用したデモ開発について書いていこうと思います。

## 私について
2019年10月にRistに入社。

元々はWebプログラマーを長くやっていました。
LAMPで仕事をすることが多かった[Laravel](https://laravel.com/)とか[Vue.js](https://vuejs.org/)が好きなプログラマでした。
Ristでは、Flaskを使用したアプリに携わったりしていて、本当にWebの人間がROSに挑戦してみた話でもあります。


## どんなデモ？
製造業の工場内の検査を想定したデモを開発します。
走行ロボットが工場内を想定し社内を巡回します。
アナログメーターの前で停止し、メーターの表示に異常が無いかをチェックします。

## このデモでやりたいこと
ARMarkerを利用した自律制御
Manipulatorの制御

## 環境
OS:Ubuntu 16.04
ROS:ROS1 Kinetic
 
## 実機
[Turtlebot 3 Waffle Pi](https://emanual.robotis.com/docs/en/platform/turtlebot3/hardware_setup/#assembly-manual)
[Open Manipulator](https://emanual.robotis.com/docs/en/platform/openmanipulator_x/assembly/#assembly-manual) 

## カスタマイズしました！
Turtlebot3 FriendsのCarrierを参考にカスタマイズを行いました。

違いはマニュピレーターの先端にグリッパーを付けず、変わりにメーターを撮影するためにWebカメラを取り付けています。

## ツール
[ArduinoIDE](https://www.arduino.cc/en/main/software)
[Dynamixel Wizard 2.0](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/)

## セットアップ
セットアップ等も[ここ](https://emanual.robotis.com/docs/en/platform/turtlebot3/pc_setup/#pc-setup)にそって、catkin_makeではなくcatkin buildを使います。

### Remote PCセットアップ

#### 1. スクリプト落としてきてシェル変更して実行します。
aptパッケージのインストールやワークスペースを作ってくれます。

* catkin buildをするので、install_ros_kinetic.shの67行目〜76行目いらないですね。
使わなかったので、コメントか削除してしまって問題ないと思います。

```
$ sudo apt-get update
$ sudo apt-get upgrade
$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/robotis_tools/master/install_ros_kinetic.sh && chmod 755 ./install_ros_kinetic.sh && bash ./install_ros_kinetic.sh
```
シェルの中身は[こちら](https://raw.githubusercontent.com/ROBOTIS-GIT/robotis_tools/master/install_ros_kinetic.sh)

#### 2. Turtlebot3を動かすためのパッケージをインストールします。

```
$ sudo apt-get install ros-kinetic-joy ros-kinetic-teleop-twist-joy ros-kinetic-teleop-twist-keyboard ros-kinetic-laser-proc ros-kinetic-rgbd-launch ros-kinetic-depthimage-to-laserscan ros-kinetic-rosserial-arduino ros-kinetic-rosserial-python ros-kinetic-rosserial-server ros-kinetic-rosserial-client ros-kinetic-rosserial-msgs ros-kinetic-amcl ros-kinetic-map-server ros-kinetic-move-base ros-kinetic-urdf ros-kinetic-xacro ros-kinetic-compressed-image-transport ros-kinetic-rqt-image-view ros-kinetic-gmapping ros-kinetic-navigation ros-kinetic-interactive-markers
```
* Turtlebot3リポジトリとTurtlebot3 Manipulatorリポジトリから

```
$ cd ~/catkin_ws/src/
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone -b kinetic-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_manipulation.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_manipulation_simulations.git
$ cd ~/catkin_ws && catkin build
```

## Turtlebot3のセットアップ
Turtlebot3はRaspberry pi3 + OpenCR + Open Manipulator
#### 1.Raspberry pi 3 B+

* [こちら](https://emanual.robotis.com/docs/en/platform/turtlebot3/raspberry_pi_3_setup/#raspberry-pi-3-setup)にそってOS imageをSDカードに焼きます。
    * Raspian用の[OS image](https://www.robotis.com/service/download.php?no=1738)をDownloadして、[Etcher.io](https://www.balena.io/etcher/)ソフトで焼いて設定を少ししているだけです。

* 起動後にIPの確認をしてください。

```
$ ifconfig
```

* IPの設定

```
$ vi ~/.bashrc
```

* 追加

```shell
  # REMOTE_PC_IP　RASPBERRY_PI_3_IPをそれそれ設定する
  
  export ROS_MASTER_URI=http://REMOTE_PC_IP:11311
  export ROS_HOSTNAME=RASPBERRY_PI_3_IP
```

* 反映実行する

```
source ~/.bashrc
```

ラズパイカメラとUSBカメラをTurtlebot3上で実行するためにクローンとビルド

* ラズパイカメラ

```
$ git clone https://github.com/UbiquityRobotics/raspicam_node
$ catkin build
```

* USBカメラ

```
$ git clone  https://github.com/ros-drivers/usb_cam.git
$ catkin build
```

#### 2. OpenCRへのプログラムアップロード 
ArduinoIDEを使用します。ない人はインストールをします。

* PCとOpenCRを繋いでください。

* ArduinoIDEを起動します。

* ファイル->環境設定->追加ボードマネージャーのURL
* 下記を追加
    
```
https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json
```

* ツール->ボードｰ>ボードマネージャーでOpenCRを選択で OpenCR by ROBOTISパッケージをインストール* 

* ツールｰ>シリアルポートｰ>/dev/ttyACM0を選択


#### 3.  モーターにIDをふる
ID振らなきゃいけないの知らなくて、長い間、マニュピレーターを動かせずにいましたね。反省。

* [マニュアル](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/)からインストールしてください。
* ファイルｰ>スッケッチ例ｰ>OpenCR->10.Etc->usb_to_dxlを選択
* 矢印ボタン押下でアップロードします。
* コンソールに"jump_to_fw"と出れば成功です。
* Dynamixel Wizard 2.0を起動すると左上のスキャンボタンを押下するとスキャン開始。
* wheel motorsにID:1とID:２を振ります。
* マニュピレーターは下からID:11、ID:12、ID:13、ID:14と振ります。
* Baud Rate 100000000(桁あってるかな。。。)に設定

#### 4. OpenCRへのTurtlbot Manipulatorプログラムをアップロード

* ファイルｰ>スッケッチ例ｰ>Turtlebot3->turtlebot3_with_open_manipulator->turtlebot3_with_manipulator_coreを選択

*  矢印ボタン押下でアップロードします。
    
* このやり方が走行用のモーターがおかしくなった時に役立ちます。




#### 5. 組み立てる
Raspberry pi3 + OpenCR + Manipulatorの完成形
組み立て方は、Robotis e-Manualから[Turtlebot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/hardware_setup/#assembly-manual)と[Open Manipulator](https://emanual.robotis.com/docs/en/platform/openmanipulator_x/assembly/#assembly-manual)がダウンロードできます。部品が細かくて大変、極力落とさないように、Lidar乗っていますが使ってないです。笑

![IMG_1292.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/24466/30bb96f5-8361-1ff8-58dc-7e32825f4296.jpeg)


## ARMarkerを使えるようにする
##### ar_track_usb_cam.launchをもとにar_track_rpicam_9.0cm.launchを作成"9.0"はマーカーの大きさです。

* ar_tag_toolboxをクローンして、ビルドしてください。

```
$ cd ~/catkin_ws/src
$ git clone https://github.com/atomoclast/ar_tag_toolbox.git
$ catkin build
```
~/catkin_ws/src内にオリジナルのパッケージを作成する。依存関係は良しなにrospyだけでいいと思います。

* ar_controllerって名前にします。

```
$ cd ~/catkin_ws/src
# 例 : catkin_create_pkg 任意のパッケージ名 std_msgs rospy
$ catkin_create_pkg ar_controller std_msgs rospy
```

* ar_tag_toolboxパッケージのar_track_usb_cam.launchをコピーする。<br>
    * ``` roscp コピー元パッケージ名 コピー元ファイル名 コピー先ファイルパス ```ですね。
    * ar_track_rpicam_9.0cm.launchを作る

```
$ roscp ar_tag_toolbox ar_track_usb_cam.launch ar_track_rpicam_9.0cm.launch
```

* ar_track_rpicam_9.0cm.launchの中

```xml

<launch>

    <!-- <arg name="marker_size" default="5.0" /> -->
    <arg name="marker_size" default="9.0" />
    <arg name="max_new_marker_error" default="0.08" />
    <arg name="max_track_error" default="0.2" />

    <!-- <arg name="cam_image_topic"      default="/usb_cam/image_raw" /> -->
    <!-- <arg name="cam_info_topic"       default="/usb_cam/camera_info" /> -->
    <arg name="cam_image_topic" default="/raspicam_node/image_raw" />
    <arg name="cam_info_topic" default="/raspicam_node/camera_info" />


    <!-- <arg name="output_frame" default="usb_cam" /> -->
    <arg name="output_frame" default="odom" />


    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
        <param name="marker_size"           type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
        <param name="output_frame"          type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>
    <!-- 固定のTF -->
    <node name="static_tf_pub_raspicam_frame" type="static_transform_publisher" pkg="tf2_ros" output="screen" args="0 0 0 0 0 0 1 /raspicam /odom" />
</launch>
```

* ar_controller.launch作成、ar_track_rpicam_9.0cm.launchをインクルードして、ar_controller.pyを実行しています。

```xml

<launch>
    <!-- 先程作成したランチファイルを実行する -->
    <include file="$(find rist_ar_controller)/launch/ar_track_rpicam_9.0cm.launch" />
    
    <!-- /raspicam_node/imageから/raspicam_node/image_rawにリパブリッシュ -->
    <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/raspicam_node/image raw out:=/raspicam_node/image_raw" />
    
    <!-- ar_controller.pyを実行 -->
    <node pkg="rist_ar_controller" type="ar_controller.py" name="ar_controller" output="screen" />
</launch>
```

* ar_controller.pyの中身<br>
カメラ内にマーカーが存在する場合に、下記のvisualization_markerトピックにPublishされるのでvisualization_markerトピックをSubscribeする。コールバック内でIDデータを取得して、指定マーカーIDを突き合わせることができます。

    * 指定マーカーID : [0,1,2,3] - 四ヶ所に移動するように指定できる

```python

# Subscribe
self.controller_sub = rospy.Subscriber(
                        'visualization_marker',
                        Marker,
                        self.callback_controller)

# callbackでマーカーIDだけ取得
def callback_controller(self, data):
    rospy.loginfo('(callback_controller)')
    self.tag = data
    self.marker_id = data.id
    rospy.loginfo('tag in viewport: ' + str(self.marker_id))
```
##### 上記でマーカーのIDが取れれば、指定マーカーへ動くようの/cmd_velトピックをいじって上げればマーカーに向うようなプログラム書けばいいです。

## Remote PCとTurtlebot3 PC上でランチファイルを実行し、ナビゲーションする

#### [Remote PC]
* ROSマスタ起動

```
$ roscore
```

##### [Turtlebot3 PC] - sshでログインしてランチファイル実行

```
$ ssh pi@192.168.X.XXX
```
* Turtlebot3 起動

```
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch
```

* ラズパイカメラ起動

```
$ roslaunch raspicam_node camerav2_1280x720.launch
```

* USBカメラ起動

```
$ roslaunch usb_cam usb-test.launch
```

#### [Remote PC]に戻る
* Turtlebot Manipulaterの起動

```
$ roslaunch turtlebot3_manipulation_bringup turtlebot3_manipulation_bringup.launch
```
* Turtlebot3 Manipulatorのmoveit_configパッケージのmove_group実行

```
roslaunch turtlebot3_manipulation_moveit_config move_group.launch
```

* ar_controller.pyを実行

```
roslaunch ar_controller ar_controller.launch
```

長い。。。[続く](https://qiita.com/kazuharayama/items/9d52a43e2c48079f5de2)。。。