ロボットシステムにおけるセンシング・アクチュエーション・通信②

root@dynamics:~/roomba_hack# rostopic echo -n 1 /odom
header:
  seq: 2115
  stamp:
    secs: 1649692132
    nsecs: 791056254
  frame_id: "odom"
child_frame_id: "base_footprint"
pose:
  pose:
    position:
      x: -0.014664691872894764
      y: -0.0010878229513764381
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0056752621080531414
      w: 0.9999838955703261
  covariance: [0.08313143998384476, 0.00019857974257320166, 0.0, 0.0, 0.0, 0.004368376452475786, 0.00019857988809235394, 0.015032557770609856, 0.0, 0.0, 0.0, -0.26573312282562256, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0043683769181370735, -0.26573312282562256, 0.0, 0.0, 0.0, 6.021446704864502]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

root@dynamics:~/roomba_hack# rostopic type /odom
nav_msgs/Odometry

root@dynamics:~/roomba_hack# rosmsg info nav_msgs/Odometry
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

#!/usr/bin/env python3
import numpy as np

import rospy
import tf
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

class SimpleController:
    def __init__(self):
        rospy.init_node('simple_controller', anonymous=True)

        # Publisher
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        # Subscriber
        odom_sub = rospy.Subscriber('/odom', Odometry, self.callback_odom)

        self.x = None
        self.y = None
        self.yaw = None
        while self.x is None:
            rospy.sleep(0.1)

    def callback_odom(self, data):
        self.x = data.pose.pose.position.x
        self.y = data.pose.pose.position.y
        self.yaw = self.get_yaw_from_quaternion(data.pose.pose.orientation)

    def go_straight(self, dis, velocity=0.3):
        vel = Twist()
        x0 = self.x
        y0 = self.y
        while(np.sqrt((self.x-x0)**2+(self.y-y0)**2)<dis):
            vel.linear.x = velocity
            vel.angular.z = 0.0
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

    def turn_right(self, yaw, yawrate=-0.5):
        vel = Twist()
        yaw0 = self.yaw
        while(abs(self.yaw-yaw0)<np.deg2rad(yaw)):
            vel.linear.x = 0.0
            vel.angular.z = yawrate
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

    def turn_left(self, yaw, yawrate=0.5):
        vel = Twist()
        yaw0 = self.yaw
        while(abs(self.yaw-yaw0)<np.deg2rad(yaw)):
            vel.linear.x = 0.0
            vel.angular.z = yawrate
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

    def stop(self):
        vel = Twist()
        vel.linear.x = 0.0
        vel.angular.z = 0.0
        self.cmd_vel_pub.publish(vel)

    def get_yaw_from_quaternion(self, quaternion):
        e = tf.transformations.euler_from_quaternion(
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w))
        return e[2]

if __name__=='__main__':
    simple_controller = SimpleController()
    try:
        simple_controller.go_straight(1.0)
        simple_controller.turn_left(90)
        simple_controller.turn_right(90)
    except rospy.ROSInitException:
        pass

#!/usr/bin/env python3  # 1
import ~  # 2

class SimpleController:  # 3
    def __init__(self):  # 4
        pass
    def callback_odom(self, data):  # 5
        pass
    def go_straight(self, dis, velocity=0.3):
        pass
    def turn_right(self, yaw, yawrate=-0.5):
        pass
    def turn_left(self, yaw, yawrate=0.5):
        pass
    def stop(self):
        pass
    def get_yaw_from_quaternion(self, quaternion):
        pass

if __name__=='__main__':  # 6
    simple_controller = SimpleController()  # 7
    pass

shebang(シバン)と呼ばれるもので，このファイルを実行する際に，どのプログラムを使って実行するかを指定する． #!/usr/bin/env python3 と書いてあるので，このファイルはpython3で実行するのだとコンピュータに教えている．

import numpy as np

import rospy
import tf
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

• pythonの標準の関数(printなど)だけでは機能が足りないので，別のモジュールをインポートしている．
• このソースコードでは，numpy, rospy, tf というモジュールをインポートしている．
  • rospyというのが，pythonでrosを使うためのモジュール．
• また，geometry_msgs.msgというモジュールからTwistというデータ型, nav_msgs.msgというモジュールからOdometoryというデータ型をそれぞれインポートしている．

• SimpleControllerという名前のクラスを定義している．
• クラスはオブジェクトの設計図のようなもの．
  • オブジェクトとは，データとそのデータの振る舞いをまとめたもの．

class Car:
def __init__(self, color, speed):
    self.color = color
    self.speed = speed
    self.fuel = 100
