ROS2プログラミング基礎_2-1【解答例】

## はじめに
https://qiita.com/pez/items/86828028f0db52ec2c61

上記のロボット動作プログラム演習でのプログラム修正課題の解答例になります
あくまで一例として参考にして下さい


```ruby:my_move_node.py
import math
import sys
import rclpy
import tf_transformations
from rclpy.node import Node   
from rclpy.executors import ExternalShutdownException    
from geometry_msgs.msg import Twist  # Twistメッセージ型をインポート
from nav_msgs.msg import Odometry    # Odometryメッセージ型をインポート
from tf_transformations import euler_from_quaternion 



class MyMove(Node):  # 簡単な移動クラス
    def __init__(self):   # コンストラクタ
        super().__init__('my_move_node')        
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.sub = self.create_subscription(Odometry, 'odom', self.odom_cb, 10)   
        self.timer = self.create_timer(0.1, self.timer_callback)
        self.x, self.y, self.yaw = 0.0, 0.0, 0.0

        self.x0, self.y0, self.yaw0 = 0.0, 0.0, 0.0
        self.first = True

        self.vel = Twist()  # Twist メッセージ型インスタンスの生成
        self.set_vel(0.0, 0.0)  # 速度の初期化
        
 
    def get_pose(self, msg):      # 姿勢を取得する
        
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        q_x = msg.pose.pose.orientation.x
        q_y = msg.pose.pose.orientation.y
        q_z = msg.pose.pose.orientation.z
        q_w = msg.pose.pose.orientation.w
        (roll, pitch, yaw) = tf_transformations.euler_from_quaternion(
            (q_x, q_y, q_z, q_w))
        if self.first == True:
            self.x0 = x
            self.y0 = y
            self.yaw0 = yaw
            self.first = False
        return x, y, yaw
  
    def odom_cb(self, msg):         # オドメトリのコールバック関数
        self.x, self.y, self.yaw = self.get_pose(msg)
        self.get_logger().info(
            f'x={self.x: .2f} y={self.y: .2f}[m] yaw={self.yaw: .2f}[rad/s]')     
    
    def set_vel(self, linear, angular):  # 速度を設定する
        self.vel.linear.x = linear   # [m/s]
        self.vel.angular.z = angular  # [rad/s]    
    
    def move_distance(self, dist):  # 指定した距離distを移動する
        error = 0.05  # 許容誤差 [m] 
        diff = dist - math.sqrt((self.x-self.x0)**2 + (self.y-self.y0)**2)
        print('diff = ' + str(diff))
        print('x = ' + str(self.x))
        print('y = ' + str(self.y))
        print('x0 = ' + str(self.x0))
        print('y0 = ' + str(self.y0))
        
        if math.fabs(diff) > error:
            self.set_vel(0.1, 0.0)
            return False
        else:
            self.set_vel(0.0, 0.0)
            return True

    def rotate_angle(self, angle):  # 指定した角度angleを回転する
        error = 0.01
        diff = angle - self.yaw
        print('diff = ' + str(diff))
        if math.fabs(diff) > error:
            self.set_vel(0.0,0.25)
            return False
        else:
            self.set_vel(0.0, 0.0)
            return True

    def timer_callback(self):  # タイマーのコールバック関数
        self.pub.publish(self.vel)  # 速度指令メッセージのパブリッシュ
        print(self.vel)
        
    def my_move(self,  distance, angle):  # 簡単な状態遷移
        state = 0
        while rclpy.ok():
            if state == 0:
                print('distance = ' + str(distance))
                if self.move_distance(distance):
                    state = 1
                    print('move_distance')
            elif state == 1:
                if self.rotate_angle(angle):
                    self.pub.publish(self.vel) # 最後にロボットを止める
                    break
            else:
                print('エラー状態')
            rclpy.spin_once(self)


def main(args=None):  # main関数
    rclpy.init(args=args)
    node = MyMove()

    try:
        node.my_move(0.7, math.pi/2)
    except KeyboardInterrupt:
        print('Ctrl+Cが押されました．')     
    except ExternalShutdownException:
        sys.exit(1)
    finally:
        rclpy.try_shutdown()

```

・msg.pose.pose で取得する姿勢情報が最初から0でない場合があるので、初期値を0ではなく最初に読む値とした

・ロボットが回転したあとtimer_callback関数が呼ばれずに終了することがあるので最後に速度0をpublishするようにした

・回転角度についてはまだ適当です（スミマセン・・・）