LoginSignup
3
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@take4eng

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

Last updated at Posted at 2021-01-11

概要

この記事はRaspberyPi , Arduino , ROS を使ってラジコンを作る！！のソフトウェア制作編です。
全体の概要や流れに関しては、本編をご覧ください。

ROS を用いたラジコン作成：本編 (全体まとめ)
ROS を用いたラジコン作成：要件定義編
ROS を用いたラジコン作成：ROS環境構築編
ROS を用いたラジコン作成：Arduino制御編
ROS を用いたラジコン作成：コントローラー制御編
ROS を用いたラジコン作成：ハードウェア設計編
ROS を用いたラジコン作成：ソフトウェア設計編
ROS を用いたラジコン作成：ソフトウェア制作編 ← 今ここ
ROS を用いたラジコン作成：ハードウェア制作編

作成すべき内容はもう分かっているので、この記事では**『作成手順』や『コード内容の紹介』が主な内容**になります。
S__24641539.jpg

今回やること

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

今回できるもの

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

ソフトウェア制作

前回のソフトウェア設計編より、制御内容と作成すればいいプログラム内容を把握することができました。
今回は**『設計した内容に合わせてプログラムを作成』**していきます。

設計書まとめ

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

ROSプログラム概要図
ROS-CAR-ノード関係図.png
フローチャート (ros_car_node)
ROS-CAR-制御フローチャート1.png
フローチャート (Arduino)
ROS-CAR-制御フローチャート2.png
ピンアサイン (Arduino out)
ROS-CAR-ピンアサイン2.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)』**に相当するものになります。

~/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>

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

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』の値を持っています。
ここでは、コントローラーの状態を適当な値に変換し、先ほど宣言した変数に値を代入しています。

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トピック』をパブリッシュしていること
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』をビルドリストに加えているだけですね。

~/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』
~/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制御編』に記載していますが、『Arduinoプログラム』と『ROSプログラム』を混ぜ合わせたような記述方法になっています。

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つのコールバック関数を用意しました。

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){
  // トピックを受け取ったときの処理
}

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

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)

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

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

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

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

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

ソースのコンパイル

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

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

動作確認

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

Arduinoにプログラム書き込み

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

各パーツの接続

これまでの設計で出てきたように、各パーツのコネクタやピンを接続していきます。
下記のイメージ図を見れば、特に間違えることなく接続は完了するかと思います。
ros_car_ブレッドボード.png

roslaunch の実行

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

roslaunch ros_car ros_car.launch

コントローラーでの制御

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

以下の動画は、コントローラーで制御している様子を撮影したものです。
コントローラーの操作に合わせて、『LED』『サーボモーター』『DCモーター』が動作していることが分かります。

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

まとめ

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

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

3
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
3
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?