ロボットプログラミングⅡ-2021:ROS2演習8 – Happy Miniをプログラムで動かそう!(Python)

この記事は金沢工業大学 ロボティクス学科で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
  • ソースコードの作成
    • 好きなエディタで次のファイルを開き、上のコードを上書き保存する。
      •  ~/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]直進して停止する。つまり、スタート地点に戻る。

 

ホームワーク

  1. ソースコードのeuler_from_quaternion関数はクオータニオンからオイラー角への変換する関数です。クオータニオンとオイラー角を調べて、この関数の処理を理解しよう。
  2. main関数は開始状態から前進状態→停止状態→後進状態→終了状態へと状態遷移に基づくコードになっており、whileループの最後に、毎回spin_once()関数でコールバック関数を呼び出しています。前進状態は、状態1になったときの時間から計測してt[s]経過したら、次の状態2(停止)に遷移することで実現しています。このような処理ではなく、t[s]経過したら停止するmove_second(t)関数を作ってください。

happy_mini_empty_world.png

 

終わり。お疲れ様!

コメント

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