ROS を用いたラジコン作成：ソフトウェア制作編

# 概要
この記事は[RaspberyPi , Arduino , ROS を使ってラジコンを作る！！](https://qiita.com/take4eng/items/0b21c4927d42b3d7cd62)のソフトウェア制作編です。
全体の概要や流れに関しては、本編をご覧ください。

[ROS を用いたラジコン作成：本編 (全体まとめ)](https://qiita.com/take4eng/items/0b21c4927d42b3d7cd62)
[ROS を用いたラジコン作成：要件定義編](https://qiita.com/take4eng/items/916fbbc68e2f7e34e384)
[ROS を用いたラジコン作成：ROS環境構築編](https://qiita.com/take4eng/items/70f167320ede46e4139c)
[ROS を用いたラジコン作成：Arduino制御編](https://qiita.com/take4eng/items/527793c9a1f902a0a62c)
[ROS を用いたラジコン作成：コントローラー制御編](https://qiita.com/take4eng/items/e067628971fa171d6d5a)
[ROS を用いたラジコン作成：ハードウェア設計編](https://qiita.com/take4eng/items/271924b986e072378439)
[ROS を用いたラジコン作成：ソフトウェア設計編](https://qiita.com/take4eng/items/1d5007ab623c3f7d3ad4)
[ROS を用いたラジコン作成：ソフトウェア制作編](https://qiita.com/take4eng/items/99df06b0e28d7c601632) **← 今ここ**
[ROS を用いたラジコン作成：ハードウェア制作編](https://qiita.com/take4eng/items/21d34d5bf56f99aba54e)


作成すべき内容はもう分かっているので、この記事では**『作成手順』や『コード内容の紹介』が主な内容**になります。
![S__24641539.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/634805/b0d59ec3-4226-c14b-6730-acd771a44b9d.jpeg)

## 今回やること

- **設計通りにプログラムを作成する**
- **設計で足りない点は適宜補いながら記述する**
- **仕様通り動作するかの確認**

## 今回できるもの
GitHubに今回の制作プログラムを置いています。
この記事の中にも全て記載しますが、こちらからまとめて見ることもできます。
https://github.com/kakuto-Takeshi/ros_car

# ソフトウェア制作
前回の[ソフトウェア設計編](https://qiita.com/take4eng/items/1d5007ab623c3f7d3ad4)より、制御内容と作成すればいいプログラム内容を把握することができました。
今回は**『設計した内容に合わせてプログラムを作成』**していきます。

## 設計書まとめ
これまでに作成した設計図をここにまとめておきます。
コーディングの際に必要となる内容が記載されているはずです。

**ROSプログラム概要図**
![ROS-CAR-ノード関係図.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/634805/9e58ed30-d3a3-093e-29a7-dc98e651d853.png)
**フローチャート (ros_car_node)**
![ROS-CAR-制御フローチャート1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/634805/ad1a466b-8112-2012-40ea-e5951e6f4b6b.png)
**フローチャート (Arduino)**
![ROS-CAR-制御フローチャート2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/634805/71069fda-febf-558b-76b6-badfc0d514c6.png)
**ピンアサイン (Arduino out)**
![ROS-CAR-ピンアサイン2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/634805/94e0ed1d-ba97-09fa-8524-2b99696f9c0d.png)

## ROSパッケージの作成
まずは、ROSパッケージを作成します。
上記の**『ROSプログラム概要図』**を満たすように作成します。

1. joy：既存パッケージ
2. **ros_cra：これから作成する**
3. Arduino：パッケージ無し

```
cd ~/catkin_ws/src
catkin_create_pkg ros_car roscpp rospy std_msgs
```

## ROSソースの作成
ROSのプログラムソースは、各パッケージ内の『/src ディレクトリ内』に作成します。
今回作成するものは、上の設計図で言う**『フローチャート (ros_car_node)』**に相当するものになります。

```cpp:~/catkin_ws/src/ros_car/src/ros_car.cpp
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/Joy.h>

std_msgs::Int32 led_msg;
std_msgs::Int32 servo_msg;
std_msgs::Int32 motor_msg;

void joy_callback(const sensor_msgs::Joy &joy_msg)
{
  led_msg.data = joy_msg.buttons[0];
  servo_msg.data = -joy_msg.axes[2] * 25 + 90;

  if (joy_msg.axes[1] > 0.5)
    motor_msg.data = 1;
  else if (joy_msg.axes[1] < -0.5)
    motor_msg.data = -1;
  else
    motor_msg.data = 0;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "ros_car_node");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("joy", 10, joy_callback);
  ros::Publisher pub_led = nh.advertise<std_msgs::Int32>("led", 10);
  ros::Publisher pub_servo = nh.advertise<std_msgs::Int32>("servo", 10);
  ros::Publisher pub_motor = nh.advertise<std_msgs::Int32>("motor", 10);
  ros::Rate loop_rate(30);

  while (ros::ok())
  {
    pub_led.publish(led_msg);
    pub_servo.publish(servo_msg);
    pub_motor.publish(motor_msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```
たいして難しいコードでもないのですが、せっかくなので簡単にだけ解説を入れておきます。

**ヘッダーのインクルード**
`ros.h`に加えて、このソース内で扱うトピックの型である`std_msgs/Int32.h`と`sensor_msgs/Joy.h`もインクルードしておきます。

```
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/Joy.h>
```

**各データの変数宣言**
送信するトピックのデータを保持する変数を用意しておきます。
これらはメイン関数とコールバック関数の両方で使用するため、グローバル変数として定義しておきます。

```cpp
std_msgs::Int32 led_msg;
std_msgs::Int32 servo_msg;
std_msgs::Int32 motor_msg;
```

**コールバック関数**
コントローラーの操作情報を受信したときに実行するコールバック関数を記述します。
引数の`joy_msg`は`button[]`と`axis[]`に値を保有しており、`button[]`は『0 or 1』、`axis[]`は『-1~1』の値を持っています。
ここでは、コントローラーの状態を適当な値に変換し、先ほど宣言した変数に値を代入しています。

```cpp
void joy_callback(const sensor_msgs::Joy &joy_msg)
{
  led_msg.data = joy_msg.buttons[0];
  servo_msg.data = -joy_msg.axes[2] * 25 + 90;

  if (joy_msg.axes[1] > 0.5)
    motor_msg.data = 1;
  else if (joy_msg.axes[1] < -0.5)
    motor_msg.data = -1;
  else
    motor_msg.data = 0;
}
```

**メイン関数**
本プログラムのメイン関数です。
通常のROSプログラムのメイン関数と同じ内容ですね。

ここでの処理で重要な点は、以下の2つです。
1. コントローラの『joyトピック』をサブスクライブしていること
2. whileループで『ledトピック』『servoトピック』『motorトピック』をパブリッシュしていること

```cpp
int main(int argc, char **argv)
{
  ros::init(argc, argv, "ros_car_node");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("joy", 10, joy_callback);
  ros::Publisher pub_led = nh.advertise<std_msgs::Int32>("led", 10);
  ros::Publisher pub_servo = nh.advertise<std_msgs::Int32>("servo", 10);
  ros::Publisher pub_motor = nh.advertise<std_msgs::Int32>("motor", 10);
  ros::Rate loop_rate(30);

  while (ros::ok())
  {
    pub_led.publish(led_msg);
    pub_servo.publish(servo_msg);
    pub_motor.publish(motor_msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

## CMakeLists.txt の編集
作成したソースをビルドするため、**『CMakeLists.txt』**の編集を行います。
こちらは通常のROSの使用方法と何も変わらないので、コード内容だけ記載しておきます。

やっていることは、先ほど作成した『ros_car.cpp』をビルドリストに加えているだけですね。

```cmake:~/catkin_ws/src/ros_car/CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(ros_car)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES ros_car
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(ros_car_node src/ros_car.cpp)

target_link_libraries(ros_car_node
  ${catkin_LIBRARIES}
)

```

## launchファイルの作成
実行しなければならないノードが何個かあるので、launchファイルを作成しておきます。
実行するものは、以下の3つです。

1. joyパッケージの『joy_node』
2. rosserialクライアントアプリケーション (ポート名、通信速度)
3. ros_carパッケージの『ros_car_node』


```xml:~/catkin_ws/src/ros_car/launch/ros_car.launch
<launch>
  <node name="joy_node" pkg="joy" type="joy_node" />
  <node name="serial_node" pkg="rosserial_python" type="serial_node.py">
    <param name="port" value="/dev/ttyACM0" />
    <param name="baud" value="115200" />
  </node>
  <node name="ros_car_node" pkg="ros_car" type="ros_car_node" />
</launch>

```

## パッケージのビルド
続いて、パッケージのビルドを行います。
特にエラー無くビルドが完了すればOKです。

```
cd ~/catkin_ws
catkin_make
```

## Arduinoプログラムの作成
続いて『Arduino に組み込むプログラム』を作成していきます。
これにはノード名や所属パッケージが無いので、適当な場所に保存しておきます。
ファイル名は『car_serial.ino』としておきました。

今回作成するものは、上の設計図で言う**『フローチャート (Arduino)』及び『ピンアサイン (Arduino out)』**に相当するものになります。

コードの書き方や詳細は[『Arduino制御編』](https://qiita.com/take4eng/items/527793c9a1f902a0a62c)に記載していますが、**『Arduinoプログラム』と『ROSプログラム』を混ぜ合わせたような記述方法**になっています。

```cpp:car_serial.ino
#include <Servo.h>
#include <ros.h>
#include <std_msgs/Int32.h>

#define REAR_LED_L 8
#define REAR_LED_R 9
#define FRONT_LED_L 10
#define FRONT_LED_R 11
#define SERVO_PIN 6
#define MOTOR_SIG1 3
#define MOTOR_SIG2 4
#define ENABLE 5

Servo myServo;
ros::NodeHandle nh;
bool ledControlleFlag = true;

void messageCb_led(const std_msgs::Int32 &led_msg){
  if(led_msg.data){
    ledControlleFlag=false;
    digitalWrite(REAR_LED_L, HIGH);
    digitalWrite(REAR_LED_R, HIGH);
    digitalWrite(FRONT_LED_L, HIGH);
    digitalWrite(FRONT_LED_R, HIGH);
  }else{
    ledControlleFlag=true;
    digitalWrite(REAR_LED_L, LOW);
    digitalWrite(REAR_LED_R, LOW);
    digitalWrite(FRONT_LED_L, LOW);
    digitalWrite(FRONT_LED_R, LOW);
  }
}

void messageCb_servo(const std_msgs::Int32 &servo_msg){
  if(65<=servo_msg.data && servo_msg.data<=115){
    myServo.write(servo_msg.data);

    if(ledControlleFlag){
      if(servo_msg.data<90) {
        digitalWrite(REAR_LED_L, HIGH);
        digitalWrite(REAR_LED_R, LOW); 
      }else if(90<servo_msg.data) {
        digitalWrite(REAR_LED_R, HIGH);
        digitalWrite(REAR_LED_L, LOW); 
      }else {
        digitalWrite(REAR_LED_L, LOW); 
        digitalWrite(REAR_LED_R, LOW);
      }
    }
  }
}

void messageCb_motor(const std_msgs::Int32 &motor_msg){
  if(motor_msg.data == 1){
    // 前進
    digitalWrite(MOTOR_SIG1, HIGH);
    digitalWrite(MOTOR_SIG2, LOW);
    digitalWrite(ENABLE, HIGH);
    // 前LED点灯
    if(ledControlleFlag){
      digitalWrite(FRONT_LED_L, HIGH);
      digitalWrite(FRONT_LED_R, HIGH);
    }
  }else if(motor_msg.data == -1){
    // 後退
    digitalWrite(MOTOR_SIG1, LOW);
    digitalWrite(MOTOR_SIG2, HIGH);
    digitalWrite(ENABLE, HIGH);
    // 前LED消灯
    if(ledControlleFlag){
      digitalWrite(FRONT_LED_L, LOW);
      digitalWrite(FRONT_LED_R, LOW);
    }
    
  }else{
    // ストップ
    digitalWrite(MOTOR_SIG1, LOW);
    digitalWrite(MOTOR_SIG2, LOW);
    digitalWrite(ENABLE, LOW);
    // 前LED消灯
    if(ledControlleFlag){
      digitalWrite(FRONT_LED_L, LOW);
      digitalWrite(FRONT_LED_R, LOW);
    }
  }
}

// サブスクライバー
ros::Subscriber<std_msgs::Int32> sub_led("led", &messageCb_led);
ros::Subscriber<std_msgs::Int32> sub_servo("servo", &messageCb_servo);
ros::Subscriber<std_msgs::Int32> sub_motor("motor", &messageCb_motor);

void setup() {
  // LED
  pinMode(REAR_LED_L,  OUTPUT);
  pinMode(REAR_LED_R,  OUTPUT);
  pinMode(FRONT_LED_L,  OUTPUT);
  pinMode(FRONT_LED_R,  OUTPUT);

  // サーボ
  myServo.attach(SERVO_PIN);
  myServo.write(90);

  // DCモーター
  pinMode(MOTOR_SIG1, OUTPUT);
  pinMode(MOTOR_SIG2, OUTPUT);
  pinMode(ENABLE, OUTPUT);

  // ROS初期設定
  nh.getHardware()->setBaud(115200);
  nh.initNode();

  // サブスクライブ実行
  nh.subscribe(sub_led);
  nh.subscribe(sub_servo);
  nh.subscribe(sub_motor);
}

void loop() {
  nh.spinOnce();
  delay(1);
}
```

全部説明すると長くなってしまうので、重要なところだけ掻い摘んで説明します。

**各種コールバック関数**
それぞれのトピックを受信したときに実行する処理を記述しておきます。
今回は『LED』『サーボモーター』『DCモーター』に関する3つのコールバック関数を用意しました。

```cpp
void messageCb_led(const std_msgs::Int32 &led_msg){
  // トピックを受け取ったときの処理
}

void messageCb_servo(const std_msgs::Int32 &servo_msg){
  // トピックを受け取ったときの処理
}

void messageCb_motor(const std_msgs::Int32 &motor_msg){
  // トピックを受け取ったときの処理
}
```

**サブスクライバーのインスタンス化**
サブスクライバーのインスタンスを作成します。
引数には『トピック名』と先ほど記述した『コールバック関数』を代入します。
これにより、トピック名とコールバック関数が紐付けられます。

```cpp
ros::Subscriber<std_msgs::Int32> sub_led("led", &messageCb_led);
ros::Subscriber<std_msgs::Int32> sub_servo("servo", &messageCb_servo);
ros::Subscriber<std_msgs::Int32> sub_motor("motor", &messageCb_motor);
```

**ROS初期設定**
`setup()`関数内でROSに関する初期設定を行います。
1行目で通信速度の設定をしており、早い方がいいかなと思ったので115200に設定しておきました。(設定を省略すると9600)

```cpp
nh.getHardware()->setBaud(115200);
nh.initNode();
```

**サブスクライブ実行**
この記述により、先ほどインスタンス化したサブスクライバーがメッセージの購読を開始します。
今回はサブスクライバーが3つあるので、それぞれのインスタンスに対して記述しています。

```cpp
nh.subscribe(sub_led);
nh.subscribe(sub_servo);
nh.subscribe(sub_motor);
```

**コールバック関数の実行**
`loop()`関数により、`nh.spinOnce()`関数が無限に繰り返されます。
`nh.spinOnce()`関数によりトピックメッセージがコールバック関数へ送られるため、実際はこのタイミングでコールバック関数が実行されます。

```cpp
void loop() {
  nh.spinOnce();
  delay(1);
}
```

## ソースのコンパイル
『Arduino IDE』より、先ほど作成したプログラムのコンパイルを行います。
特に説明することは無く、エラー無くコンパイルできればOKです。

以上で、ソフトウェア制作は完了しました！！

# 動作確認
プログラムの作成が完了したので、想定通りに制御ができるかの確認を行います。
実際の制作では、プログラム作成と動作確認を交互に行いながら微調整を繰り返しました。

## Arduinoにプログラム書き込み
先ほど作成した**『car_serial.ino』**を、Arduino に書き込みます。
コンパイル時にエラーが無いことを確認しているので、『Arduino IDE』を用いてプログラムを Arduino に書き込んでください。

## 各パーツの接続
これまでの設計で出てきたように、各パーツのコネクタやピンを接続していきます。
下記のイメージ図を見れば、特に間違えることなく接続は完了するかと思います。
![ros_car_ブレッドボード.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/634805/c39936ef-387d-e35d-30a3-2a32d0115e6b.png)

## roslaunch の実行
既にlaunchファイルを作成しているので、これを実行するだけでOKです。

```
roslaunch ros_car ros_car.launch
```

## コントローラーでの制御
上記のプログラムを実行した状態で、コントローラーを操作します。
プログラムの内容や各パーツの接続が間違っていなければ、仕様通りに制御ができるはずです。

以下の動画は、コントローラーで制御している様子を撮影したものです。
コントローラーの操作に合わせて、『LED』『サーボモーター』『DCモーター』が動作していることが分かります。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">自作ラジコン、ほぼできた！！<br>操作関係は大体オッケー🙆‍♂️<br>あとはモニターとか切り離せば動くハズ。<br><br>クッソ見栄え悪いけど気にしない。 <a href="https://t.co/GRfVtBzYas">pic.twitter.com/GRfVtBzYas</a></p>&mdash; かくとー🤖ロボット開発 (@take4eng) <a href="https://twitter.com/take4eng/status/1340626250654617606?ref_src=twsrc%5Etfw">December 20, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

※ 動画撮影は車体の作成が完了した後に行っています。([車体の作成はこちら](https://qiita.com/take4eng/items/21d34d5bf56f99aba54e))
　 サーボは前輪の向き、DCモーターは後輪の回転を見てもらえると分かると思います。

# まとめ
以上で、プログラムの作成および動作確認が完了しました！！

Arduinoとの連携準備や設計資料を作成しておけば、ここの工程は簡単な内容かと思います。
準備や設計をちゃんとしていれば・・・ね。