自作ノード

このレッスンでは、ROS2 Humble環境において、Pythonで簡単な自作ノードを作成し、移動ロボットを制御する基本的なプログラムの作り方を学びます。ノードは /kachaka/manual_control/cmd_vel トピックに対して geometry_msgs/Twist 型のメッセージを発行します。

1. プログラムの概要

  • 移動ロボットを簡単なフィードフォワード制御で制御します
  • 基本的な動作（前進、停止、回転）を順番に実行します

2. 前提条件

  • ROS2 Humble がインストール済み
  • Python3 が利用可能
  • ROS2ワークスペース（例: ~/ros2_ws）がセットアップ済み

必要なパッケージ

  • rclpy: ROS2のPythonクライアントライブラリ
  • geometry_msgs: 位置や速度などの幾何学メッセージを扱うためのパッケージ

3. パッケージの作成

まず、ROS2ワークスペースの src ディレクトリ内でパッケージを作成します。--node-name オプションを使用して、ノード名を指定します。

cd ./ros2_ws/src
ros2 pkg create --build-type ament_python kachaka_simple_control --dependencies rclpy geometry_msgs --node-name kachaka_simple_control

このコマンドにより、以下の構造のパッケージが生成されます：

kachaka_simple_control/
├── resource/
│   └── kachaka_simple_control
├── kachaka_simple_control/
│   ├── __init__.py
│   └── kachaka_simple_control.py
├── test/
├── package.xml
└── setup.py

4. ノードの実装

kachaka_simple_control パッケージ内の kachaka_simple_control ディレクトリに、以下の内容の kachaka_simple_control.py ファイルを作成します。

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from rclpy.duration import Duration

class KachakaSimpleControl(Node):
    def __init__(self):
        super().__init__('kachaka_simple_control')
        # Create a Publisher that sends Twist type messages to the /cmd_vel topic
        self.publisher = self.create_publisher(Twist, '/kachaka/manual_control/cmd_vel', 10)
        self.get_logger().info('Robot control node has started')

    def move_robot(self, linear_x, angular_z, duration):
        """
        Move the robot at specified speed and time
        
        Parameters:
            linear_x: Forward speed [m/s]
            angular_z: Rotation speed [rad/s]
            duration: Movement duration [seconds]
        """
        # Create velocity command message
        vel = Twist()
        vel.linear.x = linear_x
        vel.angular.z = angular_z
        
        # Calculate end time
        end_time = self.get_clock().now() + Duration(seconds=duration)
        
        # Continue publishing velocity commands until the specified time
        while self.get_clock().now() < end_time:
            self.publisher.publish(vel)
            rclpy.spin_once(self, timeout_sec=0.1)  # Wait for 0.1 seconds
        
        # Stop after the movement is complete
        self.stop_robot()
        self.get_logger().info(f'Movement completed: Forward={linear_x}, Rotation={angular_z}, Time={duration} seconds')
    
    def stop_robot(self):
        """Stop the robot"""
        vel = Twist()
        vel.linear.x = 0.0
        vel.angular.z = 0.0
        self.publisher.publish(vel)
        self.get_logger().info('Robot has stopped')

def main(args=None):
    rclpy.init(args=args)
    robot_control = KachakaSimpleControl()

    try:
        # Execute the following movement sequence
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop
        robot_control.move_robot(0.1, 0.0, 1.0)  # Forward
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop
        robot_control.move_robot(0.0, 0.5, 2.0)  # Rotate left
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop
        robot_control.move_robot(0.0, -0.5, 2.0)  # Rotate right
        robot_control.move_robot(0.0, 0.0, 1.0)  # Stop
    except KeyboardInterrupt:
        robot_control.get_logger().info('Program was interrupted')
    finally:
        robot_control.stop_robot()
        robot_control.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

5. setup.py の編集

–node-nameを使用していない場合はパッケージルートにある setup.py のエントリーポイントを設定します。

entry_points={
    'console_scripts': [
        'kachaka_simple_control = kachaka_simple_control.kachaka_simple_control:main'
    ],
},

6. パッケージのビルドと実行

パッケージのビルド

cd ~/ros2_ws
colcon build --packages-select kachaka_simple_control

環境設定の読み込み

source install/setup.bash

ノードの実行

ros2 run kachaka_simple_control kachaka_simple_control

7. コードの説明

クラスとメソッド

  • KachakaSimpleControl: メインのノードクラス
    • __init__: ノードの初期化とパブリッシャーの設定
    • move_robot: 指定された速度と時間でロボットを動かすメソッド
    • stop_robot: ロボットを停止させるメソッド

制御パラメータ

  • linear_x: 直進速度 [m/s]
    • 正の値: 前進
    • 負の値: 後退
    • 0: 停止
  • angular_z: 回転速度 [rad/s]
    • 正の値: 左回転
    • 負の値: 右回転
    • 0: 回転なし
  • duration: 動作を継続する時間 [秒]

動作シーケンス

プログラムは以下の動作シーケンスを実行します：

  1. 停止 (1.0秒)
  2. 前進 (1.0秒)
  3. 停止 (1.0秒)
  4. 左回転 (2.0秒)
  5. 停止 (1.0秒)
  6. 右回転 (2.0秒)
  7. 停止 (1.0秒)

8. ROS2の基本概念の説明

ノード（Node）

ノードは、ROS2システムの基本的な実行単位です。各ノードは特定の機能を持ち、他のノードと通信します。この例では、KachakaSimpleControlクラスがノードとして機能します。

トピック（Topic）

トピックは、ノード間でメッセージをやり取りするための通信チャネルです。この例では、/kachaka/manual_control/cmd_velトピックを使用して、ロボットへの速度指令を送信します。

メッセージ（Message）

メッセージは、トピックを通じて送信されるデータの形式を定義します。この例では、geometry_msgs/Twist型のメッセージを使用して、ロボットの線形速度と角速度を指定します。

パブリッシャー（Publisher）

パブリッシャーは、特定のトピックにメッセージを発行（送信）するためのオブジェクトです。この例では、self.publisherが/kachaka/manual_control/cmd_velトピックに速度指令を発行します。

フィードフォワード制御

フィードフォワード制御は、事前に定義された指令を実行する制御方式です。この例では、速度と時間を指定して、ロボットの動作を制御します。センサーフィードバックを使用せず、事前に決められた動作を実行します。