ZeusCarにRaspberry PiとROS2を搭載してPCから制御する方法

2025年1月31日

URLをコピーしました！

本記事では、Raspberry Pi (Ubuntu Server ) と PC (Ubuntu Desktop) でROS 2 Humble を使用してロボットを制御するシステムの構築手順をまとめます。

環境

PC（ホスト側）
OS: Ubuntu Desktop 22.04
役割: ROS 2 Publisher ノードの実行、指令の送信
接続方法: 同一LAN内での通信

Raspberry Pi 4
モデル: Raspberry Pi 4B (または 5)
OS: Ubuntu Server 22.04
役割: ROS 2 Subscriber ノードの実行、シリアル通信を介した Arduino 制御
接続方法: LAN接続

Arduino
モデル: Arduino Uno R3
役割: Raspberry Pi からのシリアル通信を受け取り、モーター駆動指令を実行
接続方法: USB接続

ベースとなるメカナムホイールロボットZeusCarについては以下の記事で詳細を解説しています。

Murasan Lab

Arduinoで制御するスマートカー ZeusCarをレビュー | Murasan Lab ZeusCarは、SunFounder社が販売するArduino Uno R3互換ボードで制御するスマートカーです。このロボットカーは4WD全方向性駆動が可能なメカナムホイールを装着しています。…

ROS2のセットアップについては以下の記事で詳細を解説しています。あらかじめ済ませておいてください。

Murasan Lab

Ubuntu ServerでROS2 Humbleをインストールする手順を解説 | Murasan Lab 今回はUbuntu ServerでROS2 Humbleをインストールする手順について解説します。手順自体は簡単なステップで完了しますが、環境によってはインストール時にエラーが発生す…

Raspberry PiとPCを用いたROS 2 を使ったロボット制御手順

Python パッケージ zeuscar_robot_package を作成し、Publisher ノード (PC側) でコマンドを送信し、Subscriber ノード (Raspberry Pi 側) でそのコマンドを受信して Arduino 経由でロボットを制御します。

1. ワークスペースとパッケージの作成

今回はzeuscar_ros2_wsという名前のワークスペースを作成して進めます。

PC と Raspberry Pi どちらも同じ手順です。

# ワークスペース作成
mkdir -p ~/zeuscar_ros2_ws/src
cd ~/zeuscar_ros2_ws
source /opt/ros/humble/setup.bash

# ROS 2 パッケージの作成
cd src
ros2 pkg create zeuscar_robot_package --build-type ament_python

このコマンドで以下のフォルダ構成が作成されます。

zeuscar_ros2_ws/
├── src/
│   └── zeuscar_robot_package/
│       ├── package.xml
│       ├── setup.cfg
│       ├── setup.py
│       └── zeuscar_robot_package/
│           ├── __init__.py
│           └── ...

エラー対策: もし PermissionError: [Errno 13] Permission denied が発生した場合は以下を実行し、権限を変更してください。

sudo chown -R $USER:$USER ~/zeuscar_ros2_ws

2. コードの配置

src/zeuscar_robot_package/zeuscar_robot_package/ ディレクトリ内に以下の Python スクリプトを作成します。

Publisherノード (PC側) のコード: `publisher.py`

ホストPC側のディレクトリに以下の以下のコードをpublisher.pyとして配置します。

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

COMMANDS_MAP = {
    0: "FORWARD",
    1: "BACKWARD",
    2: "LEFT",
    3: "RIGHT",
    4: "LEFTFORWARD",
    5: "RIGHTFORWARD",
    6: "LEFTBACKWARD",
    7: "RIGHTBACKWARD",
    8: "TURNLEFT",
    9: "TURNRIGHT",
    10: "STOP"
}

class MinimalPublisher(Node):
    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        self.get_logger().info('MinimalPublisher node has been started.')

    def publish_command(self, command_str: str):
        msg = String()
        msg.data = command_str
        self.publisher_.publish(msg)
        self.get_logger().info(f'Publishing command: "{command_str}"')

def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = MinimalPublisher()

    try:
        while rclpy.ok():
            rclpy.spin_once(minimal_publisher, timeout_sec=0.1)
            user_input = input("Enter command number (0=FORWARD, 1=BACKWARD, etc.) or 'exit': ").strip()
            if user_input.lower() == 'exit':
                print("Exiting command input loop.")
                break

            if user_input.isdigit():
                cmd_num = int(user_input)
                if cmd_num in COMMANDS_MAP:
                    command_str = COMMANDS_MAP[cmd_num]
                    minimal_publisher.publish_command(command_str)
                else:
                    print("Invalid number. Please enter a value between 0 and 10.")
            else:
                print("Invalid input. Please enter a number (0–10) or 'exit'.")
    except KeyboardInterrupt:
        pass
    finally:
        minimal_publisher.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Subscriberノード (Raspberry Pi 側) のコード: `subscriber.py`

