この記事は金沢工業大学 ロボティクス学科で2021年後学期開講中のロボットプログラミングⅡ用です．
今回は、Happy MiniをPythonプログラムで動かします。
ソースコード
今回使用するソースコードです。コード中にコメントを多く入れたのでそれを読むと理解できると思います。必要があれば説明を補足します。
import rclpy # ROS2のPythonモジュールのインポート
import time # timeモジュールのインポート
import math # 数学関数モジュールのインポート
import numpy as np # numpyモジュールを別名npをつけてインポート
from rclpy.node import Node # rclpy.nodeモジュールからNodeクラスをインポート
from std_msgs.msg import String # トピック通信に使うStringメッセージ型をインポート
from geometry_msgs.msg import Twist # トピック通信に使うTwistメッセージ型をインポート
from nav_msgs.msg import Odometry # nav_msgs.msgモジュールからOdometryクラスをインポート
class Pose:
""""姿勢のクラス
位置(x, y)、向き(yaw)
"""
def __init__(self, x = 0.0, y = 0.0, yaw = 0.0):
self.x = x
self.y = y
self.yaw = yaw
def set(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
def get(self):
return self.x, self.y, self.yaw
class MyRobot(Node):
"""MyRobotクラス。ロボットに速度指令cmd_velをパブリッシュしたり、
odomトピックをサブスクライブして姿勢を取得するクラス。
Node: MyRobotクラスが継承するクラス
"""
def __init__(self):
"""コンストラクタ。パブリッシャーとタイマーを生成する。
"""
# Nodeクラスのコンストラクタを呼び出し、'my_robot'というノード名をつける。
super().__init__('my_robot')
# パブリッシャーの生成。create_publisherの1番目の引数はトピック通信に使うメッセージ型。
# Twist型は速度指令値を通信するのに使われる。2番目の引数'cmd_vel'はトピック名。
# 3番目の引数はキューのサイズ。キューサイズはQOS(quality of service)の設定に使われる。
# サブスクライバーがデータを何らかの理由で受信できないときのキューサイズの上限となる。
self.pub = self.create_publisher(Twist,'cmd_vel', 10)
# サブスクライバーの生成。create_subscriptionの1番目の引数Twistはトピック通信に使うメッセージ型。
# Twist型は速度指令値を通信するのに使われる。2番目の引数'cmd_vel'はトピック名。
# 3番目の引数はコールバック関数。 4番目の引数はキューのサイズ。
self.sub = self.create_subscription(Odometry,'odom', self.odom_callback, 10)
# Twistメッセージ型オブジェクトの生成。メンバーにdVector3型の並進速度成分linear、
# 角速度成分angularを持つ。
self.vel = Twist()
print("*** my_robot node ***")
print("Input f, b, r, l key, then press Enter key.")
self.state = 0
self.odom = Odometry()
self.pose = Pose()
def euler_from_quaternion(self,quaternion):
"""クオータニオンからオイラー角を計算する関数
Converts quaternion (w in last place) to euler roll, pitch, yaw
quaternion = [x, y, z, w]
Bellow should be replaced when porting for ROS 2 Python tf_conversions is done.
"""
x = quaternion.x
y = quaternion.y
z = quaternion.z
w = quaternion.w
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = np.arctan2(sinr_cosp, cosr_cosp)
sinp = 2 * (w * y - z * x)
pitch = np.arcsin(sinp)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def odom_callback(self, odom):
"""サブスクライバーのコールバック関数。端末に並進と角速度を表示する。
"""
self.get_logger().info("Pose: x=%f y=%f" % (odom.pose.pose.position.x,odom.pose.pose.position.y))
self.get_logger().info("Vel: Linear=%f angular=%f" % (odom.twist.twist.linear.x,odom.twist.twist.angular\
.z))
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
roll, pitch, yaw = self.euler_from_quaternion(odom.pose.pose.orientation)
self.pose.set(x, y, yaw)
print("callback pose =", x, y, yaw)
def move(self, linear, angular = 0.0):
"""移動関数
linear: 並進速度, angular:角速度. 省略されたときは0.0を代入
"""
self.vel.linear.x = linear
self.vel.angular.z = angular
self.pub.publish(self.vel)
def distance(self, pose1, pose0):
"""距離関数
pose1: 現在の姿勢(x, y), pose0:スタートの姿勢
"""
d = math.sqrt((pose1.x - pose0.x) * (pose1.x - pose0.x) \
+ (pose1.y - pose0.y) * (pose1.y - pose0.y))
print("d=",d)
return d
def print(self, sentence):
""" 表示
sentence: 文字列
"""
self.get_logger().info(sentence)
def main(args=None):
rclpy.init(args=args) # rclpyモジュールの初期化
my_robot = MyRobot() # ノードの作成
v = Twist()
t = 5.0 # time [s]
d = 1.0 # distance [m]
while rclpy.ok():
if my_robot.state == 0: # 開始状態
my_robot.state = 1
start_time = time.time()
my_robot.print("State 1")
elif my_robot.state == 1: # 前進状態。t[s]間前進。
my_robot.move(0.2)
if time.time() - start_time > t:
my_robot.state = 2
start_time = time.time()
my_robot.print("State 2")
elif my_robot.state == 2: # 停止状態。t[s]間停止。
my_robot.move(0.0)
if time.time() - start_time > t:
my_robot.state = 3
start_pose = Pose()
start_pose.x, start_pose.y, start_pose.ya = my_robot.pose.get()
my_robot.print("State 3")
elif my_robot.state == 3: # 後進状態。d[m]後進。
my_robot.move(-0.2)
print("start=", start_pose.x, start_pose.y)
if my_robot.distance(my_robot.pose, start_pose) > d:
my_robot.state = 4
my_robot.print("State 4")
else: # 終了状態。停止してノードを破壊
my_robot.move(0.0)
my_robot.print("End")
my_robot.destroy_node()
rclpy.spin_once(my_robot)
rclpy.shutdown() # rclpyモジュールの終了処理
if __name__ == '__main__':
main()
準 備
- Gazeboを使うためのROSパッケージ gazebo_ros_pkgsをインストールする．
$ sudo apt -y install ros-foxy-gazebo-ros-pkgs
- Turtlebot3のパッケージをインストールする．
$ sudo apt -y install ros-foxy-dynamixel-sdk$ sudo apt -y install ros-foxy-turtlebot3-msgs$ sudo apt -y install ros-foxy-turtlebot3
- 次のGithubのサイトからHappy Mini TurtleBot3 Simulationsパッケージを次のコマンドでクローンして，必要なファイルを別の場所にコピーする．
$ cd ~/airobot_ws/src$ git clone https://github.com/demulab/happy_mini_turtlebot3_sim.git$ cd ~/airobot_ws/src/happy_mini_turtlebot3_sim/turtlebot3_gazebo/models/turtlebot3_happy_mini/urdf$ sudo cp turtlebot3_happy_mini.urdf /opt/ros/foxy/share/turtlebot3_description/urdf
- ビルドする．
$ cd ~/airobot_ws$ colcon build --symlink-install
ハンズオン
- パッケージの作成
- ここでは、パッケージ名とノード名を同じmy_robotにする。
$ cd ~/airobot_ws/src$ ros2 pkg create --build-type ament_python --node-name my_robot my_robot
- ここでは、パッケージ名とノード名を同じmy_robotにする。
- ソースコードの作成
- 好きなエディタで次のファイルを開き、上のコードを上書き保存する。
- ~/airobot_ws/src/my_robot/my_robot/my_robot.py
- 好きなエディタで次のファイルを開き、上のコードを上書き保存する。
- setup.pyとpackage.xmlの変更
- 以前の演習を参考に必要に応じてsetup.pyとpackage.xmlを変更する。ここでは、特に変更しなくてもOK。
- ビルド
$ cd ~/airobot_ws$ colcon build --symlink-install- なお、symlink-installはPython言語の場合にソースコードを変えても再ビルドが必要なくなる便利なオプション。
- 実 行
- 端末を開き、byobuを実行する。
$ byobu
- Shift-F2で端末を分割し、上の窓で次のコマンドを実行する。
- $ export TURTLEBOT3_MODEL=happy_mini
$ ros2 launch turtlebot3_gazebo empty_world.launch.py
- 下の窓で次のコマンドを実行する。
$ ros2 run my_robot my_robot
- 実行すると速度0.2[m/s]で5秒間直進し、3秒停止、速度-0.2[m/s]で1[m]直進して停止する。つまり、スタート地点に戻る。
- 端末を開き、byobuを実行する。
ホームワーク
- ソースコードのeuler_from_quaternion関数はクオータニオンからオイラー角への変換する関数です。クオータニオンとオイラー角を調べて、この関数の処理を理解しよう。
- main関数は開始状態から前進状態→停止状態→後進状態→終了状態へと状態遷移に基づくコードになっており、whileループの最後に、毎回spin_once()関数でコールバック関数を呼び出しています。前進状態は、状態1になったときの時間から計測してt[s]経過したら、次の状態2（停止）に遷移することで実現しています。このような処理ではなく、t[s]経過したら停止するmove_second(t)関数を作ってください。
終わり。お疲れ様！
コメント