Gazebo에서 WASD로 TurtleBot 움직이기

"Ubuntu 26 + ROS 2 Lyrical + Gazebo에서 WASD로 TurtleBot 움직이기"

환경은 Ubuntu 26.04 + ROS 2 Lyrical + Gazebo + TurtleBot3 Burger입니다.

 

명령어는 ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

 

먼저 ROS 2 버전을 확인합니다.

   echo $ROS_DISTRO

결과는 다음과 같습니다.

   lyrical


Gazebo에서 TurtleBot이 실행된 상태에서 토픽을 확인합니다.

   ros2 topic list

현재 주요 토픽은 다음과 같습니다.

/clock
/cmd_vel
/imu
/joint_states
/odom
/parameter_events
/robot_description
/rosout
/scan
/tf
/tf_static


  /cmd_vel 토픽을 확인합니다.

  ros2 topic info /cmd_vel

현재 환경에서는 다음과 같이 확인됩니다.

Type: ['geometry_msgs/msg/Twist', 'geometry_msgs/msg/TwistStamped']
Publisher count: 1
Subscription count: 1


현재 Gazebo의 TurtleBot은 TwistStamped 형식으로 입력했을 때 정상적으로 움직이는 환경입니다.
( 만약, TwistStamped 가 아니면 Twist 명령어를 입력해 주세요. )

1. TurtleBot 전진 테스트입니다.

ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: ''}, twist: {linear: {x: 0.2}, angular: {z: 0.0}}}"


2. TurtleBot 왼쪽 회전 테스트입니다.

ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: ''}, twist: {linear: {x: 0.0}, angular: {z: 0.5}}}"


3. TurtleBot 오른쪽 회전 테스트입니다.

ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: ''}, twist: {linear: {x: 0.0}, angular: {z: -0.5}}}"


4. TurtleBot 정지입니다.

ros2 topic pub --once /cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: ''}, twist: {linear: {x: 0.0}, angular: {z: 0.0}}}"


5. ROS 2 작업공간을 생성합니다.

  mkdir -p ~/ros2_ws/src
  cd ~/ros2_ws/src


6. Python ROS 2 패키지를 생성합니다.

  ros2 pkg create --build-type ament_python turtlebot_wasd --dependencies rclpy geometry_msgs


7. Python 파일을 생성합니다.

  cd ~/ros2_ws/src/turtlebot_wasd/turtlebot_wasd

  nano wasd_teleop.py

8. wasd_teleop.py 내용입니다.

#!/usr/bin/env python3

import sys
import termios
import tty
import select

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped


HELP = """
--------------------------------
 TurtleBot WASD Teleop
--------------------------------

 W : Forward
 S : Backward
 A : Turn Left
 D : Turn Right

 X : Stop
 Space : Stop

 R : Linear Speed Up
 F : Linear Speed Down

 T : Angular Speed Up
 G : Angular Speed Down

 Q : Quit

--------------------------------
"""


class WasdTeleop(Node):

    def __init__(self):

        super().__init__('wasd_teleop')

        self.publisher_ = self.create_publisher(
            TwistStamped,
            '/cmd_vel',
            10
        )

        self.linear_speed = 0.20
        self.angular_speed = 0.60

        self.get_logger().info('WASD Teleop started')

        print(HELP)

        self.show_speed()


    def publish_velocity(self, linear_x, angular_z):

        msg = TwistStamped()

        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = ''

        msg.twist.linear.x = float(linear_x)
        msg.twist.linear.y = 0.0
        msg.twist.linear.z = 0.0

        msg.twist.angular.x = 0.0
        msg.twist.angular.y = 0.0
        msg.twist.angular.z = float(angular_z)

        self.publisher_.publish(msg)


    def stop(self):

        self.publish_velocity(0.0, 0.0)


    def show_speed(self):

        print(
            f'\rLinear = {self.linear_speed:.2f} m/s   '
            f'Angular = {self.angular_speed:.2f} rad/s',
            end='',
            flush=True
        )


def get_key(settings):

    tty.setraw(sys.stdin.fileno())

    rlist, _, _ = select.select(
        [sys.stdin],
        [],
        [],
        0.1
    )

    if rlist:

        key = sys.stdin.read(1)

    else:

        key = ''

    termios.tcsetattr(
        sys.stdin,
        termios.TCSADRAIN,
        settings
    )

    return key