Raspberry Pi 4側のディレクトリにsubscriber.pyという名前で以下のコードを作成して保存します。

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import serial
import time

class MinimalSubscriber(Node):
    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(String, 'topic', self.listener_callback, 10)
        serial_port = '/dev/ttyACM0'
        baud_rate = 9600

        try:
            self.ser = serial.Serial(serial_port, baud_rate)
            time.sleep(2)
            self.get_logger().info(f"Serial connection established on {serial_port} at {baud_rate} bps.")
        except Exception as e:
            self.get_logger().error(f"Failed to open serial port: {e}")
            self.ser = None

    def listener_callback(self, msg):
        self.get_logger().info(f"I heard: '{msg.data}'")
        if self.ser is not None and self.ser.is_open:
            try:
                self.ser.write(msg.data.encode())
                self.get_logger().info(f"Sent to Arduino: {msg.data}")
                time.sleep(0.1)
                if self.ser.in_waiting > 0:
                    received_data = self.ser.readline().decode('utf-8', errors='ignore').rstrip()
                    self.get_logger().info(f"Received from Arduino: {received_data}")
            except Exception as e:
                self.get_logger().error(f"Serial write/read error: {e}")
        else:
            self.get_logger().warning("Serial port is not open. Unable to send command.")

def main(args=None):
    rclpy.init(args=args)
    minimal_subscriber = MinimalSubscriber()
    rclpy.spin(minimal_subscriber)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

3. `setup.py` の修正

zeuscar_robot_package/setup.py を以下のように修正します。

ホストPC側とRaspberry Pi側でそれぞれ同様の作業を行ってください。

from setuptools import setup
package_name = 'zeuscar_robot_package'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='YourName',
    maintainer_email='your_email@example.com',
    description='Example package for robot publisher & subscriber',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'publisher_node = zeuscar_robot_package.publisher:main',
            'subscriber_node = zeuscar_robot_package.subscriber:main',
        ],
    },
)

publisher_node：publisher.py の main() をエントリポイントとして登録
subscriber_node：subscriber.py の main() をエントリポイントとして登録

4. ビルドとセットアップ

ワークスペースディレクトリに戻り、ビルドを実行します。

ホストPC側とRaspberry Pi側でそれぞれ同様の作業を行ってください。

cd ~/zeuscar_ros2_ws
source /opt/ros/humble/setup.bash
colcon build
source install/setup.bash

5. 実行手順

実際にプログラムを実行します。

まず順番が前後しますが次項のArduino側のプログラムを作成し、転送を行っておいてください。

Arduino側のプログラムが起動したらUbuntuのプログラムを実行していきます。

Subscriber ノードの起動 (Raspberry Pi 側)

Raspberry Pi側で以下のコマンドを入力し、サブスクライバーノードを実行します。

source /opt/ros/humble/setup.bash
source ~/zeuscar_ros2_ws/install/setup.bash
ros2 run zeuscar_robot_package subscriber_node

Publisher ノードの起動 (PC 側)

ホストPC側で以下のコマンドを入力し、パブリッシャーノードを実行します。

source /opt/ros/humble/setup.bash
source ~/zeuscar_ros2_ws/install/setup.bash
ros2 run zeuscar_robot_package publisher_node

Publisher側で 0 ~ 10 を入力 → Arduino側のロボットが対応する動作を実行
exit を入力するとプログラム終了

6. 注意点

ネットワーク設定
- PCとRaspberry Piが同じLAN内で通信できるように設定してください。
- 必要に応じて export ROS_DOMAIN_ID=<同一ID> を両方の端末で設定します。
シリアルポートの確認
- Arduinoの接続ポート (/dev/ttyACM0) が異なる場合は subscriber.py 内の serial_port を変更してください。
ボーレートの一致
- Pythonコードの 9600 とArduino側の Serial.begin(9600); は一致させてください。

Arduino

ZeusCarのコントローラとして搭載されているArduino側のプログラムを実装します。

USBケーブルを接続しArduino IDEでコンパイル後に書き込みを行ってください。

/*******************************************************************
 * basic_move
 
  Control the direction and speed of motors rotation by pwm,
  to make the car go forward, backward, left turn, right turn and stop.

******************************************************************/
#include <Arduino.h>
#include <SoftPWM.h>

// 各動作に対応する定数の定義
#define FORWARD        "FORWARD"
#define BACKWARD       "BACKWARD"
#define LEFT           "LEFT"
#define RIGHT          "RIGHT"
#define LEFTFORWARD    "LEFTFORWARD"
#define RIGHTFORWARD   "RIGHTFORWARD"
#define LEFTBACKWARD   "LEFTBACKWARD"
#define RIGHTBACKWARD  "RIGHTBACKWARD"
#define TURNLEFT       "TURNLEFT"
#define TURNRIGHT      "TURNRIGHT"
#define STOP           "STOP"

