ROS move_base를 이용한 주행 - python 편 -

최근에 저는 국민대학교에서 주행로봇을 이용해서 겨울방학 특강을 진행했었는데요. 그리고 그 강의가 원격으로 진행되다 보니 수강하는 학생들에게 부족한 부분을 보완하기 위해 영상을 유투브에 공개하기로 결정을 했죠.

www.youtube.com/playlist?list=PL83j7f4UkozHPUshNQfPpogJBE0gSnCYH

OMO R1mini로 진행하는 AI Robot 수업

그 강의 목록이 위의 링크입니다. 강의 당시 강의에서는 틈틈이 이야기를 했는데 강의를 다시 유투브에 올리는 과정에서 빼먹은 내용이 몇 가지 있습니다. 오늘은 그 중 하나를 이야기를 하려고 합니다. 

youtu.be/8r6x4sKu4EM

강의를 진행하던 중 저는 Jupyter Notebook으로 로봇을 다루는 부분을 했었는데요. 수업때도 몇번 이야기를 했지만, Jupyter Notebook으로는 로봇을 다룰때는 기능 테스트를 빠르게 진행하는 용도로 괜찮습니다. 저 역시 그런 용도로 설명을 했는데요. 로봇하는 분들에게는 생소할 수 있지만, 기능을 빠르게 확인하고 학습하고 적용해 볼때 꽤 편합니다. 이렇게 테스트하고 공부한 다음 패키지로 만드는 것을 수행하면 되는거죠 이미 기능 학습은 끝났으니까요.

youtu.be/-VZnrHv2KOs

그리고, 나서 위 영상을 통해 여러분들은 이미 SLAM으로 맵을 만들고 네비게이션을 최소한 RVIZ에서 다룰 줄 안다고 보겠습니다. 보통은 이 상태에서 아마도

위와 같은 RVIZ 화면에서 초기자세를 2D Pose Estimate 하고 로봇이 가야할 곳을 2D Nav Goal로 지정하는 정도까지 된 것으로 보죠.^^. 이제 그리고 나면 어떻게 할 건지 보죠. 이번 포스팅의 전체 과정도 

youtu.be/fA5u9XYcrv8

위 동영상에 담겨있습니다. 이제 바로 위 동영상과 글을 함께 보시면서 따라해보시면 좋을것 같습니다.

일단 위 그림처럼 4개의 터미널에서 저는 우측 두 개의 터미널에 ssh로 로봇에 접근합니다. 이 내용은 AI Robot 수업 09라는 동영상에서 설명을 했습니다.

위 그림처럼 터미널이 구성되면 네비게이션이 실행될겁니다. 지금 글은 R1mini라는 로봇을 대상으로 하고 있지만, 어떤 주행로봇이든 상관없습니다. amcl로 localization을 하면서 move_base로 로봇을 움직이는 경우는 가능합니다. 일단 위 터미널을 설명하면 아래와 같습니다. 실행 순서도 아래 설명의 순서로 하나씩 터미널 명령을 실행하면 됩니다.

• [좌측상단] roscore : 로봇을 MASTER_URI 설정을 통해 원격으로 제어하는 경우 내 PC에서 roscore를 실행합니다. 그게 여러모로 편하더군요.
• [우측상단] bringup : 어떤 로봇이든 로봇을 구동하는 bringup 패키지를 지원할 겁니다. 지금 화면에는 omo r1mini라는 로봇의 bringup 패키지이구요. 거기서 실행한 launch 파일은 원래라면 omo_r1mini_robot.launch여야하는데, 지금은 국민대 특강으로 구성된 기구여서 omo_r1mini_robot_hlds.launch입니다. 이 글을 읽는 omo r1mini 로봇을 구매하신 분들은 omo_r1mini_robot.launch를 실행하시면 됩니다.
• [우칙하단] navigation.launch : 이미 slam을 통해 지도는 완성했다고 보고 mova_baase와 amcl 등을 실행하는 navigation을 실행합니다. 여기까지는 흔히 알고 있는 rviz가 실행되지 않습니다. 인터넷 연결 문제등이 있을 수 있으니, 로봇쪽 PC의 파워가 충분하다면 이 명령은 로봇에서 실행하고, rviz만 따로 원격으로 로봇을 조종하는 PC에서 실행하면 됩니다.
• [좌측하단] navigation_rviz.launch : 여기는 단지 navigation pkg의 rviz 화면만 보고 있습니다. r1mini라는 로봇이 없어도 여러분이 가진 로봇의 pkg대로 실행하면 됩니다.

RVIZ에서 저렇게 그림이 나타날겁니다. 위에 보이는 2D Pose Estimate 버튼을 눌러서 자기 위치를 잡아줍니다.

이제 위 그림처럼 자기 위치도 잡았다면 화면, 그러면 위에 AI Robot 수업 16번 동영상에서 언급한 대로 Jupyter Notebook이 설치되었다고 보겠습니다. 아 Juptyer Notebook은 설치 후 PC를 재부팅해야 하는 것으로 기억나네요.

