1/48シャアザクロボット化(2)

#はじめに
例の友達より「上半身全部できたから動かしてくれ」との依頼を受けたのでとりあえずROSのGUIを使って動かせるようにしました。
友達曰くプレステのコントローラから動かしたいそうなので、ティーチングプレイバックでボタンごとにモーションを割り当てようと思います。
でもモーションを逆運動学とかで作るのは10自由度だとさすがに面倒。なのでROSを使ってGUIでサーボモータを動かせるようにしてコマ送りのポーズを取らせその時のサーボモータへの入力を記録していこうと思います。

#環境
OS:Ubuntu16.04 LTS
ROS:kinetic
#ハードウェア
正直友達の工作スピードには驚かされる笑　所要日数3日笑
![20180518_230051.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/b30316a7-fb15-1500-613a-2c05a354e520.jpeg)
![20180518_230103.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/0c7822b1-790b-a345-3757-15783f0046a3.jpeg)
![20180518_230107.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/48dd8013-0bc9-1fcb-dcad-0ed5f5d22caf.jpeg)
![20180518_230112.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/438b80a4-388c-5e74-ca3e-5868763f54f6.jpeg)
![20180518_230121.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/d4879191-b871-0a8f-b48e-b1900d7e8b91.jpeg)
![20180518_230126.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/2df0b562-8cd9-87dc-3e7e-238d04732ec4.jpeg)
![20180518_230150.jpg](https://qiita-image-store.s3.amazonaws.com/0/239543/aa6b60c1-55d0-7170-7f9a-9de07c64856a.jpeg)
サーボモータは上半身だけで10個ついてます。電源は安定化電源を使用しました。6V以下で電流がある程度流せるなら何でもいいと思います。配線は割愛します。

#rosserial arduino
まず、arduinoとrosを結びつけるrosserialを使います。インストールは下記のコマンドで

```
 sudo apt-get install ros-kinetic-rosserial-arduino
 sudo apt-get install ros-kinetic-rosserial
 cd $ARDUINO HOME$/libraries
 rosrun rosserial_arduino make_libraries.py
```
この操作後にarduino IDE上のスケッチ例にroslibがあればオッケー。これでArduinoでROSのトピックを購読できます。

#GUI
GUIでトピックをパブリッシュしたかったのでrqt_ez_publisher(http://wiki.ros.org/rqt_ez_publisher )を使いました。(自分で作るのが面倒だった)
インストールは下記のコマンドで行いました。

```
sudo apt-get install ros-kinetic-rqt-ez-publisher
```

これで

```
rosrun rqt_ez_publisher rqt_ez_publisher
```
とうてばGUIが表示されるはず。あとはGUI上で配信したいトピックを選ぶだけでオッケー。

#Arduino
今回動かすサーボモータは10個、Arduino UNOのPWM対応ピンは6個なのでサーボドライバーPCA9685を使用しました。ライブラリはhttps://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library.git を使いました。Arduinoのスケッチは以下のとおりです。

```test_motion.ino
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

int LED=0;
int HEAD=1; 
int LBUST=2;
int RBUST=3;
int LSH=4;
int RSH=5;
int LSH2=6; 
int RSH2=7;
int LROLL=8;
int RROLL=9;
int WAIST=10;


ros::NodeHandle  nh;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

int BUFF=500/20000.0*4096.0;

void messageCb( const std_msgs::UInt16MultiArray& msg){
  pwm.setPWM(LED, 0, msg.data[0]/20000.0*4096.0 );
  pwm.setPWM(HEAD, 0, msg.data[1]/20000.0*4096.0 );
  pwm.setPWM(LBUST, BUFF, msg.data[2]/20000.0*4096.0+BUFF );
  pwm.setPWM(RBUST, BUFF, msg.data[3]/20000.0*4096.0+BUFF );
  pwm.setPWM(LSH, 2*BUFF, msg.data[4]/20000.0*4096.0+2*BUFF );
  pwm.setPWM(RSH, 2*BUFF, msg.data[5]/20000.0*4096.0+2*BUFF );
  pwm.setPWM(LSH2, 3*BUFF, msg.data[6]/20000.0*4096.0+3*BUFF );
  pwm.setPWM(RSH2, 3*BUFF, msg.data[7]/20000.0*4096.0+3*BUFF );
  pwm.setPWM(LROLL, 4*BUFF, msg.data[8]/20000.0*4096.0+4*BUFF );
  pwm.setPWM(RROLL, 4*BUFF, msg.data[9]/20000.0*4096.0+4*BUFF );
  pwm.setPWM(WAIST, 0, msg.data[10]/20000.0*4096.0 );
}

ros::Subscriber<std_msgs::UInt16MultiArray> sub("pwm", messageCb );


void setup()
{
  pwm.begin();
  pwm.setPWMFreq(50);
  nh.initNode();
  nh.subscribe(sub);
  Wire.setClock(400000);
}

void loop()
{
  nh.spinOnce();
}
```
pca9685を使えばPWM信号を16個出せるので、欲張って頭部のLEDにもPWM出力しました。また、すべてのピンでPWMのhighになるタイミングが同じだと瞬間的に大きな電流が必要になる可能性があるのでそのタイミングをずらしてます。
これをArduinoに書き込んだあと以下のコマンドを実行します。

```
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0
```
さらに上記のGUIを起動してトピックを配信すればできます。ただいきなり全部配線して安定化電源の電源をONすると各モータがあらぬ方向を向くのでちゃんと一つ一つ確認しながやるのがよし。

#実践
動画はこんな感じです。
https://www.youtube.com/watch?v=lErnkvDOla0
友達のチャンネルです。なんか変な編集入ってますがYoutubeやっていきたいらしいんでチャンネル登録してあげてください。

#感想
*やる前からわかってたけど腕周りのサーボモータのトルク足りない。
*最近は何するにもインストールするだけやから楽。
*友達の大胆な工作に脱帽。
*制御とか運動方程式とか考えなくていいから楽。
総括：やっぱり工作は楽しい。