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

#はじめに

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

###環境

OS	Ubuntu14.04
ROS	indigo
ArduinoIDE	1.8.5
Arduino	Arduino UNO

#インストール

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

バージョン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にコードの書き込みができないので,書き込み権限をつける.

ttyACM0の場合

sudo chmod 0666 /dev/ttyACM0

#ノードの構成

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

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

#回路

サーボを動かすだけなので回路構成は単純.TinkerCADを使用して簡単な図を示した.

#使用コード

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

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ノードは以下のコードを使う.

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)

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を個別のターミナルで実行.

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