ROS2プログラミング演習_03: Lidarからの信号でロボットを動かすプログラムの自作 [Gazebo]

## はじめに
Gazeboシミュレータでturtlebot3を動かします．
このページではLidarからの信号をもとにロボットを制御するプログラムを自作します．

#### 環境
・OS: Windows11 Pro (64bit)
・ROS: ROS2 humble (WSL2のUbuntu22.04に)

ここまでの環境構築については下記参照
・WSL2にROS2を入れてgazeboでシミュレーションするまでの手順
https://qiita.com/pez/items/16011df0e663ceac694a



## 1. ワークスペース作成・準備
今回は lidar_ws としておきます
```ruby:
$ mkdir -p ~/lidar_ws/src/
```

#### 今回必要なライブラリ
入っていなければ入れておく
sudo apt install ros-humble-tf-transformations
sudo pip3 install transforms3d


## 2. パッケージ作成
今回ノード名は my_lidar_node
パッケージ名は my_lidar
にすることにします
```ruby:
$ cd ~/lidar_ws/src
$ ros2 pkg create --build-type ament_python --node-name my_lidar_node my_lidar
```


## 3. プログラミング
#### 3.1 package.xml ファイルの編集
```ruby:
$ cd ~/lidar_ws/src/my_lidar
$ emacs package.xml
```
自作パッケージを一般公開するときには変更する必要がありますが，今回も特になにもしません．

#### 3.2 setup.py ファイルの編集
```ruby:
$ cd ~/lidar_ws/src/my_lidar
$ emacs setup.py
```
23行目を確認
ノード名=パッケージ名.ノード名:main
となっている必要があります．今回もパッケージを作成する際に --node-name のオプションを付けているので自動的に下記のようになっているはずです．
```ruby:
entry_point={
    'console_scripts': [
        'my_lidar_node = my_lidar.my_lidar_node:main' 
    ],
},
```
※もし１つのパッケージに複数のPythonファイルがある場合はそのファイルごとにエントリポイントを指定しなければならないので setup.py を変更する必要があります．

