HARD2021: ルンバをPythonプログラムで動かそう！

2021.03.11

Pythonプログラムでルンバを動かしてみましょう！まず、ソースコードを見てみましょう。今回もたったの39行です。PythonプログラムはC++でコーディングする場合と比較して圧倒的に短くなるので初心者にもとっつきやすいと思います。

サンプルプログラム

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist

def set_vel(vel_msg, lv, av):
    vel_msg.linear.x  = lv
    vel_msg.linear.y  = 0
    vel_msg.linear.z  = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = av

def main_loop():
    rospy.init_node('move')
    vel_publisher = rospy.Publisher('/create1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()
    set_vel(vel_msg, 0, 0)
    print("Let's move your robot")
    linear_vel  = input("Input linear velocity [m/s] :")
    angular_vel = input("Input angular velocity [rad/s] :")
    vel_msg.linear.x  = linear_vel
    vel_msg.angular.z = angular_vel
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        vel_publisher.publish(vel_msg)
        rospy.loginfo("Velocity: Linear=%s Angular=%s", vel_msg.linear.x, vel_msg.angular.z)
        rate.sleep()

if __name__ == '__main__':
        main_loop()

サンプルプログラムの説明

このプログラムではROSの速度指令トピック/create1/cmd_velをmove_baseノードへパブリッシュ(配信)することによりロボットを動かしています。通常速度指令のトピックは/cmd_velですが、このcreate_autonomyパッケージでは複数台のロボットを識別するために/create1を前に付けています。２台目のロボットなら/create2になります。なお、速度指令トピックの型はTwist型で、3次元の並進速度ベクトルと角速度ベクトルがメンバーになっています。

次に重要なことはROSではループの周期をrospy.Rate(周波数)で設定し、ループの中にrospy.sleep()を入れることで簡単に一定の周期でループを制御できることです。ロボットの制御においては一定周期でループを回して処理をすることがとても大切です。

それ以外の詳細については、ソースコードに説明を加えたものを以下示しますので参照してください。

#!/usr/bin/env python
# -*- coding: utf-8 -*- # 日本語を使うためのおまじない。
import rospy  # ROSでpython使う場合に必要
from geometry_msgs.msg import Twist # ロボットの速度を扱う場合は必要

# set_vel関数：速度の設定。
# １番目の引数は速度メッセージ、２番目の引数は並進速度、３番目の引数は角速度
def set_vel(vel_msg, lv, av):
    vel_msg.linear.x  = lv
    vel_msg.linear.y  = 0
    vel_msg.linear.z  = 0
    vel_msg.angular.x = 0
    vel_msg.angular.y = 0
    vel_msg.angular.z = av

# main関数
def main():
    #  ノードの初期化。引数はノード名。ROSでは同じノード名のノードを複数作ることはできない。
    rospy.init_node('move')

    # パブリッシャーの生成。一番目の引数はトピック名、２番目の引数はメッセージの型、
    # 速度指令はTwist型。３番目の引数はメッセージバッファーのサイズ。
    vel_publisher = rospy.Publisher('/create1/cmd_vel', Twist, queue_size=10)

    vel_msg = Twist()       # vel_msgオブジェクトの生成    
    set_vel(vel_msg, 0, 0)  # 速度の初期化。安全のために0に設定。

    # キーボードからの入力
    linear_vel  = input("Input linear velocity [m/s] :")
    angular_vel = input("Input angular velocity [rad/s] :")

    #　並進速度または角速度の設定rospy.is_shutdown():
    #  ROS座標系は右手系、ロボットの前進方向がX軸、右方向がy軸、上方向がz軸
    vel_msg.linear.x  = linear_vel
    vel_msg.angular.z = angular_vel

    # ループの周期を設定。10Hz。つまり、1ループ100ms。
    # ロボットを一定の周期で動かすことはとても重要
    rate = rospy.Rate(10)

    # Ctrl-Cが押されるまで無限ループ
    while not rospy.is_shutdown():        
        vel_publisher.publish(vel_msg) # メッセージの配信。ロボットに速度指令を送る。        
        rospy.loginfo("V: Linear=%s Angular=%s", vel_msg.linear.x, vel_msg.angular.z)  # 表示        
        rate.sleep() # rospy.Rate()で指定した時間になるように調整

# このプログラムをモジュールとしてimportできるようにするおまじない。
# なお、モジュールとは他のプログラムから再利用できるようにしたファイルのこと。
if __name__ == '__main__':
        main()

パッケージの作成

  • パッケージは次のcatkin_create_pkgコマンドで作ります。依存パッケージはそのパッケージを作るために必要なパッケージです。
    • catkin_create_pkg <パッケージ名> [依存パッケージ1] [依存パッケージ2]　[依存パッケージ3]
  • では、次のコマンドでmoveパッケージを作りましょう！既にhard2021ディレクトリを作成している場合は、2番目のcdコマンドから実行してください。
    • $ mkdir -p ~/catkin_ws/src/hard2021
    • $ cd ~/catkin_ws/src/hard2021
    • $ catkin_create_pkg move roscpp rospy std_msgs
  • 上手く作成できたか確認しましょう。
    • $ cd ~/catkin_ws/src/hard2021/move
    • $ ls
    • 次のファイルができていれば成功です。
      • CMakeLIsts.txt, include, package.xml, srcができていれば成功。

ソースコードの作成

  • エディタを開き、上の説明のない方のソースコードをコピペしてセーブします。以下のコマンドはエディタにgeditを使う場合です。
    • $ cd ~/catkin_ws/src/hard2021/move/src
    • $ gedit move.py

ビルド

  • Pythonはビルドする必要はないが、ROSで使用する場合は必要になる場合があるので、以下のコマンドを実行しましょう。
    • $ cd ~/catkin_ws
    • $ catkin build  move
  • 作成したpythonプログラムに実行権を与えましょう。実行権を与えないとrosrunコマンドで実行できません。
    • $ source ~/.bashrc
    • $ roscd move/src
    • $ chmod u+x  move.py

実　行　

  • シミュレータの起動
    • 今回はrvizを使わないので、端末を開き、次のコマンドでGazeboと同時に起動しないようにする。
      • $ export RVIZ=false
    • 続けて、以下のコマンドを実行してシミュレータGazeboを起動する。一番上図のルンバをベースにしたロボットが現れる。
      • $ roslaunch ca_gazebo create_empty_world.launch
  • move.pyノードの実行
    •  端末をもう一つ開き、以下のコマンドを実行する。
      • $ roscd move/src
      • $ rosrun move move.py
    • 以下のように並進速度と角速度を聞かれるので入力する。この例では、linear velocity (並進速度)：0.5 [m/s]、Angular velocity（角速度）: 0.5 [rad/s]を入力している。
    • ロボットが円軌道上を動いたら成功。なお、角速度を０にすると直進するが、半径無限大の円周上を移動していると考えることもできる。
      ロボットを停止する場合は、まず、rosrunを実行した端末でCtrl+cキーを押してプロセスを止める。次にもう一度rosrun move move.pyを実行して並進と角速度を0にする。並進速度に負の値を入れると後進し、角速度に負の値を入れるとロボットを上から見て時計周りに回転する。

ハンズオン

  1. サンプルプログラムの実行
    • 上の説明に従ってサンプルプログラムを実行して動作を確認しよう！
  2. 指定した時間だけ移動する
    • 指定した時間[s]の間、指定した速度[m/s]と角速度[rad/s]で移動するプログラムを作ろう！
  3. 指定した距離/角度だけ移動する
    • 指定した距離[m]/角度[rad]だけ移動するプログラムを作ろう！
  4. 正方形を描く
    • １辺がx[m]の正方形の軌跡を描くようにロボットを動かそう。
  5. 円を描く
    • 半径x[m]の円の軌跡を描くようにロボットを動かそう。
  6. オドメトリ関数の実装
    • ロボットを自作するとオドメトリも自分で実装しなければなりません。それに備えてオドメトリを実装してみましょう。
    • 距離[m] = 並進速度[m/s]　×　時間[s]、回転角度[rad] = 角速度[rad/s]　×　時間[s]
    • ロボットの姿勢を考慮して、三角関数を使い進んだ距離と角度を計算する。距離はx軸、y軸に分ける考える。計測時間が必要になるのでネットで調べよう。
    • 計算に必要な情報
      • ルンバの車輪間の距離
      • 車輪の直径
      • 車輪の回転量
    • プログラムができたら、/odomと比較して正しいか確認しよう。

終わり

コメント

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