Python ROS2 プログラム 高専4年生

【ROS2講座⑨】実機を使ったロボット制御の準備【DOGZILLA】【Python】

前回までで、ROS2の基礎的な理解は終わりました。

いよいよ今回からは実機を使ってロボット制御をしてゆきます。

今回は環境構築を行います。

使用するロボットDOGZILLA（ドッグジラ）とは

DOGZILLAは、12自由度を持つAIロボット犬です。
12個のサーボモータで体を動かし、実際の犬のように歩いたり、体をひねったりすることができます。

本体はアルミ合金製のフレームでできており、丈夫で安全。
さらに、**広角カメラやIMU（姿勢センサー）**を搭載しており、ロボット自身の動きを正確に把握することができます。

主な特徴

  • 本物の犬のように歩行・旋回動作が可能。
  • 6つの高精度サーボモータと軽量アルミボディを採用。
  • 広角カメラを使って周囲の環境を認識。
  • **Raspberry Pi（ラズベリーパイ）**をメインコントローラとして使用。
  • ROS2 HumbleとPythonによるプログラミングに対応。
  • RVizシミュレーションで3D上の動きを確認可能。
  • スマホアプリ、コントローラ、Web操作など、多様な遠隔操作方法をサポート。

DOGZILLAは、AI（人工知能）技術とセンサー情報を組み合わせることで、さまざまな機能を実現します。

機能カテゴリ内容
画像認識カメラでラベル認識、顔検出、ターゲット追跡、ライン走行が可能。
姿勢制御IMU（加速度・ジャイロセンサー）でロボットの傾きや角度を検出。
LiDAR（ライダー）レーザーで周囲をスキャンし、マップ作成や障害物回避が可能（S2モデル）。
音声制御音声モジュールにより、音声で指示を出すことが可能（S2モデル）。

公式ドキュメントはこちらにあります。

英語ですが、こちらで十分にROS2の勉強をすることができます。

https://www.yahboom.net/study/DOGZILLA

VNC接続の設定

このロボットはRaspberry Pi 5が搭載されておりますが、ディスプレイなどは付属しておらず、直接GUIの操作ができません。

手持ちのWindowsPCからこのロボットのRaspberry Pi5に接続します。

まずはパソコンとロボットを同じネットワーク上に繋ぎます。

一番簡単な方法はWindows PC上でモバイルホットスポット（≒テザリング）を立てる方法です。

Windowsの検索機能でモバイル ホットスポットと検索してください。モバイルホットスポットのシステム設定が開きます。

編集をクリックします。

ネットワーク名とパスワードを任意の物に設定します。

他の人と被らないようにしてください。必ず実習後にはパスワードを変えるようにしてください。

ホットスポットをオンにします。

他、スマホ等のデバイスでWi-Fiスポットが立っていることを確認出来たら完了です。

次にロボットをこのネットワークにつなぎます。

接続ができると、IPアドレスが表示されます。

続いてPC側でリモートアクセスするためのソフトウェアをダウンロードします。

realvnc.com/en/connect/download/viewer/

ダウンロードができたら画面の指示に従ってインストールします。

起動します。

ログインが求められますが、Cancelしても問題ありません。

上部にIPアドレスを入力し、エンターを押します。

Continueをクリックします。

ロボットのパスワードはデフォルトで以下のようになっています。入力し、OKを押します。

接続できれば成功です。

VSCodeの導入

開発をしやすいようにRaspberry Pi 5にVSCodeを導入します。

ターミナルで以下のコマンドを実行します。

sudo apt update
sudo apt install code

ファイル→プログラミング→VSCodeで起動できます。

起動したら拡張機能から日本語化します。

Change Language and Restartをクリックし再起動すると日本語化されます。

このロボットはRaspberry Pi 5内のDockerコンテナでROS2 Humbleを起動するようになっています。

SSH接続したPCからそのコンテナ内にアクセスできるように設定してゆきます。

拡張機能をクリックし、Dev Containersで検索し、インストールします。

表示→ターミナルで以下のコマンドを実行し、コンテナを起動します。

./run_humble.sh

次にコマンドパレットを表示します。

Dev Containers: Attach to Running Container…を入力、選択します。

すると現在Dockerで実行されているコンテナの候補が表示されます。

起動時にランダムに名称が割り振られるため、以下の文字と違うかもしれません。

1つしか表示されていないと思いますのでそちらをクリックして選択します。

フォルダーを開くでそのままroot/フォルダを選択しましょう。

