ROS2ハンズオン3-2:Happy Miniをプログラムで動かそう!

今回は,Happy MiniをPythonプログラムで動かします.教科書 [第4章 ナビゲーション] の[4.3.3節 シミュレータとリアルロボットを動かす方法 ● 自動でロボットを動かしてみよう!(p.112)]に相当します.

ソースコード

今回使用するソースコード (p.112 プログラムリスト4.2 happy_move_node.py)です。コードの説明は教科書pp.112~114をご覧ください.

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 HappyMove(Node):  # 簡単な移動クラス
    def __init__(self):   # コンストラクタ
        super().__init__('happy_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.25, 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 happy_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 = HappyMove()

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

準 備

ハンズオン

  • パッケージの作成
    • ここでは、パッケージ名とノード名を同じmy_robotにする。
      • $ cd ~/happy_ws/src
      • $ ros2 pkg create --build-type ament_python --node-name my_happy_move my_happy_move_node
  • ソースコードの作成
    • VSCodiumで次のファイルを開き、上のコードを上書き保存する。
      •  ~/happy_ws/src/my_happy_move/my_happy_move/my_happy_move_nodet.py
  • setup.pyとpackage.xmlの変更
    • 以前の演習を参考に必要に応じてsetup.pyとpackage.xmlを変更する。ここでは、特に変更しなくてもOK。
  • ビルド
    • $ cd ~/happy_ws
    • $ colcon build
  • 実 行
    • 端末を開き2分割する.
    • 上の窓で次のコマンドを実行する。
      • $ export TURTLEBOT3_MODEL=happy_mini
      • $ ros2 launch turtlebot3_gazebo empty_world.launch.py
    • 下の窓で次のコマンドを実行する。
      • $ ros2 run my_happy_move my_happy_move_node

ホームワーク

  1. チャレンジ4.1 (p.174)をやってみよう!
  2. チャレンジ4.2 (p.174)をやってみよう!
  3. チャレンジ4.3 (p.174)をやってみよう!
  4. ソースコードのeuler_from_quaternion関数はクオータニオンからオイラー角への変換する関数です。クオータニオンとオイラー角を調べて、この関数の処理を理解しよう。
  5. main関数は開始状態から前進状態→停止状態→後進状態→終了状態へと状態遷移に基づくコードになっており、whileループの最後に、毎回spin_once()関数でコールバック関数を呼び出しています。前進状態は、状態1になったときの時間から計測してt[s]経過したら、次の状態2(停止)に遷移することで実現しています。このような処理ではなく、t[s]経過したら停止するmove_second(t)関数を作ってください。

happy_mini_empty_world.png   終わり。お疲れ様!

コメント

タイトルとURLをコピーしました