LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@pez(Ryo ITO)

ROS2プログラミング演習_02: ロボット動作プログラムの自作

Last updated at Posted at 2025-05-19

はじめに

Gazeboシミュレータでturtlebot3を動かします．
このページではセンサ信号などの制御はなしで単純にTurtlebot3にあらかじめ決められた動作をさせるプログラムを自作します．

環境

・OS: Windows11 Pro (64bit)
・ROS: ROS2 humble (WSL2のUbuntu22.04に)

ここまでの環境構築については下記参照
・WSL2にROS2を入れてgazeboでシミュレーションするまでの手順
https://qiita.com/pez/items/16011df0e663ceac694a

1. ワークスペース作成・準備

今回は move_ws としておきます

$ mkdir -p ~/move_ws/src/

今回必要なライブラリ

入っていなければ入れておく
sudo apt install ros-humble-tf-transformations
sudo pip3 install transforms3d

2. パッケージ作成

今回ノード名は my_move_node
パッケージ名は my_move
にすることにします

$ cd ~/move_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_move_node my_move

3. プログラミング

3.1 package.xml ファイルの編集

$ cd ~/move_ws/src/my_move
$ emacs package.xml

自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．

3.2 setup.py ファイルの編集

$ cd ~/move_ws/src/my_move
$ emacs setup.py

23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．

entry_point={
    'console_scripts': [
        'my_move_node = my_move.my_move_node:main' 
    ],
},

※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．

3.3 ソースコード作成

今回もパッケージ作成時に自動的に作られている my_move_node.py を編集することにします．

$ cd ~/move_ws/src/my_move/my_move
$ emacs my_move_node.py

下記のように記述してみましょう．

my_move_node.py
import math
import sys
import rclpy
import tf_transformations
from rclpy.node import Node   
from rclpy.executors import ExternalShutdownException    
from geometry_msgs.msg import Twist  # Twistメッセージ型をインポート
from nav_msgs.msg import Odometry    # Odometryメッセージ型をインポート
from tf_transformations import euler_from_quaternion 


class MyMove(Node):  # 簡単な移動クラス
    def __init__(self):   # コンストラクタ
        super().__init__('my_move_node')        
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.sub = self.create_subscription(Odometry, 'odom', self.odom_cb, 10)   
        self.timer = self.create_timer(0.01, self.timer_callback)
        self.x, self.y, self.yaw = 0.0, 0.0, 0.0
        self.x0, self.y0, self.yaw0 = 0.0, 0.0, 0.0
        self.vel = Twist()  # Twist メッセージ型インスタンスの生成
        self.set_vel(0.0, 0.0)  # 速度の初期化
 
    def get_pose(self, msg):      # 姿勢を取得する
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        q_x = msg.pose.pose.orientation.x
        q_y = msg.pose.pose.orientation.y
        q_z = msg.pose.pose.orientation.z
        q_w = msg.pose.pose.orientation.w
        (roll, pitch, yaw) = tf_transformations.euler_from_quaternion(
            (q_x, q_y, q_z, q_w))
        return x, y, yaw
  
    def odom_cb(self, msg):         # オドメトリのコールバック関数
        self.x, self.y, self.yaw = self.get_pose(msg)
        self.get_logger().info(
            f'x={self.x: .2f} y={self.y: .2f}[m] yaw={self.yaw: .2f}[rad/s]')     
    
    def set_vel(self, linear, angular):  # 速度を設定する
        self.vel.linear.x = linear   # [m/s]
        self.vel.angular.z = angular  # [rad/s]    
    
    def move_distance(self, dist):  # 指定した距離distを移動する
        error = 0.05  # 許容誤差 [m] 
        diff = dist - math.sqrt((self.x-self.x0)**2 + (self.y-self.y0)**2) 
        if math.fabs(diff) > error:
            self.set_vel(0.1, 0.0)
            return False
        else:
            self.set_vel(0.0, 0.0)
            return True

    def rotate_angle(self, angle):  # 指定した角度angleを回転する
        # このメソッドは間違っています．move_distanceを参考に完成させてください．
        self.set_vel(0.0, 0.25)
        return False

    def timer_callback(self):  # タイマーのコールバック関数
        self.pub.publish(self.vel)  # 速度指令メッセージのパブリッシュ 
        
    def my_move(self,  distance, angle):  # 簡単な状態遷移
        state = 0
        while rclpy.ok():
            if state == 0:
                if self.move_distance(distance):
                    state = 1
            elif state == 1:                
                if self.rotate_angle(angle):
                    break
            else:
                print('エラー状態')
            rclpy.spin_once(self)


def main(args=None):  # main関数
    rclpy.init(args=args)
    node = MyMove()

    try:
        node.my_move(0.7, math.pi/2)
    except KeyboardInterrupt:
        print('Ctrl+Cが押されました．')     
    except ExternalShutdownException:
        sys.exit(1)
    finally:
        rclpy.try_shutdown()

4. ビルド

ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます

$ cd ~/move_ws
$ colcon build

成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

5. 設定ファイルの反映

アンダーレイ設定ファイル（ROS2システムの設定）は .bashrc に記述済みとします
※下記が.bashrcに既にある
source /opt/ros/humble/setup.bash

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記

.bashrcの編集
$ emacs ~/.bashrc

$ source ~/move_ws/install/setup.bash
を追加してから反映させる

$ source ~/.bashrc

6. ノードの実行（シミュレーション）

同じことをシミュレーション上で行うこともできます．
シミュレーションに関するセットアップは別記事参照
https://qiita.com/pez/items/d795978c5436a87bc1b7

gazeboでTurtlebot3のシミュレーション
$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
別のターミナルにて
$ ros2 run my_move my_move_node

ロボットが自動的に走り出すはずです．
止まらなくなってしまった場合は「bringup」コマンドを実行したターミナルの方を「ctrl＋c」などで立ち下げましょう．

7. 演習

プログラムの中に書いてあるように関数 rotate_angle は未完成なので，指定された角度でロボットを停止させる関数として完成させて下さい

MEMO
＜プログラムを更新したときは＞
　1.プログラムの中身を編集して保存
　2.ビルド
　3.設定ファイルの反映
　4.ノードの実行
　→ 動作を確認

0
0
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
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?