def drive(self):
self.fuel -= 20
print(‘drove!')
print(f’残りの燃料は{self.fuel}リットルです’)
def charge(self):
self.fuel = 100
print(‘charged!')
print(f’残りの燃料は{self.fuel}リットルです’)
def info(self):
print(f’色は{self.color}です')
print(f’速度は{self.speed}km/hです')
print(f’残りの燃料は{self.fuel}リットルです')

mycar = Car('red', 200)
mycar.drive()
#drove!
#残りの燃料は80リットルです
mycar.drive()
#drove!
#残りの燃料は60リットルです
mycar.charge()
#charged!
#残りの燃料は100リットルです
mycar.info()
#色はredです
#速度は200km/hです
#残りの燃料は100リットルです

people = ['Alice', 'Bob', 'Charlie']
people.append('Dave')
print(people)
#[‘Alice’, ‘Bob’, ‘Charlie’, ‘Dave’]

• コンストラクタ__init__()とは，オブジェクト生成時に呼び出される関数のこと．
• 初期化のための関数というイメージ．

• メソッドとは，オブジェクトが持つ関数のこと．
• classの定義の中では，self.method_name(引数1, 引数2)という形で呼び出すことができる．
• オブジェクトの外から使用するときには，上のCarの例のように，object_name.method_name(引数1, 引数2)という形で呼び出すことができる．
• 定義の第一引数には，必ずselfを指定する．これは，そのオブジェクト自身を指す．
  • 呼び出すときにはselfは省略する．

• このif文の中の処理は，ファイルを直接実行したときにのみ実行される．
• __name__は特殊な変数で，ファイルを直接実行したときには'__main__'という値を持つ．
  • importされたときには__name__にはファイル名が入るため，このif文の中の処理は実行されない．

    #!/usr/bin/env python3
    print(__name__)
    

    とだけ記述したファイルを実行してみると，ふるまいが理解しやすいかもしれない．

    def __init__(self):
        rospy.init_node('simple_controller', anonymous=True)

        # Publisher
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        # Subscriber
        odom_sub = rospy.Subscriber('/odom', Odometry, self.callback_odom)

        self.x = None
        self.y = None
        self.yaw = None
        while self.x is None:
            rospy.sleep(0.1)

上にも述べた通り，__init__はコンストラクタと呼ばれ，オブジェクト生成時に自動で呼び出される関数です． 各行について順番に見ていきます．

        rospy.init_node('simple_controller', anonymous=True)

• simple_controllerという名前のノードを作成しています．

        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

• rospy.Publisher()によって，上で作成したノードがpublisherとして機能することを宣言しています．
• このノードは，/cmd_velというトピックに対して，Twistというデータ型のメッセージを送信しています．
  • Twistの情報は，geometry_msgsのドキュメントより確認できます．
  • Twistは速度３成分と角速度３成分を格納するデータ型です．
• 第２引数のqueue_sizeは，メッセージを送信する命令が，許容できる周期より短い場合に，メッセージをキューにためておく数を指定します．
  • ここでは，10個までキューにためておくことを指定しています．
  • たまった数が10個より少ない場合には，古い順にメッセージをパブリッシュしていきます．
  • たまった数が10個に達した場合には，最も古いメッセージを破棄します．

        odom_sub = rospy.Subscriber('/odom', Odometry, self.callback_odom)

• rospy.Subscriber()によって，作成したノードがsubscriberとして機能することを宣言しています．
• このノードは，/odomというトピックから，Odometryというデータ型のメッセージを受信しています．
  • Odometryの情報は，nav_msgsのドキュメントより確認できます．
  • Odometryは，位置と姿勢，及び速度と角速度を格納するデータ型です．
  • ここでは，Odometryには，ルンバの現在の位置や運動の様子が格納されており，それを受信しています．
• rospy.Subscriber()の第三引数として，self.callback_odomというコールバック関数を指定しています．
  • subscriberにはコールバック関数を指定する必要があります．
  • コールバック関数とは，subscriberがメッセージを受信したときに実行される関数のことです．
    • コールバック関数は，メッセージを引数として実行します．
  • コールバック関数の中身は，後述します．

        self.x = None
        self.y = None
        self.yaw = None

• アトリビュートを定義しています．
• クラスの定義の中でself.<name>の形式で表される変数は，そのクラスのオブジェクトが持つ"attribute(アトリビュート)“と呼ばれます．
  • アトリビュートには，そのclassの定義の中であればどこからでもアクセスできます．
• ここでは，self.x, self.y, self.yawというアトリビュートを定義しています．
  • これらのアトリビュートは，後述するコールバック関数の中で値が更新されます．
  • このアトリビュートには，ロボットの現在の位置や姿勢が格納されます．

        while self.x is None:
            rospy.sleep(0.1)

• self.xがNoneである間，0.1秒間隔で待機し続けます．

    def callback_odom(self, data):
        self.x = data.pose.pose.position.x
        self.y = data.pose.pose.position.y
        self.yaw = self.get_yaw_from_quaternion(data.pose.pose.orientation)

• コンストラクタの項目で説明した通り，subscriberを定義する際にはコールバック関数を指定する必要があります．
• コールバック関数はsubscriberがメッセージを受信した際，そのメッセージを引数として実行する関数でした．
• 上のコールバック関数では，引数dataには，Odometry型のメッセージが格納されます．
  • data.pose.pose.positionという書き方によって，Odometry型の中の，位置を表すpositionという要素にアクセスしています．
  • 同様に，data.pose.pose.orientationという書き方によって，Odometry型の中の，姿勢を表すorientationという要素にアクセスしています．
• コンストラクタで定義していたアトリビュートself.x, self.y, self.yawに，メッセージから得られた位置と姿勢を格納しています．
  • self.yawに値を格納する時に使用している関数get_yaw_from_quaternion()については，後述します．
• このコールバック関数が一度でも呼ばれると，self.xに入っているNoneの値が上書きされ，コンストラクタの中のwhile文が終了します．

go_straight関数やturn_right関数とは順番が前後しますが，先にget_yaw_from_quaternion関数について説明します．

    def get_yaw_from_quaternion(self, quaternion):
        e = tf.transformations.euler_from_quaternion(
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w))
        return e[2]

• tfモジュールのeuler_from_quaternion関数を利用しています．
  • euler_from_quaternion()は，クォータニオン(4要素)を引数として，オイラー角(3要素)を返す関数です．
  • クォータニオンについてはLearn/オドメトリのセンサ情報を用いた制御/クォータニオンの項目で説明しました．
• Odometry型のメッセージのうち，姿勢を表すorientationという要素は，クォータニオンで表されているため，オイラー角で制御したい場合には，この関数を用いてオイラー角に変換する必要があります．
• ルンバはxy平面上を動くため，z軸周りのオイラー角さえわかれば十分です．そのため，このget_yaw_from_quaternion関数ではオイラー角のz軸成分(第2成分)のみを返しています．

    def go_straight(self, dis, velocity=0.3):
        vel = Twist()
        x0 = self.x
        y0 = self.y
        while(np.sqrt((self.x-x0)**2+(self.y-y0)**2)<dis):
            vel.linear.x = velocity
            vel.angular.z = 0.0
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)
        self.stop()

ロボットを直進させる関数です．順に説明します

        vel = Twist()

• Twist型のオブジェクトを生成しています．

        x0 = self.x
        y0 = self.y

• self.x, self.yには，コールバック関数で更新された(直前の)ルンバの位置が格納されています．それをx0, y0に代入しています．

        while(np.sqrt((self.x-x0)**2+(self.y-y0)**2)<dis):
            vel.linear.x = velocity
            vel.angular.z = 0.0
            self.cmd_vel_pub.publish(vel)
            rospy.sleep(0.1)

• 点(self.x, self.y)と点(x0, y0)の距離が指定したdisより小さい間，while以下の処理を繰り返します．
  • velの並進成分のx成分に，引数で指定したvelocityの値を格納します．
  • velの回転成分のz成分(z軸周りの角速度)に，0を格納します．
  • コンストラクタの中で定義したpublisherであるself.cmd_vel_pubの.publish()関数を用いて，publishを行います．
    • 引数にvelを指定しているので，確かにTwist型のメッセージをパブリッシュしています．
  • rospy.sleep(0.1)で0.1秒待ち，次のループに入ります．

        self.stop()

• while文が終了したら，ロボットを停止させます．
  • stop関数については，説明を省略します．
• turn_right, turn_left関数についても，go_straight関数の説明と同様なので省略します．

． jetsonでdockerコンテナを起動

(開発PC):~$ ssh roomba_dev1
(jetson):~$ cd ~/group_a/roomba_hack
(jetson):~/group_a/roomba_hack ./RUN-DOCKER-CONTAINER.sh
(jetson)(docker):~/roomba_hack#  

開発PCでdockerコンテナを起動

(開発PC):~$ cd ~/group_a/roomba_hack
(開発PC):~/group_a/roomba_hack ./RUN-DOCKER-CONTAINER.sh 192.168.10.7x
(開発PC)(docker):~/roomba_hack#

(jetson)(docker):~/roomba_hack# roslaunch roomba_bringup bringup.launch

/odomの型を確認

(開発PC)(docker):~/roomba_hack# rostopic type /odom

/odomの中身を確認

(開発PC)(docker):~/roomba_hack# rostopic echo /odom

simple_control2.pyを実行してみよう．

開発PCでteleopのコードを実行しましょう

(開発PC)(docker):~/roomba_hack# roslaunch roomba_teleop teleop.launch

このプログラムを動かすときには，コントローラのYボタンを押してからBボタンを押してautoモードにしておきましょう．

1メートルほど前に進んだあと，左に90度程度旋回し，右に90度程度旋回したら成功です．

(開発PC)(docker):~/roomba_hack# rosrun navigation_tutorial simple_control2.py

try it! simple_control2.pyの中身を読んでコードを変更してみよう