OSOYOO メカナム４輪 ROS化

#４輪ロボットのROS化
[4輪ロボット](https://osoyoo.com/2019/11/08/metal-chassis-mecanum-wheel-robotic-for-arduino-mega2560-lesson1-robot-car-assembly/)をROS(cmd_vel)で操作したい。

通信は以下を採用。
ubuntu→(ssh)→raspi→(serial)→arduino→(gpio)→モータ

##前準備
###ubuntuPCにArduino開発環境を用意
以下の参考記事の通り
[ros_wiki](http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup)


###４輪ロボットのスペック確認
モーターのデータシート
https://osoyoo.com/picture/mecanum_wheel_robotic/GM25-370-motor%20specification.pdf
・ギア比1/75（今回は未使用）


## コーディング

### システム構成
基本的なロボットのシステム構成は以下のよう．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/452637/b11af5a7-1d14-910a-885f-d611b49c270d.png)
しかし今回は~~めんどくさかったので~~簡単のためにエンコーダ未使用です．
~~FBによる細かい速度の調整もめんどくさいので諦めました．~~

今回使用したコードはgithubに置いてあります．
https://github.com/nobunoby/Osoyoo4WheelROS

### 逆運動学
速度成分から各タイヤの回転速度への変換を以下の記事，論文を参考にコードを記述．
http://robotsforroboticists.com/drive-kinematics/
http://www2.lib.yamagata-u.ac.jp/you-campus/sangitan/kiyou-sangitan/11/p81-84.pdf

### 実装
Arduinoに以下のコードを書き込み