def main(args=None):

    rclpy.init(args=args)

    node = WasdTeleop()

    settings = termios.tcgetattr(sys.stdin)

    try:

        while rclpy.ok():

            key = get_key(settings)

            if key.lower() == 'w':

                node.publish_velocity(
                    node.linear_speed,
                    0.0
                )

            elif key.lower() == 's':

                node.publish_velocity(
                    -node.linear_speed,
                    0.0
                )

            elif key.lower() == 'a':

                node.publish_velocity(
                    0.0,
                    node.angular_speed
                )

            elif key.lower() == 'd':

                node.publish_velocity(
                    0.0,
                    -node.angular_speed
                )

            elif key.lower() == 'x' or key == ' ':

                node.stop()

            elif key.lower() == 'r':

                node.linear_speed += 0.05

                node.show_speed()

            elif key.lower() == 'f':

                node.linear_speed = max(
                    0.05,
                    node.linear_speed - 0.05
                )

                node.show_speed()

            elif key.lower() == 't':

                node.angular_speed += 0.10

                node.show_speed()

            elif key.lower() == 'g':

                node.angular_speed = max(
                    0.10,
                    node.angular_speed - 0.10
                )

                node.show_speed()

            elif key.lower() == 'q':

                node.stop()

                break

            rclpy.spin_once(
                node,
                timeout_sec=0.0
            )

    except Exception as e:

        node.get_logger().error(str(e))

    finally:

        node.stop()

        termios.tcsetattr(
            sys.stdin,
            termios.TCSADRAIN,
            settings
        )

        node.destroy_node()

        rclpy.shutdown()


if __name__ == '__main__':

    main()

wasd_teleop.py

0.00MB

9. 저장 후 실행 권한을 설정합니다.

chmod +x ~/ros2_ws/src/turtlebot_wasd/turtlebot_wasd/wasd_teleop.py


10. setup.py 파일을 수정합니다.

nano ~/ros2_ws/src/turtlebot_wasd/setup.py


11. setup.py 내용입니다.

from setuptools import find_packages, setup

package_name = 'turtlebot_wasd'

setup(

    name=package_name,

    version='0.0.0',

    packages=find_packages(exclude=['test']),

    data_files=[

        (
            'share/ament_index/resource_index/packages',
            ['resource/' + package_name]
        ),

        (
            'share/' + package_name,
            ['package.xml']
        ),

    ],

    install_requires=['setuptools'],

    zip_safe=True,

    maintainer='user',

    maintainer_email='user@example.com',

    description='WASD teleop for TurtleBot using TwistStamped',

    license='Apache-2.0',

    tests_require=['pytest'],

    entry_points={

        'console_scripts': [

            'wasd_teleop = turtlebot_wasd.wasd_teleop:main',

        ],

    },

)


12. ROS 2 환경을 적용합니다.

source /opt/ros/lyrical/setup.bash


13. 작업공간으로 이동합니다.

cd ~/ros2_ws


14. 패키지를 빌드합니다.

colcon build --symlink-install


15. 빌드된 작업공간을 적용합니다.

source ~/ros2_ws/install/setup.bash


16. WASD 제어 노드를 실행합니다.

ros2 run turtlebot_wasd wasd_teleop


17. 키보드 조작 방법입니다.

W : 전진입니다.

S : 후진입니다.

A : 왼쪽 회전입니다.

D : 오른쪽 회전입니다.

X : 정지입니다.

Space : 정지입니다.

R : 직진 속도 증가입니다.

F : 직진 속도 감소입니다.

T : 회전 속도 증가입니다.

G : 회전 속도 감소입니다.

Q : 프로그램 종료입니다.


18. 기본 속도입니다.

직진 속도는 0.20 m/s입니다.

회전 속도는 0.60 rad/s입니다.


19. /cmd_vel 메시지 확인 방법입니다.

ros2 topic echo /cmd_vel geometry_msgs/msg/TwistStamped


20. 실행 시 전체 명령 순서입니다.

source /opt/ros/lyrical/setup.bash

source ~/ros2_ws/install/setup.bash

ros2 run turtlebot_wasd wasd_teleop


21. Gazebo와 함께 사용할 때 실행 순서입니다.

첫 번째 터미널에서는 Gazebo와 TurtleBot을 실행합니다.

두 번째 터미널에서는 다음 명령을 실행합니다.

source /opt/ros/lyrical/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 run turtlebot_wasd wasd_teleop

이후 W, A, S, D 키를 이용하여 Gazebo의 TurtleBot을 움직이면 됩니다.