Arduinoを使いROSでサーボモータを動かす

#はじめに

ROSには,rosserial_arduinoというArduinoとシリアル通信をするためのパッケージが用意されている.
これを用いArduinoとROSを接続,ROS経由でArduinoを使用し,キーボード操作でサーボモータを動かそうと試みる.

###環境
|||
|:--:|:--:|
|OS|Ubuntu14.04|
|ROS|indigo|
|ArduinoIDE|1.8.5|
|Arduino|Arduino UNO|

#インストール

まずはROSにrosserialのパッケージをインストール.

```shell-session:バージョンindigoの場合
$ sudo apt-get install ros-indigo-rosserial-arduino
$ sudo apt-get install ros-indigo-rosserial
```

ArduinoIDEをインストールしていない場合は,これもインストールする.
下記サイトにてダウンロードして解凍する.
https://www.arduino.cc/en/main/software

任意のディレクトリ上で解凍

```
$ sudo tar Jxfv arduino-1.8.5-linux64.tar.xz
```

インストールしたIDEに**ros_lib**を追加する.

```
$ cd arduino-1.8.5/libraries/
$ rosrun rosserial_arduino make_libraries.py ./
```

#デバイスファイルに権限を付与する

PCとArduinoをUSBで接続すると,/dev/ttyACM*が生成される.
しかし,のままではArduinoにコードの書き込みができないので,書き込み権限をつける.

```shell-session:ttyACM0の場合
sudo chmod 0666 /dev/ttyACM0
```

#ノードの構成

今回は以下のようなノードの構成になる.

![servo_test.png](https://qiita-image-store.s3.amazonaws.com/0/215782/04df1622-9822-a933-2edf-a3c80f516f19.png)


 - `keyboard_driver`ノードにてキーボード入力を受け取り,/Keysトピックにキー入力を配信する.
 - `keys_to_servo`ノードにて特定のキーの場合,サーボの設定角度を/servoトピックに配信する.
 - `serial_node`ノードにてサーボの設定角度をシリアル通信でArduinoへ転送する.

#回路

サーボを動かすだけなので回路構成は単純.[TinkerCAD](https://www.tinkercad.com)を使用して簡単な図を示した.

![servo_test.png](https://qiita-image-store.s3.amazonaws.com/0/215782/26882a12-e8a2-5bed-9c19-24c8fc99ed06.png)




#使用コード

ArduinoにはIDEを使用して以下のコードを書き込む.

```shell-session:servo_test.ino
#if defined(ARDUINO) && ARDUINO >= 100
    #include "Arduino.h"
#else
    #include <WProgram.h>
#endif

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>

ros::NodeHandle nh;

Servo servo;

void servo_cb(const std_msgs::UInt16& cmd_msg){

    servo.write(cmd_msg.data);
    digitalWrite(13, HIGH-digitalRead(13));

}

ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);

void setup(){

    pinMode(13, OUTPUT);

    nh.initNode();
    nh.subscribe(sub);

    servo.attach(9);

}

void loop(){

    nh.spinOnce();
    delay(1);

}
```
`keyboard_driver`ノードと`keys_to_servo`ノードは以下のコードを使う.

```shell-session:keyboard_driver.py
#!/usr/bin/env python
import sys, select, tty, termios
import rospy
from std_msgs.msg import String

if __name__ == '__main__':

  #KeysトピックにString型のメッセージを毎秒一回送る
  key_pub = rospy.Publisher('keys', String, queue_size=1)
  #keyboard_driverノード作成
  rospy.init_node("keyboard_driver")
  rate = rospy.Rate(100)
  
  old_attr = termios.tcgetattr(sys.stdin)
  tty.setcbreak(sys.stdin.fileno())
  
  print "Publishing [z], [x] or [a]. Press Ctrl-C to exit..."

  while not rospy.is_shutdown():
    
    if select.select([sys.stdin], [], [], 0)[0] == [sys.stdin]:
      key_pub.publish(sys.stdin.read(1))
    rate.sleep()
    
  termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_attr)
```

```shell-session:keys_to_servo.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from std_msgs.msg import UInt16

key_mapping = { 'z': 0, 'x': 90, 'a': 180 }

def keys_cb(msg, twist_pub):

  if len(msg.data) == 0 or not key_mapping.has_key(msg.data):
    return # unknown key.

  vels = key_mapping[msg.data]

  t = UInt16()
  t = vels
  publisher.publish(t)

if __name__ == '__main__':
  #keys_to_servoノード作成
  rospy.init_node('keys_to_servo')
  #servoトピックにUInt16型のメッセージを送る
  publisher = rospy.Publisher('servo', UInt16, queue_size=1)
  rospy.Subscriber('keys', String, keys_cb, publisher)
  rospy.spin()
```
#実行

`roscore`,`rosrun`を個別のターミナルで実行.

```shell-session:ttyACM0の場合
$ roscore
$ rosrun rosserial_python serial_node.py /dev/ttyACM0
$ rosrun keyboard_driver keyboard_driver.py
$ rosrun keys_to_servo keys_to_servo.py
```

# 参考記事

ROSのrosserialを使ってArduinoでLチカをする
https://qiita.com/nnn112358/items/059487952eb3f9a5489b

ROS.org Servo Controller Example
http://wiki.ros.org/rosserial_arduino/Tutorials/Servo%20Controller