左のツリーにyahboomcer_wsが見えれば成功です。

これで様々な開発を進めてゆけます。

歩行テスト

いよいよ動かしていきます。

1つ目のターミナルで以下のコマンドを実行します。

センサ/状態ノードを起動（LiDAR、IMU、関節状態などのブリングアップ）します。

ros2 launch bringup Navigation_bringup.launch.py

2つ目のターミナルで以下を実行します。

ros2 launch yahboom_gait yahboomGaitLaunch.launch.py gait:=trot mark:=0

gait:=trot が歩様タイプ指定。mark はモード用の追加パラメータ（公式手順どおり0で起動）。

続いてキーボード入力で速度topicを出力するプログラムを実行します。

ros2 run teleop_twist_keyboard teleop_twist_keyboard

このターミナルをアクティブにした状態でキー入力を行うとロボットが歩き出します。

主なキー操作：
i 前進 / , 後退 / j その場左旋回 / l その場右旋回 / k 停止（出力ゼロ）

Dockerイメージの固定

このままでは起動時に毎回Dockerコンテナが初期化されます。

以下の固定コンテナ作成スクリプトを作成し、再起動しても続きから作業できるようにします。

以下は最初の一回だけ実行し、コンテナを作成するスクリプトです。

home/pi/create_dogzilla_container_localws.sh

#!/usr/bin/env bash
set -euo pipefail

NAME="dogzilla_ros2"
IMG="yahboomtechnology/ros-humble:3.6"
DISPLAY_VAL="${DISPLAY:-:0}"   # HDMI表示なら :0
XSOCK="/tmp/.X11-unix"

# 既存なら終了
if docker ps -a --format '{{.Names}}' | grep -qx "$NAME"; then
  echo "[INFO] $NAME already exists"; exit 0
fi

# 動的に --device を組み立て（存在するものだけ付与）
DEV_ARGS=()
for d in /dev/ttyAMA1 /dev/ttyAMA0 /dev/video0 /dev/myspeech; do
  [ -e "$d" ] && DEV_ARGS+=( --device="$d" )
done
# /dev/input はディレクトリなので --device ではなくボリュームで共有（必要時のみ）
[ -d /dev/input ] && DEV_ARGS+=( -v /dev/input:/dev/input:ro )

# xhost はホスト側で起動中のX向け。無くても進める
command -v xhost >/dev/null 2>&1 && xhost +local:root || true

docker create \
  --name "$NAME" \
  --net=host \
  --restart unless-stopped \
  -e DISPLAY="$DISPLAY_VAL" \
  -e QT_X11_NO_MITSHM=1 \
  -v "$XSOCK:/tmp/.X11-unix:rw" \
  "${DEV_ARGS[@]}" \
  "$IMG" \
  bash -lc 'sleep infinity'

echo "[OK] created $NAME"

保存出来たら以下のコマンドで実行します。

chmod +x create_dogzilla_container_localws.sh
./create_dogzilla_container_localws.sh

同様に以下のファイルを作成します。

以下が2回目以降の起動スクリプトです。

home/pi/start_dogzilla_container.sh

#!/usr/bin/env bash
set -euo pipefail
NAME="dogzilla_ros2"
export DISPLAY="${DISPLAY:-:0}"

docker start "$NAME" >/dev/null || true

# 起動完了待ち
for i in {1..20}; do
  s=$(docker inspect -f '{{.State.Status}}' "$NAME" 2>/dev/null || echo unknown)
  [ "$s" = "running" ] && break
  sleep 0.5
done

# 入る
exec docker exec -it -e DISPLAY="$DISPLAY" "$NAME" bash -lc '
  source /opt/ros/humble/setup.bash || true
  mkdir -p /root/ros2_ws && cd /root/ros2_ws
  echo "[Inside] DISPLAY=${DISPLAY}"
  bash
'

以下でコンテナを実行できます。

chmod +x start_dogzilla_container.sh
./start_dogzilla_container.sh

再起動時は上記のコマンドの実行で同じコンテナを開くことができます。

次にコマンドパレットを表示します。

Dev Containers: Attach to Running Container…を入力、選択します。

すると現在Dockerで実行されているコンテナが表示されますので、選択してアタッチします。

新しくVSCodeが立ち上がります。

オリジナルプログラムの作成

それではロボットを制御するための自作プログラムを作ってみましょう。

ワークスペースを作成します。

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

パッケージを作成します。

ros2 pkg create motion_control_pkg \
  --build-type ament_python \
  --license Apache-2.0 \
  --dependencies rclpy geometry_msgs

