이번 글은 별도의 참고자료가 존재하지 않고, 참고하는 블로그가 있는 경우 바로 다음에 표시를 하고 지나가도록 하겠습니다.
맵행렬과 gazebo의 동기화 문제
이전시간에 현재의 좌표와 이동하고 싶은 좌표를 입력하면 최단경로를 얻을 수 있게 만들었습니다
이제 이를 ROS상에서 구현하면됩니다. 직접 cmd_vel을 발행하는 방법을 한 번 해보고자 합니다.
여기서 한 가지 문제가 있습니다. 로봇이 다음 그리드로 이동하는데 몇 m를 이동해야하는가..
그리드를 분할하면 분할한 길이가 있을 것입니다. 이를 픽셀에 곱하면 이미지상에서 다음 위치로 이동하는 픽셀의 개수를 알 수 있습니다.
map.yaml 파일 보면 resolution이라는 값이 보일 것입니다. 이 값이 이 문제를 해결하는 열쇠입니다. 0.05는 1픽셀의 길이를 뜻합니다. 여기서 0.05는 0.05m를 의미한다고 합니다.
그러면 다음과 같이 진행을 해봅시다. resolution과 그리드의 가로 길이를 곱한값, 그리고 그리드와 세로 길이를 곱한값을 따로 구합니다.
# YAML 파일 경로
# 나중에 유저에게 맵 위치만 입력을 받게할 수 있을
yaml_path = "./map.yaml"
# YAML 파일 읽기
with open(yaml_path, "r") as stream:
yaml_data = yaml.safe_load(stream)
# resolution 값 가져오기
resolution = yaml_data["resolution"]
print(resolution)현재 map.yaml에서 resolution값을 가져오는 코드입니다.
결과는 이렇게 되겠죠 이 값을 그리드의 가로 세로와 곱함으로서 가제보상에서 로봇이 움직여야할 거리를 구합니다.
#그리드 한 칸의 크기와 resolution의 곱이 결국 이동한 m값이 됩니다.
garo_m = resolution * cell_width
sero_m = resolution * cell_height
print(garo_m, sero_m)
바닥에 있는 네모가 1m인가 봅니다. 편집툴을 사용해서 정확하게 잘라내었으니 정확한 값이 나오는 것 같습니다.
ROS로 Turtlebot3 움직이기
그러면 이제 cmd_vel 토픽을 파이썬 상에서 발행해봅시다. 다음의 과정을 따라하면됩니다.
(ROS 패키지로 만들지 않는 이유는 코드가 약간이라도 수정되면 colcon build를 계속 진행해야하지만 ipynb로 우선 작업하면 colcon build를 하지 않아도 되며, 나중에 완성되면 코드 단위로 옮길 수 있어서 편합니다.)
import rclpy
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile
from rclpy.node import Node다음과 같이 입력합니다. 여기서 문제가 하나 발생합니다.
(사진과 코드가 다릅니다. 위의 코드가 맞습니다.)
일단 VScode를 종료를 하고 다음을 다시 따라하면됩니다.
터미널에서 turtlebot환경을 불러온 후에 가상환경을 불러오고 code를 입력해 VScode 를 실행시킵니다.
다음의 과정으로 열면
정상적으로 입력되는 것을 볼 수 있습니다. 여튼 이제 cmd_vel 토픽을 발행해봅시다.
우선 turtlebot3 gazebo를 실행하도록 합니다.
ros2 launch turtlebot3_gazebo turtlebot3_duworld.launch.py
이 상태에서 다음을 입력합니다.
class my_publisher(Node):
def __init__(self):
super().__init__('move_turtlebot')
qos_profile = QoSProfile(depth=10)
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', qos_profile)
timer_period = 1
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0
def timer_callback(self):
msg = Twist()
msg.linear.x = 1.0
self.publisher_.publish(msg)
self.get_logger().info('move ->')
def main(args=None):
rclpy.init(args=args)
minimal_publisher = my_publisher()
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()노드를 만들었습니다. timer_callback은 1초마다 turtlebot을 움직이게 cmd_vel topic을 발행할 것입니다.
main()다음을 입력하면 turtlebot이 앞으로 나아갑니다.
그러면 지금까지 구한 경로를 따르게 할 수 있을까?
1. cmd_vel을 통해
지금부터는 코드를 짜야합니다. cmd_vel 토픽은 turtlebot 실행과 동시에 발행되는 토픽입니다. 그리고 위에서 보았듯이 x방향으로 1.0의 토픽을 발행하면 직진을 하게됩니다.
chatgpt에 따르면 1.0 topic을 발행하면 1.0m를 움직인다고하는데, 이게 맞는지는 모르겠습니다. 우선 한 칸부터 제대로 이동하는 것을 목표로 진행을 해보도록 하겠습니다.
위에서의 코드를 약간 수정합니다.
class my_publisher(Node):
def __init__(self):
super().__init__('move_turtlebot')
qos_profile = QoSProfile(depth=10)
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', qos_profile)
timer_period = 1
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0
def timer_callback(self):
msg = Twist()
msg.linear.x = 1.0
self.publisher_.publish(msg)
self.get_logger().info('move ->')
def main(args=None):
rclpy.init(args=args)
minimal_publisher = my_publisher()
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()이 코드에서 timer_callback부분을 수정합시다.
class my_publisher(Node):
def __init__(self):
super().__init__('move_turtlebot')
qos_profile = QoSProfile(depth=10)
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', qos_profile)
timer_period = 1
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = True
def timer_callback(self):
msg = Twist()
if self.i == True:
msg.linear.x = 1.0
self.i = False
else:
msg.linear.x = 0.0
self.publisher_.publish(msg)
self.get_logger().info('move ->')
def main(args=None):
rclpy.init(args=args)
minimal_publisher = my_publisher()
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()처음 1초만 1.0 으로 발행하고, 나머지는 0으로 발행함으로써 1.0으로 주었을때 1초간 얼마나 이동하는지를 체크하고자 합니다.
정확하게 이동하는 것으로 보입니다. 약간의 오차가 있을 수도 있다고 생각은 됩니다.
나중에 다룰 문제가 조금 있지만 현재로써는 잘움직인다고 판단이 됩니다.
이제 코드를 조금 짜보려고 합니다. 앞서 A* 알고리즘에서 pathj를 반환하게 코드를 작성했었었습니다.
이 path를 출력해보면 다음과 같습니다.
이 값을 따라서 로봇을 이동하게 만들면됩니다. 첫 번째는 로봇의 현재위치입니다. 알고리즘은 간단합니다.
xt+1 - xt 가 -1이면 앞으로 움직인 것이고, 1이면 뒤로 움직인 것입니다. 반대로 yt+1 - yt 가 1이면 오른쪽으로 이동한 것이고, -1이면 왼쪽으로 이동한 것입니다.
이를 참고해서 코드를 작성해봅시다.
이 부분에 추가를 합시다.
def move_my_bot(path, count):
if path[count+1][0] - path[count][0] == 1:
return 3
elif path[count+1][0] - path[count][0] == -1:
return 1
elif path[count+1][1] - path[count][1] == -1:
return 0
elif path[count+1][1] - path[count][1] == 1:
return 2다음과 같은 코드로 시작할 것입니다.
그리고 한 가지 문제점은 cmd_vel에서 앞으로 이동해라 라고 한다면 로봇이 바라보고 있는 앞으로 이동합니다.
그 말은 결국 오른쪽으로 움직여야한다면 각속도도 넣어주어야한다는 이야기이며, 로봇이 현재 방향을 알고 있어야 합니다.
그러면 새로운 변수로 robot_direction 변수를 만들고 다음처럼 지정하겠습니다.
| 1 (xt+1 - xt = -1) | ||
| 0 (yt+1 - yt = -1) | 로봇 | 2 (yt+1 - yt = 1) |
| 3 (xt+1 - xt = 1) |
북쪽을 바라보고 있다면 1 오른쪽을 바라보고 있다면 2를 줍니다. 뭐 이런 느낌입니다.
숫자로 표현한 이유는 알고리즘 생성에 편리함을 위함입니다. 만약에 현재의 방향이 1 의 방향인데, 왼쪽이냐 오른쪽이냐는 결국 다음 방향과 뺄셈을 하였을 때 -1 이 나오냐 1이 나오냐에 따라서 왼쪽으로 가야하는지 오른쪽으로 가야할지 판단을 할 수 있을 것입니다.
이 방법이 아니면 모든 경우의 수를 조건문으로 모두 따져야합니다.
def timer_callback(self):
msg = Twist()
if len(path) == self.count+1:
msg.angular.z = 0.0
msg.linear.x = 0.0
self.publisher_.publish(msg)
else:
next_point = move_my_bot(path, self.count)
self.count = self.count + 1
if self.robot_direction == next_point:
msg.angular.z = 0.0
msg.linear.x = 1.0
self.publisher_.publish(msg)
time.sleep(0.5)
msg.angular.z = 0.0
msg.linear.x = 1.0
self.publisher_.publish(msg)
self.robot_direction = next_point
elif self.robot_direction - next_point == -1 or self.robot_direction - next_point == 3:
msg.linear.x = 0.0
msg.angular.z = -1.0
self.publisher_.publish(msg)
time.sleep(0.5)
msg.angular.z = 0.0
msg.linear.x = 1.0
self.publisher_.publish(msg)
self.robot_direction = next_point
elif self.robot_direction - next_point == 1 or self.robot_direction - next_point == -3:
msg.linear.x = 0.0
msg.angular.z = 1.0
self.publisher_.publish(msg)
time.sleep(0.5)
msg.angular.z = 0.0
msg.linear.x = 1.0
self.publisher_.publish(msg)
self.robot_direction = next_point
elif self.robot_direction - next_point == 2:
pass
time.sleep(0.5)
msg.angular.z = 0.0
msg.linear.x = 0.0
self.publisher_.publish(msg)(사실 이렇게 짜면 안되긴 합니다..)
대충 이렇게 수정을 하고 일단 로봇이 어떻게 움직이는지 부터 체크를 해봅시다. 코드 해석은 간단하게 현재의 로봇 방향과 로봇이 움직여야하는 위치를 move_my_bot 함수에 넣으면 다음 이동해야할 위치는 동서남북 중 어디인지 판단하게됩니다.
그러면 그 값을 리턴받으면 우리는 이 값을 가지고 다시 판단합니다. 내 방향은 동쪽이니까 다음 포인트는 동쪽이면 서로 같은 값이므로 직진만합니다.
두 값의 차이에 따라서 왼쪽으로 이동할지 오른쪽으로 이동할지 판단합니다. 일단 돌려봅시다.
이동은 잘하는데, 정확한 위치로 이동했는지는 조금 의문입니다. 일단 저 짜증나는 라이다좀 지워버리겠습니다.
런치파일에서 어떠한 파일이 영향을 주고 있는지 타고타고 갈 수 있습니다. turtlebot3_gazebo 폴더에 있는 models폴더에 turtlebot3_burger 폴더의 model.sdf 파일에 들어갑니다.
다음처럼 수정하고 저장하고 다시 colcon build합니다.
라이다가 안보이게 되었습니다. 조금 편안하네요. 지금의 코드가 어떻게 움직이는지 한 번 확인을 해봅시다. 이제 수치적인 조정이 필요합니다. gif로 한 번 올려보겠습니다.
직진에서는 문제가 없는데, 회전을 하는 상황에서 문제가 있는 것 같습니다. 여기서부터는 수치적인 조정 문제로 보입니다.
수치를 한 시간 정도 만졌는데, 도저히 못맞추겠습니다..
2. Nav2 패키지를 통해서
사실 이전에 프로젝트를 진행하면서 이를 ROS상에서 구현한 적이 있습니다. 그때의 동작을 보면 이렇습니다.
이때는 cmd_vel을 발행하지 않고 각각의 좌표값들을 모두 구한 다음에 각 좌표지점으로 nav2 패키지를 통해 이동시키는 방식으로 구현했습니다.
이렇게 접근했던 이유가 nav2 패키지를 통해 이동시킬때 충돌영역(cost map) 으로 로봇이 들어가게 되면서 장거리를 이동하는 경우에 안정적인 주행이 불가능 했기 때문입니다.
그래서 cost map으로 분할을 하고 각 지점을 nav2가 알아서 이동하게 설계를 했으나, nav2를 사용하면서 많은 문제가 발생했었습니다. (장애물이 인식된 경우에도 nav2는 cancel되지 않고 계속 이동 명령을 내림)
이를 위해서는 그리드의 중앙점을 찾아내고, 이에 해당하는 좌표값을 모두 구해내야 합니다. 그리고 각 그리드의 중앙값들을 모두 변수로 저장해서
행과 열이 주어졌을 때 그에 해당하는 좌표값을 주도록 만들어야합니다. 그래서 resolution값이 굉장히 중요한 역할을 합니다.
또한 map.yaml파일에서 origin도 조절해야합니다. 상당히 많은 노가다를 요구합니다.
문제는 그러면 이게 Nav2를 사용한거지 않느냐? 이 말에 반박을 할 수가 없습니다. 그래서 저는 이 방법을 했던 적이 있다 정도로만 소개하고, 이후로는 조금 정석적인 방법을 따라가고자 합니다.
경로추종의 문제는 경로생성의 다음의 이야기 입니다. 그리드로 나누고 실제로 경로를 생성하고, 어떻게든 로봇을 이동시킬 수는 있다 이를 보여드리고 싶었습니다.
다음 시간에는 경로 생성 알고리즘인 A* 알고리즘을 분석한 이후에 경로를 추종하게 하는 알고리즘인 pure pursuit에 대해서 공부를 진행하도록 하겠습니다.
그 후에 ROS 패키지를 만드는게 좋을 것 같습니다. 지금 괜히 힘들여서 ROS 패키지를 만들어봤자. 동작하지 않는 문제가 많은 코드로는 어려울 것 같다고 생각합니다.
지금처럼 그리드로 분할하는 방법이 아니라 픽셀단위로 분할을 하고 그 안에 cost map을 설정해주고, 로봇이 움직이게 만들어봅시다.
'공부#Robotics#자율주행 > (ROS2)Path Planning' 카테고리의 다른 글
| ROS2로 turtlebot3 제어하기 6장. A* 알고리즘 탐구 (0) | 2023.06.26 |
|---|---|
| ROS2로 turtlebot3 제어하기 4장. map파일을 가지고 전역경로 생성하기 (0) | 2023.06.03 |
| ROS2로 turtlebot3 제어하기 3장. Nav2 이해하기와 파이썬 환경설정 (0) | 2023.06.03 |
| ROS2로 turtlebot3 제어하기 2장. MAP 만들기 (0) | 2023.06.02 |
| ROS2로 turtlebot3 제어하기 1장. 경로계획(Path planning)의 개요 (0) | 2023.06.02 |