Windows で ROS2 のシミュレーションを行う (4)
TurtleBot3 をプログラムで制御する

• TurtleBot3 の手動操作
• TurtleBot3 のプログラムによる制御
• TurtleBot3 のプログラムによる自律制御

TurtleBot3 の手動操作

 ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

• 仮想空間の拡大率の変更: マウスホイールの回転
• 画面内での仮想空間の位置の変更: マウスを左クリックしながらのドラッグ

wget http://brain.cc.kogakuin.ac.jp/~kanamaru/lecture/ROS/download_models.sh

sh download_models.sh

 ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py

 ros2 run teleop_twist_keyboard teleop_twist_keyboard

• i : 前進。一度押す k を入力するまで動き続ける
• j : 左旋回。一度押すと k を入力するまで動き続ける
• k : 停止。一度押すとすぐに停止する
• l : 右旋回。一度押すと k を入力するまで動き続ける
• < : 後退。一度押すと k を入力するまで動き続ける

• q : 直進速度と回転速度が 10% アップ
• z : 直進速度と回転速度が 10% ダウン
• w : 直進速度のみ 10% アップ
• x : 直進速度のみ 10% ダウン
• e : 回転速度のみ 10% アップ
• c : 回転速度のみ 10% ダウン

 ros2 run rqt_graph rqt_graph

TurtleBot3 のプログラムによる制御

 ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import sys
from geometry_msgs.msg import Twist
from rclpy.clock import Clock, ClockType
from rclpy.time import Duration

class Publisher(Node):
    def __init__(self):
        super().__init__('publisher')
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)

        self.move_cmd = Twist()
        self.stop_cmd = Twist()

        self.move_cmd.linear.x = 0.2
        self.move_cmd.linear.y = 0.0
        self.move_cmd.linear.z = 0.0
        self.move_cmd.angular.x = 0.0
        self.move_cmd.angular.y = 0.0
        self.move_cmd.angular.z = 0.0

        self.duration = 2 # sec
        self.stop_time = Clock(clock_type=ClockType.ROS_TIME).now() + Duration(seconds=self.duration)
        self.timer_period = 0.1  # seconds
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def __del__(self):
        self.publisher.publish(self.stop_cmd)

    def timer_callback(self):
        self.publisher.publish(self.move_cmd)
        if Clock(clock_type=ClockType.ROS_TIME).now() > self.stop_time:
            self.publisher.publish(self.stop_cmd)
            self.timer.cancel()
            sys.exit(0)

def main():
    rclpy.init(args=sys.argv)
    pub = Publisher()
    rclpy.spin(pub)

    pub.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

1. 「保存」ボタンをクリック
2. 「colcon_ws」→「src」→「ros_practice」→「ros_practice」とダブルクリックしてディレクトリをたどる
3. 最上部の「無題のドキュメント」の部分を「move1.py」と書き換える。これが保存するファイル名となり、つづりを間違えると後で困るのでコピー＆貼り付けが確実
4. 右上の「保存」ボタンで保存完了。

(略)

    entry_points={
        'console_scripts': [
            'autonomous_controller = ros_practice.autonomous_controller:main'
        ],
    },
)

(略)

    entry_points={
        'console_scripts': [
            'autonomous_controller = ros_practice.autonomous_controller:main',
            'move1 = ros_practice.move1:main'
        ],
    },
)

1. 「'autonomous_controller' ...」の行の末尾にコンマ「,」を追記する。
2. 「'autonomous_controller' ...」の次の行に行が追加されている

 cd ~/colcon_ws/

 colcon build

 ros2 run ros_practice move1

        self.move_cmd = Twist()
(中略)
        self.move_cmd.linear.x = 0.2
        self.move_cmd.linear.y = 0.0
        self.move_cmd.linear.z = 0.0
        self.move_cmd.angular.x = 0.0
        self.move_cmd.angular.y = 0.0
        self.move_cmd.angular.z = 0.0
(中略)
        self.publisher.publish(self.move_cmd)

        self.stop_cmd = Twist()