그리고 저는 저렇게 화면을 자주 배치합니다.^^. 

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from math import radians, degrees, pi, sin, cos
from actionlib_msgs.msg import *
from geometry_msgs.msg import PoseWithCovarianceStamped
from copy import deepcopy
import tf

이제 위와 같이 필요한 모듈을 import합니다. 그리고, Juptyer notebook이지만 노드를 초기화해주어야 합니다.

rospy.init_node('map_navigation', anonymous=False)
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)

ROS에서 navigation 패키지를 실행하면 move_base와 함께 보통 실행되는 것이 amcl입니다. 여기서 두 개의 토픽만 그 의미를 살짝꿍 확인해볼까요.

• odom : 로봇이 측정한 자신의 위치입니다. 고전적으로는 바퀴의 엔코더같은 센서를 이용해서 바퀴가 회전하면서 진행된 거리를 계산합니다. 실제 로봇의 위치값과는 오차가 있어서 조금 더 잘 사용하려는 경우 IMU 같은 장비를 함께 사용하기도 합니다.
• amcl_pose : odom에서 전달해주는 로봇의 위치와 라이다와 같은 장치의 신호를 가지고 맵위에서 로봇의 위치를 추정해서 발행되는 토픽입니다.

우리는 지금은 맵 위에서 로봇의 움직임을 어떻게 하고 싶으니 amcl_pose라는 토픽을 subcribe할 겁니다.

current_pose = PoseWithCovarianceStamped()

def callback(msg):
    current_pose = msg

그때 호출되는 callback 함수를 위와 같이 정의해 두겠습니다. 별거 없죠. 아마 패키지를 만들었더라면 저 함수에서 하는 일이 많겠지만, jupyter notebook으로 만들때는 저렇게 해두고 current_pose라는 변수를 여기저기서 사용할거라 그냥 저렇게 사용하는 것이 저는 편했습니다.

odom_sub = rospy.Subscriber('/amcl_pose', 
                            PoseWithCovarianceStamped, 
                            callback)

그리고 amcl_pose를 읽어오는 subscriber를 선언합니다. amcl_pose 토픽의 데이터타입이 PoseWithCovarianceStamped 입니다.

current_pose를 확인하면 위 그림처럼 출력이 나타납니다. pose의 pose의 position은 그래도 좌표인가부다 하는데, orientation의 경우는 쿼터니언 표현이라 눈으로 확인하기는 쉽지 않습니다. 이를 쉽게 확인하기 위해 함수를 하나 만들어 두겠습니다.

import numpy as np

convert_to_nparray = lambda o: np.array([o.x, o.y, o.z, o.w])

def cur_pos_xyth(pos):
    x = pos.pose.pose.position.x
    y = pos.pose.pose.position.y
    
    tmp = convert_to_nparray(pos.pose.pose.orientation)
    [_,_, th_rad] = tf.transformations.euler_from_quaternion(tmp)
    
    return x, y, th_rad

위 함수는 간단하게 position 성분에서 z 성분은 항상 0이라, x, y만 가져옵니다. 그리고 orientation 성분에서는 역시 유의미한 성분이 yaw여서 yaw만 (ROS의 기본이 쿼터니언이라) 오일러각으로 변경해서 z 축 중심 회전각만 가져와서 반환합니다. 현재 current_pose에서 사용하면

이렇게 됩니다. 맵의 원점 기준으로 로봇의 위치는 0.39, -0.066쯤의 위치에 오른쪽 방향으로 103도쯤 돌았군요. 화면 상단이 0도라면 그렇게 보입니다. 그리고 저는 저만 그런건지는 모르겠지만, 그때그때 편하게 쓸 수 있는 데이터형을 class로 정의해서 사용하는데요.

class GoalPose:
    x = 0.
    y = 0.
    theta = 0.
    z = 0.
    w = 0.

주행로봇의 경우는 위와 같이 많이 씁니다. 주행로봇은 z 축으로는 안 움직인다고 보는 경우가 많아서 x, y만 두고, 만약 쿼터니언이라면, x, y, 성분은 0일것이니, z, w만, 혹시 오일러로 선언한다면 theta를 사용하도록 해서 저렇게 두는 거죠. 좋은 습관은 아닐 수 있지만 뭐.. 음 그렇습니다.^^

def move_to(goal_point):
    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    
    goal.target_pose.pose.orientation.x = 0
    goal.target_pose.pose.orientation.y = 0
    goal.target_pose.pose.orientation.z = goal_point.z
    goal.target_pose.pose.orientation.w = goal_point.w
    
    goal.target_pose.pose.position.x = goal_point.x
    goal.target_pose.pose.position.y = goal_point.y
    goal.target_pose.pose.position.z = 0
    
    print goal
    ac.send_goal(goal) 