```c:wheel_cont.ino
#include <ros.h>
#include <geometry_msgs/Twist.h>

#define speedPinR 9   //  RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA 
#define RightMotorDirPin1  22    //Front Right Motor direction pin 1 to Front MODEL-X IN1  (K1)
#define RightMotorDirPin2  24   //Front Right Motor direction pin 2 to Front MODEL-X IN2   (K1)                                 
#define LeftMotorDirPin1  26    //Left front Motor direction pin 1 to Front MODEL-X IN3 (  K3)
#define LeftMotorDirPin2  28   //Left front Motor direction pin 2 to Front MODEL-X IN4 (  K3)
#define speedPinL 10   // Left WHEEL PWM pin D7 connect front MODEL-X ENB

#define speedPinRB 11   //  RIGHT WHEEL PWM pin connect Back MODEL-X ENA 
#define RightMotorDirPin1B  5    //Rear Right Motor direction pin 1 to Back MODEL-X IN1 (  K1)
#define RightMotorDirPin2B 6    //Rear Right Motor direction pin 2 to Back MODEL-X IN2 (  K1) 
#define LeftMotorDirPin1B 7    //Rear left Motor direction pin 1 to Back MODEL-X IN3  K3
#define LeftMotorDirPin2B 8  //Rear left Motor direction pin 2 to Back MODEL-X IN4  k3
#define speedPinLB 12    //   LEFT WHEEL  PWM pin D8 connect Rear MODEL-X ENB

#define WHEEL_RADIUS  0.04 // diameter 80mm
#define WHEEL_SEPARATION_WIDTH 0.107
#define WHEEL_SEPARATION_LENGTH 0.0465


ros::NodeHandle nh;

void callback(const geometry_msgs::Twist& vel)
{
  float lin_x, lin_y, ang_z;
  lin_x = vel.linear.x;
  lin_y = vel.linear.y;
  ang_z = vel.angular.z;

  int fl = (lin_x - lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z) / WHEEL_RADIUS / 10 * 250;
  int fr = (1/WHEEL_RADIUS) * (lin_x + lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
  int rl = (1/WHEEL_RADIUS) * (lin_x + lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
  int rr = (1/WHEEL_RADIUS) * (lin_x - lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;

  //nh.loginfo("receieved cmd_vel");
  FL(fl);
  FR(fr);
  RL(rl);
  RR(rr);
}

ros::Subscriber<geometry_msgs::Twist> sub("/ugv/cmd_vel", &callback);

void setup()
{
  init_GPIO();
  nh.initNode();
  nh.subscribe(sub);
}


void loop()
{
  nh.spinOnce();
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void wheel_vel(float lin_x, float lin_y, float ang_z)
{
  int fl = (lin_x - lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z) / WHEEL_RADIUS / 10 * 250;
  int fr = (1/WHEEL_RADIUS) * (lin_x + lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
  int rl = (1/WHEEL_RADIUS) * (lin_x + lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
  int rr = (1/WHEEL_RADIUS) * (lin_x - lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;

  FL(fl);
  FR(fr);
  RL(rl);
  RR(rr);
}

void FL(int fl)
{
  int speed;
  if (fl >= 0)  // rotate fw
  {
    digitalWrite(LeftMotorDirPin1,LOW);
    digitalWrite(LeftMotorDirPin2,HIGH);
    speed = fl;
  }
  else if (fl < 0)  // rotate bw
  {
    digitalWrite(LeftMotorDirPin1,HIGH);
    digitalWrite(LeftMotorDirPin2,LOW);
    speed = -1 * fl;
  }
  analogWrite(speedPinL,speed);
}

void FR(int fr)
{
  int speed;
  if (fr >= 0)  // rotate fw
  {
    digitalWrite(RightMotorDirPin1,LOW);
    digitalWrite(RightMotorDirPin2,HIGH);
    speed = fr;
  }
  else if (fr < 0)  // rotate bw
  {
    digitalWrite(RightMotorDirPin1,HIGH);
    digitalWrite(RightMotorDirPin2,LOW);
    speed = -1 * fr;
  }
  analogWrite(speedPinR,speed);
}

void RL(int rl)
{
  int speed;
  if (rl >= 0)  // rotate fw
  {
    digitalWrite(LeftMotorDirPin1B,LOW);
    digitalWrite(LeftMotorDirPin2B,HIGH);
    speed = rl;
  }
  else if (rl < 0)  // rotate bw
  {
    digitalWrite(LeftMotorDirPin1B,HIGH);
    digitalWrite(LeftMotorDirPin2B,LOW);
    speed = -1 * rl;
  }
  analogWrite(speedPinLB,speed);
}

void RR(int rr)
{
  int speed;
  if (rr >= 0)  // rotate fw
  {
    digitalWrite(RightMotorDirPin1B,LOW);
    digitalWrite(RightMotorDirPin2B,HIGH);
    speed = rr;
  }
  else if (rr < 0)  // rotate bw
  {
    digitalWrite(RightMotorDirPin1B,HIGH);
    digitalWrite(RightMotorDirPin2B,LOW);
    speed = -1 * rr;
  }
  analogWrite(speedPinRB,speed);
}

void brake()
{
  digitalWrite(LeftMotorDirPin1,LOW);
  digitalWrite(LeftMotorDirPin2,LOW);
  digitalWrite(RightMotorDirPin1,LOW);
  digitalWrite(RightMotorDirPin2,LOW);
  digitalWrite(LeftMotorDirPin1B,LOW);
  digitalWrite(LeftMotorDirPin2B,LOW);
  digitalWrite(RightMotorDirPin1B,LOW);
  digitalWrite(RightMotorDirPin2B,LOW);
}

 
 
void stop_Stop()    //Stop
{
  analogWrite(speedPinLB,0);
  analogWrite(speedPinRB,0);
  analogWrite(speedPinL,0);
  analogWrite(speedPinR,0);
}


//Pins initialize
void init_GPIO()
{
  pinMode(RightMotorDirPin1, OUTPUT); 
  pinMode(RightMotorDirPin2, OUTPUT); 
  pinMode(speedPinL, OUTPUT);  
 
  pinMode(LeftMotorDirPin1, OUTPUT);
  pinMode(LeftMotorDirPin2, OUTPUT); 
  pinMode(speedPinR, OUTPUT);
  
  pinMode(RightMotorDirPin1B, OUTPUT); 
  pinMode(RightMotorDirPin2B, OUTPUT); 
  pinMode(speedPinLB, OUTPUT);  
 
  pinMode(LeftMotorDirPin1B, OUTPUT);
  pinMode(LeftMotorDirPin2B, OUTPUT); 
  pinMode(speedPinRB, OUTPUT);
   
  stop_Stop();
}
```


## 実行
raspi↔arduino通信のためのパッケージをインストール

```shell:raspi
sudo apt-get install ros-melodic-rosserial -y
sudo apt-get install ros-melodic-rosserial-arduino -y
```

arduino接続してるシリアルポートを$ dmesg | grep tty で確認

```shell
ssh ubuntu@192.168.201.xxx
```

