Turtlebot3 좌표값 입력 받아 이동시키기

1. 파일 생성

# ~/ros2_ws/src/[pakcage_name]/[package_name]
touch [파일이름.py]

# 권한 부여
sudo chmod +x [파일이름.py]

# 해당 파일 편집
gedit [파일이름.py]

[go2goal.py 파일 코드]

## 파이썬 파일 생성
[go2goal.py]
```python
#!/usr/bin/env python

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from math import pow, sqrt, atan, pi

from rclpy.duration import Duration

MAX_LIN_X = 0.22
MAX_ANG_Z = 2.82


class TurtleBot3(Node):

    def __init__(self):
        super().__init__('turtlebot_controller')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.move2goal)

        self.lin_x = MAX_LIN_X / 2
        self.ang_z = MAX_ANG_Z / 2

        self.wise = 1

    def get_dist(self, x, y):
        return sqrt(pow(abs(x), 2) + pow(abs(y), 2))

    def get_angle(self, x, y):
        if (x >= 0 and y >= 0):  # case 1: +0
            return atan(abs(y) / abs(x))
        elif (x >= 0 and y < 0):  # case 2: -0
            return -atan(abs(y) / abs(x))
        elif (x < 0 and y < 0):  # case 3: -(pi-0)
            return -(pi - atan(abs(y) / abs(x)))
        elif (x < 0 and y >= 0):  # case 4: (pi-0)
            return pi - atan(abs(y) / abs(x))

    def move2goal(self):
        goal_x = float(input("Set your x goal: "))
        goal_y = float(input("Set your y goal: "))

        dist = self.get_dist(goal_x, goal_y)
        angle = self.get_angle(goal_x, goal_y)

        if angle < 0:
            angle = -angle
            wise = -1
        else:
            wise = 1

        time2turn = Duration(seconds=int(angle / self.ang_z))
        time2go = Duration(seconds=int(dist / self.lin_x))

        twist = Twist()

        twist.angular.z = self.ang_z * wise
        time2end = self.get_clock().now() + time2turn

        self.pub.publish(twist)
        while self.get_clock().now().to_msg() < time2end:
            pass

        twist.angular.z = 0
        self.pub.publish(twist)

        twist.linear.x = self.lin_x
        time2end = self.get_clock().now() + time2go

        self.pub.publish(twist)
        while self.get_clock().now().to_msg() < time2end:
            pass

        twist.linear.x = 0
        self.pub.publish(twist)

        wise = -wise

        twist.angular.z = self.ang_z * wise
        time2end = self.get_clock().now() + time2turn

        self.pub.publish(twist)
        while self.get_clock().now().to_msg() < time2end:
            pass

        twist.angular.z = 0
        self.pub.publish(twist)


def main(args=None):
    rclpy.init(args=args)
    turtlebot = TurtleBot3()
    rclpy.spin(turtlebot)
    turtlebot.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

2. Turtlebot 연결

• 새 터미널 열기

ssh ubuntu@[주소]

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_bringup robot.launch.py

3. go2goal.py 파일 실행

• 이전 터미널

cd ~/ros2_ws/

ros2 run [패키지 이름] go2goal