(中略)
            self.publisher.publish(self.stop_cmd)

duration = 2 # sec

TurtleBot3 のプログラムによる自律制御

 ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
import sys
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from rclpy.qos import qos_profile_sensor_data
from rclpy.clock import Clock, ClockType
from rclpy.time import Duration
import numpy as np

class PublisherSubscriber(Node):
    def __init__(self):
        super().__init__('publisher_subscriber')
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)

        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.subscription_callback,
            qos_profile=qos_profile_sensor_data)
        self.subscription  # prevent unused variable warning

        self.move_cmd = Twist()
        self.rotate_cmd = Twist()
        self.stop_cmd = Twist()

        self.move_cmd.linear.x = 0.2
        self.move_cmd.linear.y = 0.0
        self.move_cmd.linear.z = 0.0
        self.move_cmd.angular.x = 0.0
        self.move_cmd.angular.y = 0.0
        self.move_cmd.angular.z = 0.0

        self.rotate_cmd.linear.x = 0.0
        self.rotate_cmd.linear.y = 0.0
        self.rotate_cmd.linear.z = 0.0
        self.rotate_cmd.angular.x = 0.0
        self.rotate_cmd.angular.y = 0.0
        self.rotate_cmd.angular.z = 0.2

        self.distance = 100

    def __del__(self):
        self.publisher.publish(self.stop_cmd)

    def subscription_callback(self, data):
        data_float = np.array(data.ranges)
        data_float = np.where(data_float<data.range_min, data.range_max, data_float)
        self.distance = min(min(data_float[0:30]), min(data_float[330:360]))

        if self.distance < 0.5:
            self.publisher.publish(self.rotate_cmd)
        else:
            self.publisher.publish(self.move_cmd)

def main():
    rclpy.init(args=sys.argv)
    pubsub = PublisherSubscriber()
    rclpy.spin(pubsub)

    pubsub.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

1. 「保存」ボタンをクリック
2. 「colcon_ws」→「src」→「ros_practice」→「ros_practice」とダブルクリックしてディレクトリをたどる
3. 最上部の「無題のドキュメント」の部分を「auto.py」と書き換える。これが保存するファイル名となり、つづりを間違えると後で困るのでコピー＆貼り付けが確実
4. 右上の「保存」ボタンで保存完了。

(略)

    entry_points={
        'console_scripts': [
            'autonomous_controller = ros_practice.autonomous_controller:main',
            'move1 = ros_practice.move1:main'
        ],
    },
)

(略)

    entry_points={
        'console_scripts': [
            'autonomous_controller = ros_practice.autonomous_controller:main',
            'move1 = ros_practice.move1:main',
            'auto = ros_practice.auto:main'
        ],
    },
)

1. 「'move1' ...」の行の末尾にコンマ「,」を追記する。
2. 「'move1' ...」の次の行に行が追加されている

 cd ~/colcon_ws/

 colcon build

 ros2 run ros_practice auto

        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.subscription_callback,
            qos_profile=qos_profile_sensor_data)

(中略)

    def subscription_callback(self, data):
        data_float = np.array(data.ranges)
        data_float = np.where(data_float<data.range_min, data.range_max, data_float)
        self.distance = min(min(data_float[0:30]), min(data_float[330:360]))

        data_float = np.array(data.ranges)
        data_float = np.where(data_float<data.range_min, data.range_max, data_float)

        self.stop_cmd = Twist()

(中略)

        def __del__(self):
        self.publisher.publish(self.stop_cmd)

おまけ：最小距離となる位置 (角度) を知る

        self.distance = 100
        self.close_position = 0

        self.distance = min(min(data_float[0:30]), min(data_float[330:360]))
        pos_left = np.argmin(data_float[0:30])
        pos_right = 330 + np.argmin(data_float[330:360])
        self.close_position = pos_left if data_float[pos_left] < data_float[pos_right] else pos_right
        #self.get_logger().info('{} {}'.format(self.close_position, self.distance))

        self.close_position = np.argmin(data_float)