ROS에서 경로계획이 어떻게 작동하는지 알아보자

Rviz에서 path planning 시각화 하기

Rviz Display

• Map display (for costmap)
• Path display (plans)
• 2D Nav Goal

roslaunch husky_navigation_launch main.launch
roslaunch husky_navigation move_base_demo.launch
rviz

rviz를 켰으면 아래 적힌 내용들을 설정해주자
.

add > map
add > robotmodel
add > laserscan > size > 0.1
add > map > topic > /move_base/gloabal_costmap/costmap > remane > global costmap > color scheme > costmap
add > map > topic > /move_base/local_costmap/costmap > rename > local costmap > color scheme > costmap
add > by topic > /move_base > /global_plan > path > rename > global planner
add > by topic > /move_base > /local_plan > path (topic: /move_base/DWAPlannerROS/local_plan) > rename > local planner
add > by topic > /global_costmap > /footprint > polygon > rename > global polygon -> 충돌 영역 표시
add > by topic > /local_costmap > /footprint > polygon > rename > local polygon -> 충돌 영역 표시
add > by_topic > /particlecloud > poseArray > rename > AMCL particles
add > by_topic > /move_base_simple > /goal > pose > rename > goal pose

• 참고로 이미있는 것과 속성이 같다면 duplicate해서 topic만 바꿔주면 된다.

2D Nav Goal로 갈 곳을 지정해주면
빨간색 선 : global plan
초록색 선 : local plan
을 확인해 볼 수 있다.

보기 편하도록 색을 바꿔주면 좋다

이렇게 설정한 rviz를 아래 directory에 navigation.rviz로 저장해주도록 하자

~/sim_ws/src/husky/husky_navigation/rviz

.
.

오픈소스 path planning 패키지

Move_base 노드

• navigation stack에 주요 노드
• 목적: 로봇을 현재 위치에서 목표 위치까지 이동 (SimpleActionServer 이용)
• 목적지에 대한 Msg Type : geometry_msgs/PoseStamped
• Published Topic : move_base/goal

rostopic echo /move_base/goal

rviz 상에서 2D NAV GOAL을 찍어준 후 메세지를 출력해보면 goal 정보가 출력되는 것을 확인할 수 있다.

이처럼 rviz의 UI 상에서 2D nav goal로 목표 지점을 찍어줄 때마다 move_base가 /move_base/goal을 publish하는 것이다.

goal:
target_pose:
header:
seq:
stamp:
secs:
nsecs:
frame_id :
pose :
position:
x:
y:
z:
orientation:
x:
y:
z:
w:

move_base 노드의 topics

• /move_base/goal (move_base_msgs/MoveBaseActionGoal)

• /move_base/cancel (actionlib_msgs/GoalID)

• /move_base/feedback (move_base_msgs/MoveBaseActionFeedback)
  - 현재 map상에서 robot의 위치 업데이트, goal로 잘 이동하는지 확인하기 위함

• /move_base/status (actionlib_msgs/GoalStatusArray)
  - 현재 상태가 나온다 (goal로 이동중: status:1, 도착하면 goal reached 나옴)

• /move_base/result (move_base_msgs/MoveBaseActionResult)
  - 목적지에 도달하고 나면 topic 생성 (goal reached)

꼭 rviz 상에서 goal을 2d nav goal로 정해주지않아도 터미널 상에서 message를 publish해서 지정해줄 수 있다.

rostopic까지 적어준 후 tab을 치면 자동적으로 구조가 나온다.
(terminal이므로 커서가 좌우로만 움직인다..)

rostopic pub /move_base/goal move_base_msgs/MoveBaseActionGoal

.
.

실습

목표

• move_base 노드의 액션서버로 메세지 전송을 위한 액션 클라이언트 만들기 (send_goal_client.py)

mission

1. 액선 클라이언트를 사용하여 로봇을 map 의 세 가지 다른 목적지로 이동
2. 로봇이 3개의 목적지에 도달하면 루프를 다시 생성
   (로봇이 3개의 목적지로 계속 반복해서 이동하도록 함)

천천히 코드를 발전시키면서 movebase를 이해해보자

mission - goal 위치 지정, 상태 출력

<send_goal_client.py> - version1

#! /usr/bin/env python3

import rospy
import time
import actionlib

from move_base_msgs.msg import MoveBaseGoal, MoveBaseAction, MoveBaseResult, MoveBaseFeedback

def feedback_callback(msg):
    rospy.loginfo("[Feedback] Going to Goal Pose...")

rospy.init_node('move_base_action_client')
client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
client.wait_for_server()

goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = 2.0
goal.target_pose.pose.position.y = 2.0
goal.target_pose.pose.position.z = 0.0

goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.0
goal.target_pose.pose.orientation.w = 1.0


client.send_goal(goal = goal, feedback_cb = feedback_callback)
client.wait_for_result()

rospy.loginfo("[Result] State: %d" %(client.get_state()))

결과

.
.

Global planner & Global costmap

.
.

Local planner & Local costmap

.
.

Common Costmap Parameters