今回は，Happy MiniをPythonプログラムで動かします．教科書 [第４章 ナビゲーション] の[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()
準 備
- 教科書用Dockerイメージをダウンロードして使える状態にする（以下のリンク参照）．
ハンズオン
- パッケージの作成
- ここでは、パッケージ名とノード名を同じmy_robotにする。
$ cd ~/happy_ws/src$ ros2 pkg create --build-type ament_python --node-name my_happy_move my_happy_move_node
- ここでは、パッケージ名とノード名を同じmy_robotにする。
- ソースコードの作成
- VSCodiumで次のファイルを開き、上のコードを上書き保存する。
- ~/happy_ws/src/my_happy_move/my_happy_move/my_happy_move_nodet.py
- VSCodiumで次のファイルを開き、上のコードを上書き保存する。
- 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
ホームワーク
- チャレンジ4.1 (p.174)をやってみよう！
- チャレンジ4.2 (p.174)をやってみよう！
- チャレンジ4.3 (p.174)をやってみよう！
- ソースコードのeuler_from_quaternion関数はクオータニオンからオイラー角への変換する関数です。クオータニオンとオイラー角を調べて、この関数の処理を理解しよう。
- main関数は開始状態から前進状態→停止状態→後進状態→終了状態へと状態遷移に基づくコードになっており、whileループの最後に、毎回spin_once()関数でコールバック関数を呼び出しています。前進状態は、状態1になったときの時間から計測してt[s]経過したら、次の状態2（停止）に遷移することで実現しています。このような処理ではなく、t[s]経過したら停止するmove_second(t)関数を作ってください。
終わり。お疲れ様！
コメント