실내 경로생성 알고리즘의 아이디어 및 코드 설명
팀원에게 설명해주기 위해 작성한 글을 블로그에 그대로 올리는 글입니다.
초기 설정 및 파리미터 설명
노트북이 없어서 rviz는 스크린샷을 찍을 수 없는 상태입니다.
들어가기 전
minibot 패키지를 설치하는 곳은 노트북과 로봇입니다. 그래서 만약에 노트북으로 gazebo 시뮬레이션만 돌릴 예정이라면 Nav2 패키지를 노트북에서 실행하는 것이므로 노트북의 minibot 패키지에서 파라미터를 수정해야합니다. 실제 로봇의 경우는 Nav2 패키지는 실제 로봇에서 실행하는 것이므로 Na2 패키지를 로봇에서 파라미터를 수정해야합니다.
충돌 파라미터 설명
충돌 파라미터는 Nav2 패키지에 있는 파라미터로 2가지로 나누어집니다.1
global collision 과 local collision 입니다.
이 파라미터 수정은 Minibot 기준으로
해당 위치에서 nav2_params.yaml을 수정하면됩니다.
쭉 밑으로 내리다보면,
위의 그림을 확인해보면 local_costmap은 현재 로봇의 라이다 정보를 보고 그리는 그림입니다. 여튼 radius 변수를 줄이면 얼마나 곡선의 모양을 유지할지를 나타냅니다. cost scaling factor는 연한 빨간색의 길이를 나타냅니다.
로봇은 연한 빨간색의 범위에 들어가면 움직일 수 없는 상태가 됩니다.
그리고 지역내에서 경로를 계획할때 다음처럼 빨간범위 밖에서 경로를 잡게 됩니다.
global의 경우 다음의 변수로 수정할 수 있으며, 맵에서 보기에
하늘색과 진한분홍색 부분을 나타냅니다. 이 파라미터의 값은 로봇이 SLAM맵만 가지고 경로를 새울때 사용하며, SLAM으로 먼거리를 가게 만들면 하늘색과 진한분홍색이 없는 구역으로 경로를 짜게 됩니다.
그래서 이 값을 적절히 조절하면 다음처럼 만들 수 있습니다.
다음처럼 파라미터를 줄이면 로봇의 활동영역이 넓어집니다. 그러나 문제가 생깁니다. 로봇이 벽에 가깝게 붙어 이동을 하게된다는 문제와 로봇이 벽과 부딪힐 확률이 높아진다는 문제입니다.
+Robot이 경로의 cost를 계산하면서 앞으로 나아가는데 inflation_radius 는 올리고, cost_scaling_factor는 내리면 global의 분홍색 영역을 아래와 같이 조절할 수 있음.
아래처럼 분홍색 영역이 딱 맞닿으면 벽에서 적절히 떨어져서 가는 경로를 탐색하는 것으로 보임
(https://github.com/ros-planning/navigation2/blob/main/nav2_simple_commander/media/readme.gif)
속도 파라미터 설명
만약 지금 설정되어 있는대로 로봇의 Nav 명령을 내리면 속도가 빨라 모터가 고장날 확률이 높으며, 벽과 부딪힐 확률이 높습니다. 충돌파라미터를 수정한 yaml 파일을 다시 열고 맨밑으로 내려가면
다음과 같은 파라미터값이 있습니다. 여기서 max_velocity와 max_accel을 많이 줄이면됩니다. 로봇은 지금값보다 더 낮은 값을 가지게 한 것으로 기억합니다. (내일 추가할 예정)
경로계획 알고리즘 설명
우선 파일이 두 개가 있습니다 하나는 시뮬레이션에서 돌리는 코드, 또 다른 하나는 실제로봇을 돌리는 코드 입니다. 시뮬레이션 돌리는 코드는 맵을 직접 만들었습니다.
경로생성 알고리즘의 아이디어는 다음과 같습니다. 이를 설명하기 전에 우선 맵부터 보는 것이 좋을 것 같습니다.
다음의 맵은 알고리즘 검증을 위해 가장 이상적으로 만든 맵입니다. 초록색은 벽을 의미하고, 노란색은 다닐 수 있는 길을 의미합니다.
그러면 여기서 맵을 분할할 수 있다고 생각할 수 있습니다.
분할하고 보면, 노란색이 많이 보이면 다닐 수 있는 길이고, 초록색이 많으면 갈 수 없는 벽이라고 생각할 수 있습니다. 이를 이미지처리에서는 그리드 분할이라고 하는 것 같습니다. 그래서 다음의 코드를 생성합니다.
def image_Grid(h, w, img):
img_height, img_width = img.shape
grid_size = (h, w) # 세로, 가로
# 유저가 분할을 원하는 세로, 가로값을 입력받아야하는게 맞다고 생각합니다.
# 그리드 영역 크기 계산
cell_height = img_height // grid_size[0]
cell_width = img_width // grid_size[1]
# 그리드라는 표현을 써서 그렇지 분할된 영역 하나의 크기를 나타내는 겁니다. 가로 길이, 세로 길이
map_cost = []
map_col = []
#각각의 그리드 영역에서 흰색 픽셀 비율 계산
for i in range(grid_size[0]):
map_col = []
for j in range(grid_size[1]):
# 그리드 영역의 범위를 지정하는거고
cell_top = i * cell_height
cell_left = j * cell_width
cell_bottom = cell_top + cell_height
cell_right = cell_left + cell_width
# 간단하게 분할된 영역(사각형)의 꼭지점을 구한겁니다.
if i == h-1:
cell_bottom = img_height
# 마지막 그리드만 길이가 이상하게 뽑히는 경우가 있었습니다.
# 그 맵의 모든 픽셀을 가져와서
cell = img[cell_top:cell_bottom, cell_left:cell_right]
# 그냥 노란색 픽셀이 아니면 벽, 노란색 픽셀이면 길로 보는 겁니다.
# 노란색이 많으면 길로 보자는 아이디어 입니다.
wall = 0
load = 0
for row in range(len(cell)):
for col in range(len(cell[0])):
if cell[row][col] <= 210:
wall += 1;
else:
load += 1
# 이 부분은 수정될 수 있으나, 실제로봇까지 문제없이 작동했습니다.
if load > wall:
map_col.append(0)
else:
map_col.append(1)
# 그래서 판단을 하고 배열에 저장을 합니다.
map_cost.append(map_col)
return map_cost, cell_height, cell_width
그래서 얻게되는 배열이 생깁니다.
실제 맵과 정확히 일치합니다.
자 이제 여기서 경로를 어떻게 생성하고 가게할거냐? 각 분할된 영역으로 Nav2로 이동시키면 그만입니다. 그냥 예시로 보는게 편합니다.
실내 경로생성 알고리즘의 아이디어 이론적으로 접근하는 것보다. Gazebo 상의 시뮬레이션으로 확인을 해보자면 Gazebo를 통한 검증 이를 SLAM 하여 다음과 같은 이미지를 얻습니다. 먼저 다음처럼 가장 이상적인 맵을 하나 만듭니다. 그러면 이를 일정간격의 칸으로 분할 합니다.
start에서 end까지 경로계획을 세우게 하고, 순서대로 그 길을 따라가게 Nav2 알고리즘으로 가게 하면됩니다.
여기에 사용되는 경로생성 알고리즘이 A* 알고리즘이고, Nav2도 이 A* 알고리즘을 기반으로 만들어져있습니다. 여튼 A* 알고리즘을 풀기위해서는
이 배열값이 필수적으로 필요합니다. 이에 대한 코드는
Fleet management을 직접 구현해보자. (A* 알고리즘 탐구) (tistory.com)
Fleet management을 직접 구현해보자. (A* 알고리즘 탐구)
https://medium.com/@robosera.info/how-are-fleet-management-systems-implemented-in-different-robotic-companies-cdce178c12f How are Fleet Management Systems implemented in different robotic companies? Find out the significance of FMS and how the implementati
kimbrain.tistory.com
다음의 블로그 (내 블로그에) 이 A*에 대한 활용 코드를 작성해서 올려놓았습니다.
여튼 A*로 이동을 시키게 하자.
(0,1) 지점에서 (1,11) 으로 이동시키자.
print("맵입니다.")
for i in my_map:
print(i)
print()
temp_map = my_map
make_route = Astar()
start = (0, 1)
end = (1,11 )
result, path = make_route.run(temp_map , start, end)
print("경로 생성 맵입니다. 숫자 2는 경로입니다.")
for i in result:
print(i)
다음처럼 Astar를 사용하면 다음처럼 나오게 되고, 로봇이 이동해야할 좌표도 나오게된다.
(이동해야할 좌표)
여기까지 경로생성에 대한 이야기를 끝내자.
그러면 경로를 알아냈다. Nav2에 어떻게 좌표값을 줄건데?
이게 오늘의 문제였다. 좌표를 일일히 찍어서 저장시킬 것인가? ← 이건 당장 구현이 가능하나 말도 안되는 이야기다.. (자율주행이 아니니까)
그러면 결국 좌표를 맵만보고 프로그램이 스스로 판단해서 좌표값을 구하는 것이 유저친화적인 좋은 프로그램이라 할 수 있다.
자동으로 좌표값을 뽑아내게 하자.
맵을 만들면 pgm 파일과 yaml 파일이 만들어진다. yaml 파일에는 resolution이라는 값이 있다.
0.05라는 값은 곧, 한 픽셀이 실제 로봇세상에서 몇 m인가를 나타낸다. 여기서 한 픽셀은 0.05m를 나타낸다.
# YAML 파일 경로
# 나중에 유저에게 맵 위치만 입력을 받게할 수 있을
yaml_path = "/home/du/mini_bot/src/pinklab_minibot_robot/minibot_navigation2/maps/baby_map.yaml"
# YAML 파일 읽기
with open(yaml_path, "r") as stream:
yaml_data = yaml.safe_load(stream)
# resolution 값 가져오기
resolution = yaml_data["resolution"]
print(resolution)
우선 이 resolution 값을 가져오는 코드를 작성하고, (chatGPT)
#그리드 한 칸의 크기와 resolution의 곱이 결국 이동한 m값이 됩니다.
garo_m = resolution * cell_width
sero_m = resolution * cell_height
print(garo_m, sero_m)
아까 영역을 분할할때 영역의 길이와 높이값을 이 resolution 값과 곱하면 나오는 값이 로봇이 한 칸을 이동하는 거리를 나타낸다. (실제값이라 하는데, 이 값을 그냥 사용해도 rviz의 좌표값과 일치했다..) (그 좌표값이 amcl_pose라는 topic으로 나타난다)
여튼 이제 맵을 찍으면서 찾자
my_map_coordinate = []
my_map_layer = []
cor_x = -1.1 #1.1씩 더하세요
cor_y = 2.2 #1.1씩 빼세요.
for i in range(len(my_map)):
cor_x = -1.1
cor_y = cor_y - 1.1
for j in range(len(my_map[0])):
cor_x = cor_x + 1.1
if my_map[i][j] == 1:
my_map_layer.append(None)
elif my_map[i][j] == 0:
goal = PoseStamped()
goal.header.frame_id = 'map'
goal.header.stamp = nav.get_clock().now().to_msg()
goal.pose.position.x = cor_x
goal.pose.position.y = cor_y
goal.pose.position.z = 0.006378173828125
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = 0.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 1.0
my_map_layer.append(goal)
my_map_coordinate.append(my_map_layer)
my_map_layer = []
이 코드를 돌면 나오는게 결국 0으로 길목이라 생각된 지점의 좌표값을 얻게된다. Nav2에 넣기위해 가공을 하는 과정이 추가되어있다.
여튼 여기서 얻는 것은 그림으로 요약하면 다음과 같다
여튼 이제 우리는 my_map_coordinate의 배열에서
이 영역에 해당하는 값을 my_map_coordinate[i][j] 식으로 빼먹으면 된다.
그래서 로봇의 경로를 이동하는 코드를 완성시킨다.
로봇을 이동시키는 함수
def go_my_robot(my_map_coordinate, start, end):
# 이미지 분석을 다시해서 my_map을 다시 저장한
# 퍼온 코드 문제인지 A*에 넣으면 입력값에도 경로가 표시됨..
my_map, cell_height, cell_width = image_Grid(3, 12, map_img)
make_route = Astar()
result, path = make_route.run(my_map , start, end)
#여튼 유저가 start와 end 그리고 좌표정보를 주면 이를 Astar 알고리즘에 넣고
#결과와 경로를 받아옴
print("경로 생성 맵입니다. 숫자 2는 경로입니다.")
for i in result:
print(i)
# 당연히 1(벽)이 end일 경우 Path에 None이 출력됨
if path == None:
print("이동할 수 없는 지점으로 이동을 명령하고 있습니다.")
return
else:
# 방위각을 위해서 추가되는 정보로 이전의 값을 보고 내가 동서남북 어디로 이동하는지
# 판단을 해야함.
path_0 = path[0]
# 1번째 값은 현재 내 위치임으로 필요없음
path = path[1:]
# 그래서 path보면서 이제 이동을 시작하자.
for route in path:
print("명령을 이수합니다." + str(route) + "좌표로 이동합니다." )
# 손 수 찍어서 확인한 방위각으로 동서남북에서 오는지 판단하고
# waypoint에 도착하고 진행하던 불필요한 회전을 삭제 (버그픽스)
#북쪽
if route[1] - path_0[1] == 1:
my_map_coordinate[route[0]][route[1]].pose.orientation.x = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.y = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.z = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.w = 1.0
#남쪽
elif route[1] - path_0[1] == -1:
my_map_coordinate[route[0]][route[1]].pose.orientation.x = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.y = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.z = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.w = -1.0
# 서쪽
elif route[0] - path_0[0] == -1:
my_map_coordinate[route[0]][route[1]].pose.orientation.x = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.y = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.z = 0.7
my_map_coordinate[route[0]][route[1]].pose.orientation.w = 0.7
# 동쪽
elif route[0] - path_0[0] == 1:
my_map_coordinate[route[0]][route[1]].pose.orientation.x = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.y = 0.0
my_map_coordinate[route[0]][route[1]].pose.orientation.z = -0.7
my_map_coordinate[route[0]][route[1]].pose.orientation.w = 0.7
# 움직여
nav.goToPose(my_map_coordinate[route[0]][route[1]])
i=0
while not nav.isTaskComplete():
i = i+1
feedback = nav.getFeedback()
if feedback and i % 5 == 0:
print("Distance remaining : " + str(feedback.distance_remaining) + " m")
# 남은 거리가 0.1m 이하면 도착한거로 보자.
if feedback.distance_remaining > 0.1:
pass
else:
print("Distance remaining : " + str(feedback.distance_remaining) + " m")
nav.cancelTask()
print("명령을 이수했습니다.")
print(str(route) + "좌표로 이동했습니다." )
print()
path_0 = route
#밑에는 강의자료에 있는 코드로 몇초간 이행을 못하면 취소하는 코드일단 제외함.
#nav.cancelTask()
#if Duration.from_msg(feedback.navigation_time) > Duration(seconds = 10.0):
#nav.cancelTask()
그래서 다음처럼 명령을내리면됨
go_my_robot(my_map_coordinate, (1,0), (1, 11))
뭐 이런식으로 출력되는 걸 볼 수 있음
Rviz 상은 스크린샷을 찍을 수가 없는 상황임을 이해해주길 바람..
가제보상에서의 코드 실행은 다음처럼함
ros2 launch minibot_bringup bringup_robot_gazebo.launch.py world_name:=my.world
ros2 launch minibot_navigation2 bringup_launch.py use_sim_time:=true map:=`ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/maps/baby_map.yaml
rviz2 -d `ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/rviz/nav2_view.rviz
여튼 그래서 맵은 격자 모양이면 좋음
실제 주행로봇에서의 코드 차이점
실행은 다음 코드로 진행함
로봇에서 실행 (map 파일을 로봇에 넣어주어야함)
ros2 launch minibot_bringup bringup_robot.launch.py
ros2 launch minibot_navigation2 bringup_launch.py map:=`ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/maps/baby_real_map_cut.yaml
pc에서 실행
rviz2 -d `ros2 pkg prefix minibot_navigation2`/share/minibot_navigation2/rviz/nav2_view.rviz
실제 로봇에서의 아이디어는 바닥에 있는 사각형 모양의 판임
보면 알겠지만 장애물은 사각형 안에 들어가 있음. 이 사각형 덕분에 맵을 성공적으로 빌딩할 수 있었음.
실제 우리 맵이고, GIMP 라는 프로그램을 통해 이미지를 잘랐음.
너무 잘나와서 깜짝놀란부분, 진짜 잘됨.
대신 실제맵은 저렇게 세로가 길어서 코드가 살짝 수정되어 있고, 내용은 위에서 설명한 코드와 동일함
로봇에 맵 파일이 있어야함
다음 목표는 2대의 로봇을 올리는 것이고,
다음 목표는 장애물이 나타났을때 어떻게 할 것인지임. ← 이건 필수
글로벌
inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 0.5 inflation_radius: 0.1
로컬
inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 0.5 inflation_radius: 0.1
속도
velocity_smoother: ros__parameters: use_sim_time: True smoothing_frequency: 20.0 scale_velocities: False feedback: "CLOSED_LOOP" max_velocity: [0.5, 0.0, 0.7] min_velocity: [-1.0, 0.0, -1.5] max_accel: [0.7, 0.0, 0.9] max_decel: [-1.5, 0.0, -1.8] odom_topic: "base_controller/odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] velocity_timeout: 1.0