```shell:raspiターミナル1
export ROS_IP=192.168.201.xxx ##raspiのIP
roscore
```

```shell:raspiターミナル2
export ROS_IP=192.168.201.xxx ##raspiのIP
rosrun rosserial_python serial_node.py /dev/ttyACM0
```

```shell:pc
#pcのip
export ROS_IP=192.168.201.xxx
#raspiのip
export ROS_MASTER_URI=http://192.168.201.xxx:11311

rosrun joy joy_node &
rosrun teleop_twist_joy teleop_node cmd_vel:=ugv/cmd_vel
```

arduinoとraspiの通信の参考
http://wiki.ros.org/rosserial_arduino/Tutorials/Blink

ds3:
1. shareボタンを押す
2. xボタンを押しながらジョイスティック操作

うまくいくとこうなるはず
![a.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/452637/e434110c-48de-8965-e15d-38d8310e2751.gif)



###launchファイル

```xml:pc_launch.launch
<?xml version="1.0"?>
<launch>
<node name="joy" pkg="joy" type="joy_node" output="log">
  </node>
  <node name="joy_twist" pkg="teleop_twist_joy" type="teleop_node" output="log">
    <remap from="/cmd_vel" to="/ugv/cmd_vel
" />
  </node>
<node name="arduino" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM0" />

</launch>
```

本当はpcとraspiのlaunchファイルまとめたいけど，ちゃんとssh接続の設定してないのが仇となった．．．
[qiita記事1](https://dsp.logly.co.jp/click?ad=4AG4hpsDG-TXXPlbCopOY409jGIlset4KH3WrLXuQg4yLIK6IRAVtx790WigrWL6sZVET8El-fL6YZq5QULkuOJP-eHPVTR7VXHEsEpieJIQx7vaVRn4gOjroEvNyWViu4v_PCzGgaNEbhQ_BJH7OMrvHz6j9T1DbbX7D3SKwJAZkxwPnXkZbFNPR79DmaeXOriVH8DbfEfht5kInkmZc278Wm7ynNAZxaBWo32y0p8ON-SjjiLQkPJLoxhCw3BM_NvFcekLE35-_Jx6dGXPP7_Sr0aJw1WIB5F2TEcy0pjr4MKu8K40OWWdwvpJXgsJ35gF2I6jHGH1zzlQuMe2cUW42sNTay9YZuUO_-V39IQuuuP-tkSVuHbcMkjc0wDS0N9K6iq1nvJPIzlEw_cJqzLChxacHuk5Kc7WV3EqnvvhQkNz2T0V7WIcdmKOpLTDMRWxav6Y0SqE_r8IN6_u83rdisUzt3TXOTaO3DcHRp0oDY3vmYiDOaj11pDUFvcxMn1T77yaDY-DPYmBtKDx0T9LISH5RvyhTM7Ip_PXYAMkLsaap4HtzubGPpcOMbAXKyh6zlsOuQXmU-xDG2zqB4hRnTFZWdnUQkswqqJTaV0)

参考：http://wiki.ros.org/roslaunch/Tutorials/Roslaunch%20tips%20for%20larger%20projects

###トラブルシュート
・pcのipはあってるか
・ps3のshareボタンは押したか
・Arduinoの電源接続してるか
どちらでもない
→[ros wiki tutorial](http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick)を参考に接続からやり直す


##シミュレーション（執筆途中）
URDFモデルは[ここ](https://github.com/RBinsonB/nexus_4wd_mecanum_simulator)から取得して利用

http://blog.robotakao.jp/blog-entry-447.html

###参考

https://jp.mathworks.com/solutions/power-electronics-control/motor-modeling.html

https://jp.mathworks.com/solutions/power-electronics-control/motor-control.html

https://www.researchgate.net/publication/305449950_Speed_Control_of_a_DC_Motor_with_Variable_Load_Using_Sliding_Mode_Control


https://qiita.com/NaokiAkai/items/4d0b18c0d71976d3fec5


一応使わなかったが記載
エンコーダのサンプルコード
https://osoyoo.com/2019/11/08/how-to-test-motor-encoder-with-arduino/

https://github.com/ros-controls/ros_controllers/tree/melodic-devel/four_wheel_steering_controller
https://qiita.com/MoriKen/items/78b0ad8c1eae257646dd