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

複数のセンサを組み合わせてよりかしこくロボットを動かしてみよう

Learn

前回の演習では，速度と時間の指令を使ってロボットを制御しました．

周囲に障害物が何もない状況や，ロボットの滑りがない環境では，速度と時間のコマンドを使って思った通りにロボットを動かすことができるかもしれませんが，実環境では，ロボットの周囲には障害物が存在しますし，移動距離で制御する方が直感的です．

前回の演習のようにロボットに速度と時間を一回与えて，その通りに動かすようなフィードフォワード制御ではなく，今回は，ロボットが逐次的にセンサの情報を反映して振る舞いを変えるフィードバック制御を行なってみましょう．

オドメトリのセンサ情報を使ってロボットを動かしてみよう

まずは，ロボットのタイヤの回転量から計算される移動距離である（ホイール）オドメトリ（odometry）を使った制御をしてみましょう．

オドメトリのメッセージ（/odom）の中身を見てみよう

roombaのオドメトリの情報は，/odomトピックにpublishされています．

rostopic echo /odomをしてみるとメッセージとしてどんな情報が流れているかわかります．

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]
---

rostopic type /odomをしてみると，メッセージとして，nav_msgs/Odometry型が使われていることがわかります．

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

nav_msgs/Odometry型のドキュメントを確認してみると，このメッセージはposeとtwistで構成されていることがわかります．

poseは．（child_frameから見た）ロボットの推定姿勢（位置と回転角）を表していて，covarianceにはその不確かさを表す共分散が記録されています．

一方，twistは，（child_frameから見た）ロボットの速度を表していて，poseと同様にcovarianceにはその不確かさを表す共分散が記録されています．

なお，メッセージ型の定義は，rosmsg info 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

クォータニオン(quaternion)

さて，/odomのトピックでは，ロボットの回転角はクォータニオン（quaternion）で記述されています．

クォータニオンは，日本語では四元数と呼ばれ，3次元空間上での回転角を表現する方法の一つで，4つの要素を持つベクトルで表現されます．

クォータニオンによる3次元回転の表現は，角度を連続的にかつ簡潔に表現できるためROSではよく用いられます（その他には，オイラー角による表現や回転行列による表現があります）．

それぞれの回転角に関する表現のメリット・デメリットを調べてみましょう（「ジンバルロック」などのキーワードで調べるとよりよく理解できると思います）．

クォータニオンからオイラー角へは，tfパッケージのtf.transformations.euler_from_quaternionを使うことで変換できます（ドキュメント）．

サブスクライバ（subscriber)の仕組みを知ろう

それでは，オドメトリ/odomの情報を使った制御の実装の例としてnavigation_tutorialパッケージの中のsimple_control2.pyを見てみましょう（github）．

前回までに強調されてきた通り，ROSは非同期分散のシステムを簡単に作ることができるのが特徴です． そのため，ロボットから非同期に送られてくる/odomの情報をうまく扱うことが重要です．

実装例にあるように，Pythonによるノードの実装では，クラスとして定義するのがわかりやすい方法でしょう．

実装例では，SimpleControlllerクラスとして，simple_controllerというノードを定義しています． 以下のように，ノードを初期化する際に，コマンドを/cmd_velトピックに送信するパブリッシャ（publisher)と，/odomを受信するサブスクライバ(subscriber)を作成しています．

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)

パブリッシャの使い方は前回のsimple_control.pyの実装を確認してください．

パブリッシャと同様に，サブスクライバはrospyのSubscriberを用いて作成できます． サブスクライバの特徴として，メッセージを受信した時の処理であるコールバック（callback）を定義できます．

この実装例では，self.callback_odomとして定義されており，インスタンスの属性（self.x, self.y, self.yaw）を，受信したメッセージで変更するようなプログラムになっています．

    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)

つまり，self.xには/odomから受信した位置のx座標，self.yには位置のy座標，self.yawには，回転角のyawを格納しています．

クォータニオンとして受信した姿勢の回転角のyaw成分を取り出すためのself.get_yaw_from_quaternionは以下のようになっています（オイラー角はroll, pitch, yawの順で返ってくるのでe[2]でyawを取得しています）．

    def get_yaw_from_quaternion(self, quaternion):
        e = tf.transformations.euler_from_quaternion(
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w))
        return e[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()

右回転

    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()

それでは，オドメトリを使って実際にロボットを制御してみましょう．

演習

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

(開発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

ROSメッセージの可視化

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

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

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

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