/*
 *  [0]--|||--[1]
 *   |         |
 *   |         |
 *   |         |
 *   |         |
 *  [3]-------[2]
 */
/** Set the pins for the motors */
//モーターピンの設定
#define MOTOR_PINS \
  (uint8_t[8]) { \
    3, 4, 5, 6, A3, A2, A1, A0 \
  }
/** Set the positive and negative directions for the motors */
//モーターの正逆の方向を設定
#define MOTOR_DIRECTIONS \
  (uint8_t[4]) { \
    1, 0, 0, 1 \
  }

#define MOTOR_POWER_MIN 28  // 最小モーターパワー28/255

//PWM初期化処理のプロトタイプ宣言
void carBegin();

int8_t power = 80;

void setup() {
  Serial.begin(9600);  // シリアル通信の開始
  Serial.println("Zeus Car basic move");
  SoftPWMBegin();  //ソフトウェアPWMの初期化
  carBegin();      // モーターの初期化
}

void loop() {
  if (Serial.available() > 0) {
    //Raspberry Piから指令値を受信（改行単位）
    String receivedStr = Serial.readStringUntil('\n');
    Serial.flush();  // 受信バッファをクリア

    // デバッグ用の出力
    Serial.print("Received: ");
    Serial.println(receivedStr);

    // 文字列で比較してコマンドを処理
    if (receivedStr == FORWARD) {
      carForward(power);
    } else if (receivedStr == BACKWARD) {
      carBackward(power);
    } else if (receivedStr == LEFT) {
      carLeft(power);
    } else if (receivedStr == RIGHT) {
      carRight(power);
    } else if (receivedStr == LEFTFORWARD) {
      carLeftForward(power);
    } else if (receivedStr == RIGHTFORWARD) {
      carRightForward(power);
    } else if (receivedStr == LEFTBACKWARD) {
      carLeftBackward(power);
    } else if (receivedStr == RIGHTBACKWARD) {
      carRightBackward(power);
    } else if (receivedStr == TURNLEFT) {
      carTurnLeft(power);
    } else if (receivedStr == TURNRIGHT) {
      carTurnRight(power);
    } else if (receivedStr == STOP) {
      carStop();
    } else {
      Serial.println("Unknown command");
    }

    // 実行中のコマンドを送信（デバッグ用）
    Serial.print("Executing command: ");
    Serial.println(receivedStr);
    
    delay(100);  // 短めの待機時間を設定
  }
}

//PWM初期化処理
void carBegin() {
  for (uint8_t i = 0; i < 8; i++) {
    SoftPWMSet(MOTOR_PINS[i], 0);
    SoftPWMSetFadeTime(MOTOR_PINS[i], 100, 100);
  }
}

//PWM出力処理
/*
引数：各モータの出力値　power0,  power1, power2, power3
　　　+値：正転
　　　-値：逆転
*/
void carSetMotors(int8_t power0, int8_t power1, int8_t power2, int8_t power3) {
  bool dir[4];
  int8_t power[4] = { power0, power1, power2, power3 };
  int8_t newPower[4];

  for (uint8_t i = 0; i < 4; i++) {
    dir[i] = power[i] > 0;

    if (MOTOR_DIRECTIONS[i]) dir[i] = !dir[i];

    if (power[i] == 0) {
      newPower[i] = 0;
    } else {
      newPower[i] = map(abs(power[i]), 0, 100, MOTOR_POWER_MIN, 255);
    }
    SoftPWMSet(MOTOR_PINS[i * 2], dir[i] * newPower[i]);
    SoftPWMSet(MOTOR_PINS[i * 2 + 1], !dir[i] * newPower[i]);
  }
}

void carForward(int8_t power) {
  carSetMotors(power, power, power, power);
}

void carBackward(int8_t power) {
  carSetMotors(-power, -power, -power, -power);
}

void carLeft(int8_t power) {
  carSetMotors(-power, power, -power, power);
}

void carRight(int8_t power) {
  carSetMotors(power, -power, power, -power);
}

void carLeftForward(int8_t power) {
  carSetMotors(0, power, 0, power);
}

void carLeftBackward(int8_t power) {
  carSetMotors(-power, 0, -power, 0);
}

void carRightForward(int8_t power) {
  carSetMotors(power, 0, power, 0);
}

void carRightBackward(int8_t power) {
  carSetMotors(0, -power, 0, -power);
}

void carTurnLeft(int8_t power) {
  carSetMotors(-power, power, power, -power);
}

void carTurnRight(int8_t power) {
  carSetMotors(power, -power, -power, power);
}

void carStop() {
  carSetMotors(0, 0, 0, 0);
}