그리고 기본인 주행 명령입니다. 방금 선언한 GoalPose라는 데이터 형에 맞춰 저렇게 선언한 것입니다. move_base 명령은 actionlib으로 되어 있어서 send_goal 명령으로 지령을 줄 수 있습니다.

def rotate_inplace_relative(yaw):
    global current_pose

    goal = GoalPose()
    
    goal.x, goal.y, th_rad = cur_pos_xyth(current_pose)
    
    [_, _, goal.z, goal.w] =\
            tf.transformations.quaternion_from_euler(0, 0, 
                                                     th_rad + yaw)
    
    move_to(goal)

move_to 함수를 이용해서 위 함수는 제자리 회전을 시도하는 함수입니다. 현재 위치를 읽어와서 현재 x, y를 그대로 지령으로 주고 제자리에서만 돌도록 하는것이죠. 여기서 조금 직관적으로 움직일 수 있도록 맵에서의 절대적 자세가 아니라 로봇이 바라보는 자세에서 상대적인 각도를 움직이도록 합니다. 그러나 이 함수는 원하는 대로 정확히 돌지는 않습니다. 여러 이유가 있겠지만 크게는 다음의 이유가 있을 수 있습니다.

• navigation 패키지를 구성할때 설정에서 허가한 tolerance에 따라 원하는 각도만큼 가지않고 멈춤
• move_base 알고리즘의 판단에 따라 이동 경로가 생성될 수 있음
• localization의 오차가 발생하여 현 위치 정보의 갱신

그래서 이런 이유로 제자리 회전을 항상 원하는대로 하게 하려면 별도로 move_base가 아니라 odom을 보고 회전하는 함수를 만드는 것이 효율적일 수도 있습니다. 이 부분은 다시 시간이 허락하면 다뤄보도록 하겠습니다.

현재 위치와 자세는 저렇습니다. 저렇게 제자리 회전을 하기전 amcl_pose를 변환해서 본 값과

-90도로 제자리 회전을 지령으로 주었습니다.

보면 x, y 좌표가 조금 바뀌었고, 각도도 -90을 주었는데 -방향으로 60도 정도를 더 갔을 뿐이네요. move_base를 이용하는 경우 명령을 인가했을때 이런일은 자주 생깁니다. 비정상이 아니니 이를 어떻게 할지는 유저가 보다 고민을 해야합니다. 회전의 경우는 저는 별도로 odom만 보고 회전하는 명령을 좋아하는데 이 이야기도 다음에 해보도록 하겠습니다.

def move_to_relative(l):
    global current_pose
    
    [_, _, th] = cur_pos_xyth(current_pose)

    goal = GoalPose()
    
    goal.x = current_pose.pose.pose.position.x + l*cos(th)
    goal.y = current_pose.pose.pose.position.y + l*sin(th)
    goal.z = current_pose.pose.pose.orientation.z
    goal.w = current_pose.pose.pose.orientation.w
    
    print goal.x, goal.y
    move_to(goal)

위 함수는 move_to를 이용해서 현 위치에서 정해준 거리만큼 직진하는 함수입니다. 현 위치와 자세를 받아서 x, y의 값을 갱신합니다.

글씨를 알아보시기 조~금 힘들 수도 있지만, ROS 2D navigation 화면, 즉 rviz가 실행되었을때 화면 기준으로 로봇의 현 위치에서 거리 l이 주어졌을때, 다음 좌표를 계산하는 식입니다. 위 그림의 원리에 의해 x, y 좌표를 갱신하고, 로봇 입장에서 직진 방향의 지령을 수행할 수 있습니다. 위의 함수도 회전과 같이 오류가 있을 수 있습니다. 특히 자신의 맵위에서의 위치, 장애물의 존재유무, 갈수없는 지역에 대한 지령 등이 이 함수의 결과가 기대와 다를 수 있습니다. 만약 move_base를 사용하지 않고 odom만 가지고 로봇에 지령을 주는 것도 가능합니다. 그 이야기는 회전함수때 이야기한대로 다시 다뤄보도록 할게요~

출발전 현재 위치입니다.

명령을 주고 실행했을때의 위치입니다. 대략적으로 비슷해 보이지만, 뭔가 오차도 있습니다. 한 번더 실행해 볼까요?

이번에는 큰 아까보다는 더 지시를 잘 이수한 듯 합니다. 아무튼 move_base를 이용한 명령은 우리가 생각한대로 정확히 도착하지는 않습니다. 그건 앞서 설명드린 여러 상황이 있기 때문입니다. 그러나 move_base가 제공하는 경로생성, 장애물회피 등의 능력을 생각한다면 사용하기 좋은 아이는 분명하죠^^. 오늘은 조금 긴 이야기지만 move_base를 Python에서 다루어 보았습니다.  마지막 팁 하나~

맵 화면이 cost_map의 잔상으로 저렇게 지저분하다면, 위의 /move_base/clear_costmaps 서비스를 call 해보세요~

깨끗해 집니다.^^.