#### 3.3 ソースコード作成
今回もパッケージ作成時に自動的に作られている my_lidar_node.py を編集することにします．
```ruby:
$ cd ~/lidar_ws/src/my_lidar/my_lidar
$ emacs my_lidar_node.py
```
下記のように記述してみましょう．
```ruby:my_lidar_node.py

import math
import os
import sys
import rclpy
import rospkg
import time
import tf_transformations
from rclpy.node import Node   
from rclpy.executors import ExternalShutdownException    
from geometry_msgs.msg import Twist, Pose, Point  # Twistメッセージ型をインポート
from nav_msgs.msg import Odometry                 # Odometryメッセージ型をインポート
from sensor_msgs.msg import LaserScan             # LaserScanメッセージ型をインポート
from tf_transformations import euler_from_quaternion 
from ament_index_python.packages import get_package_share_directory

from rclpy.qos import qos_profile_sensor_data, QoSProfile

class MyLidar(Node):  # 簡単なLiDARクラス
    def __init__(self):   # コンストラクタ
        super().__init__('my_lidar_node')   
        self.timer = self.create_timer(0.01, self.timer_callback)
        self.pub = self.create_publisher(Twist, 'cmd_vel', 10) 
        self.sub = self.create_subscription(Odometry, 'odom', self.odom_cb, 10)
        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.vel = Twist()      # Twistメッセージ型インスタンスの生成
        self.set_vel(0.0, 0.0)  # 速度の初期化
        # LiDARを使うため追加
        qos_policy = QoSProfile(reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
                                history=rclpy.qos.HistoryPolicy.KEEP_LAST,
                                depth = 5)
        self.sub = self.create_subscription(LaserScan, 'scan', self.lidar_cb, qos_profile=qos_policy)
        #self.sub = self.create_subscription(LaserScan, 'scan', self.lidar_cb, 1 )
        self.scan = LaserScan()  # LaserScanメッセージ型インスタンスの生成
        self.scan.ranges = [-99.9] * 360 # 取得したデータと区別するためありえない値で初期化
        self.snum = 360
        
    def lidar_cb(self, msg):  # LiDARのコールバック関数
        #print('kokokiteru')
        self.scan = msg
        
        print('range(len(msg.ranges))=' + str(len(msg.ranges)))
        self.snum = len(msg.ranges)
        #time.sleep(1)
        
        for i in range(len(msg.ranges)):
            if msg.ranges[i] < msg.range_min or msg.ranges[i] > msg.range_max:
                pass # 計測範囲外のデータは使わないような処理が必要．ここではパス．
            else:
                self.scan.ranges[i] = msg.ranges[i] 
    
    def my_lidar(self): 
        steps = 0
        self.set_vel(0.0, 0.0)      # 停止  
        rclpy.spin_once(self)       # コールバック関数をよび出す  

        while rclpy.ok():
            print(f'step={steps}')
            #Lidarの角度分解能にあわせて調整
            nscan = float(self.snum)
            li_right = int(270.0*(nscan/360.0))
            li_left  = int( 90.0*(nscan/360.0))
            li_front = 0
            li_back  = int(180.0*(nscan/360.0))

            
            dist = 0.5                      # 前進してOKの距離   
            if self.scan.ranges[li_front] > dist:  # 
                self.set_vel(0.2, 0.0)        # 前進 
            else:
                self.set_vel(0.0, 0.0)        # 停止
                
            rclpy.spin_once(self)
            #self.print_lidar_info() # scanトピックの値を表示するときはコメントアウト
            print(f'r[FRONT]={self.scan.ranges[li_front]}')     # 前
            print(f'r[ LEFT]={self.scan.ranges[li_left]}')    # 左
            print(f'r[ BACK]={self.scan.ranges[li_back]}')   # 後
            print(f'r[RIGHT]={self.scan.ranges[li_right]}')   # 右
  
            time.sleep(0.1)  # 0.1 [s]
            steps += 1
    
    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))
        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) 
        if math.fabs(diff) > error:
            self.set_vel(0.25, 0.0)
            return False
        else:
            self.set_vel(0.0, 0.0)
            return True

    def rotate_angle(self, angle):  # 指定した角度angleを回転する
        # このメソッドは間違っています．move_distanceを参考に完成させてください．
        self.set_vel(0.0, 0.25)
        return False

    def timer_callback(self):  # タイマーのコールバック関数
        self.pub.publish(self.vel)  # 速度指令メッセージのパブリッシュ 


def main():  # main関数
    rclpy.init()
    node = MyLidar()
    try:
        node.my_lidar()
    except KeyboardInterrupt:
        print('Ctrl+Cが押されました．')     
    finally:
        rclpy.shutdown()






```

## 4. ビルド
ワークスペース名直下のディレクトリ以外はビルドできません
初めてビルドしたとき build, install, log ディレクトリが作成されます
```ruby:
$ cd ~/lidar_ws
$ colcon build
```
成功すると小さな別ウィンドウで 'colcon build' succesfull と表示されます

## 5. 設定ファイルの反映
アンダーレイ設定ファイル（ROS2システムの設定）は .bashrc に記述済みとします
※下記が.bashrcに既にある
source /opt/ros/humble/setup.bash

オーバーレイ設定ファイル（自作ワークスペースの設定）を .bashrc に追記
```ruby:.bashrcの編集
$ emacs ~/.bashrc
```
$ source ~/lidar_ws/install/setup.bash
を追加してから反映させる
```ruby:
$ source ~/.bashrc
```

## 6. ノードの実行（シミュレーション）

#### gazeboでTurtlebot3のシミュレーションを立ち上げる
```ruby:六角形のワールドの場合
$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```
#### または別のワールド（下記は家）でもよい
```ruby:家環境の場合
$ ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py
```

### 作ったノードの立ち上げ
```ruby:別のターミナルにて
$ ros2 run my_lidar my_lidar_node
```

ロボット前方 0.5m 以内に障害物がなければロボットは前進し続け，障害物があれば停止するという動作をすれば成功です．


## 7. 演習
Lidarからの障害物情報をもとに，ロボットに色々な動きをさせてみましょう．
→ 障害物をさけながら色々なところを徘徊させられるでしょうか？

またプログラムの中に書いてあるように関数 rotate_angle は未完成なので，指定された角度でロボットを停止させる関数として完成させて下さい



MEMO
＜プログラムを更新したときは＞
　1.プログラムの中身を編集して保存
　2.ビルド
　3.設定ファイルの反映
　4.ノードの実行
　→ 動作を確認