以下のようにファイルが生成されます。

/root/ros2_ws/src/motion_control_pkg/
├─ package.xml
├─ setup.py
├─ setup.cfg
├─ resource/motion_control_pkg
└─ motion_control_pkg/__init__.py

それでは以下のファイルパスにファイルを作成します。

/root/ros2_ws/src/motion_control_pkg/motion_control_pkg/forward_once.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class ForwardOnce(Node):
    def __init__(self):
        super().__init__('forward_once')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.timer = self.create_timer(0.05, self.tick)  # 20Hz
        self.count = 0

    def tick(self):
        msg = Twist()
        msg.linear.x = 0.08  # 前進速度[m/s]
        self.pub.publish(msg)
        self.count += 1
        if self.count >= 60:  # 約3秒(0.05×60)
            stop = Twist()
            self.pub.publish(stop)
            self.get_logger().info('Done: moved forward for 3 seconds.')
            self.timer.cancel()
            rclpy.shutdown()

def main():
    rclpy.init()
    node = ForwardOnce()
    rclpy.spin(node)

if __name__ == '__main__':
    main()

package.xmlを書き換えます。

/root/ros2_ws/src/motion_control_pkg/package.xml

<?xml version="1.0"?>
<package format="3">
  <name>motion_control_pkg</name>
  <version>0.0.0</version>
  <description>Motion nodes</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_python</buildtool_depend>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

続いてSetup.pyも書き換えます。

/root/ros2_ws/src/motion_control_pkg/setup.py

from setuptools import setup

package_name = 'motion_control_pkg'

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='root',
    maintainer_email='root@todo.todo',
    description='Motion nodes',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'forward_once = motion_control_pkg.forward_once:main',
        ],
    },
)

すべて保存したらビルドをしてゆきます。

cd /root/ros2_ws
colcon build --symlink-install --packages-select motion_control_pkg

トピックを受け取り、ロボットを動かすためのノードを立ち上げます。

source /root/ros2_ws/install/setup.bash
ros2 launch bringup Navigation_bringup.launch.py

今回作成したノードを別ターミナルで立ち上げます。

source /root/ros2_ws/install/setup.bash
ros2 run motion_control_pkg forward_once

3秒歩いて止まれば成功です。

課題13

ロボットに速度指令 /cmd_vel を送り、円運動をさせる ROS2 ノードを自作します。

  • linear.x：進む速度（m/s）
  • angular.z：角速度（rad/s）
    → この2つを同時に与えることで、ロボットは円を描きます。

円運動ノード circle_motion.py を作成せよ

以下の動作を満たす ROS2 ノードを motion_control_pkg の中に追加せよ：

  • /cmd_vel に速度指令を送る
  • linear.x > 0 と angular.z ≠ 0 を同時に出す
  • ある時間動作後に、停止する

直径約30㎝の円を描き、1週したら停止するようにしてください。

ファイルパス：

/root/ros2_ws/src/motion_control_pkg/motion_control_pkg/circle_motion.py
解答例

以下のプログラムをベースに調整してみてください。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class CircleMotion(Node):
    def __init__(self):
        super().__init__('circle_motion')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)

        self.declare_parameter('linear', 0.1)   # m/s
        self.declare_parameter('angular', 0.3)  # rad/s
        self.declare_parameter('duration', 10.0) # seconds

        self.linear = self.get_parameter('linear').value
        self.angular = self.get_parameter('angular').value
        self.duration = self.get_parameter('duration').value

        self.timer = self.create_timer(0.05, self.tick)
        self.elapsed = 0.0

    def tick(self):
        twist = Twist()
        twist.linear.x = self.linear
        twist.angular.z = self.angular
        self.pub.publish(twist)

        self.elapsed += 0.05
        if self.elapsed >= self.duration:
            self.pub.publish(Twist())
            self.get_logger().info('Circle motion done.')
            rclpy.shutdown()

def main():
    rclpy.init()
    node = CircleMotion()
    rclpy.spin(node)

if __name__ == '__main__':
    main()

setpu.pyには以下を追記します。

'circle_motion = motion_control_pkg.circle_motion:main',

以下で実行します。

ros2 run motion_control_pkg circle_motion

パラメータは以下のようにも調整できます。

ros2 run motion_control_pkg circle_motion --ros-args -p linear:=0.05 -p angular:=0.5

次回はこちら

Comming soon

Follow me!

-Python, ROS2, プログラム, 高専4年生
-, , , ,

PAGE TOP