3)[turtlebot_코드정리] navigation

* 이전 글에서 gazebo를 통해 시뮬레이션 환경을 설정하고, gmapping을 통해 map을 그리는 것 까지 완성하였다. 이번에는 navigation에 대해 정리를 해보도록 하자.

우선 이전 riviz상에서 어느정도 map이 완성되었다고 하면 아래의 명령어로 map을 저장하도록 하자

// map 저장
rosrun map_server map_saver -f ~/map

맵을 저장하고 나면 pgm 파일과 yaml 파일이 생성된다. pgm 같은 경우 이미지 형태로 map을 볼 수 있으며, yaml 파일은 map정보가 들어 있게 된다. navigation을 수행하려면 map파일이 필요하므로 여기서 만든 map을 사용하면된다.

slam을 통해 생성된 map.pgm

1) 실행코드

- Navigation
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml

2) 실행 파일 정리

-> 아래의 구조에서 maps는 위에서 저장한 map이 들어가게 되며, launch 폴더에는 실행파일이 들어간다. param은 navigation을 수행하는 데 필요한 parameter 들이 들어간다. 우선 위에서 실행한 turtlebot3_navigation.launch 파일을 확인하도록 하자.

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/>
  <arg name="open_rviz" default="true"/>
  <arg name="move_forward_only" default="false"/>
  
  // urdf 파일을 불러와서 tf frame을 생성하는 remote 파일
  <!-- Turtlebot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  // amcl을 사용하기 위해서는 map이 필요하며, map_server 패키지를 이용하여 map을 실행한다.
  <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>

  // navigation에서 locallization을 수행할 때 AMCL 방법을 사용하므로 해당 패키지를 실행한다.
  <!-- AMCL -->
  <include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>

  // 실제 이동은 planning 알고리즘에 의해서 구동되며, 여기서는 move_base를 사용하여 구동한다.
  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
    <arg name="model" value="$(arg model)" />
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
  </include>

  // navigation 구동 시에 로봇을 시각화하기 위해서 rviz 파일을 사용한다.
  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
  </group>
</launch>

위의 turtlebot3_navigation.launch 코드를 살펴보면 몇가지의 파일들이 실행되고 있다.

1) turtlebot3_remote.launch 파일은 이전 slam 수행 시에 urdf 파일로 robot_state_publisher를 실행하여 tf 정보를 제공하기 위해서 실행한다.

2)  map server 실행, 여기서는 locallization의 방법으로 AMCL을 사용하고 Planning 방법으로 move_base를 사용한다. AMCL을 사용하려면 map이 필요한데, 이는 map_server를 동작시켜 위에서 생성한 map을 실행하여 /map 토픽을 생성한다.

3) AMCL은 locallization을 하는 패키지로 navigation 수행 시 계속 로봇의 위치가 변경되는 것을 확인하기 위해서 사용한다.

4) move_base 실행 , 위에서 말한 planning 을 수행하기 위해서 move_base를 실행한다.

5) navigation 과정을 확인하기 위해서 rviz파일을 확인한다.

 

이제부터는 각각의 코드를 확인해 보도록 한다. 위의 4가지 실행 파일 중 1번은 slam에서 다루었으며, 2번 map 서버는 크게 어려운 것 없이 arg로 map 파일을 지정하면 되므로 살펴보지 않고, 5번의 rviz 또한 설정 파일이므로 다루지 않는다. 즉 3번 , 4번에 대해서만 정리하도록 한다.

 

2-1) amcl.launch

<launch>
  // 여기서 실제 topic을 받아서 쓰는 이름이 중요하다 
  <!-- Arguments -->
  <arg name="scan_topic"     default="scan"/>
  <arg name="initial_pose_x" default="0.0"/>
  <arg name="initial_pose_y" default="0.0"/>
  <arg name="initial_pose_a" default="0.0"/>

  <!-- AMCL -->
  <node pkg="amcl" type="amcl" name="amcl">

    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="3000"/>
    <param name="kld_err"                   value="0.02"/>
    <param name="update_min_d"              value="0.20"/>
    <param name="update_min_a"              value="0.20"/>
    <param name="resample_interval"         value="1"/>
    <param name="transform_tolerance"       value="0.5"/>
    <param name="recovery_alpha_slow"       value="0.00"/>
    <param name="recovery_alpha_fast"       value="0.00"/>
    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
    <param name="gui_publish_rate"          value="50.0"/>

    <remap from="scan"                      to="$(arg scan_topic)"/>
    <param name="laser_max_range"           value="3.5"/>
    <param name="laser_max_beams"           value="180"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand"              value="0.5"/>
    <param name="laser_sigma_hit"           value="0.2"/>
    <param name="laser_lambda_short"        value="0.1"/>
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="laser_model_type"          value="likelihood_field"/>

    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha1"               value="0.1"/>
    <param name="odom_alpha2"               value="0.1"/>
    <param name="odom_alpha3"               value="0.1"/>
    <param name="odom_alpha4"               value="0.1"/>
    
    //amcl의 경우 odometry에 대한 정보가 필요하며, 이는 odom 토픽에서 찾을 수 있다.
    // simulation 상에서는 gazebo를 실행할 때 해당 토픽을 보내준다.
    <param name="odom_frame_id"             value="odom"/>
    
    // 로봇의 base가 되는 frame으로 urdf에서 tf를 정의한대로 가장 로봇의 아래에 해당하는
    // base_footprint를 base_frame_id로 한다.
    <param name="base_frame_id"             value="base_footprint"/>

  </node>
</launch>

위의 파일에서 보면 여러가지 parameter를 설정하게 되어 있다. 정확한 것은 구글에서 amcl 가이드를 보고 어떤 의미인지 파악하면서 변경해서 사용하면 되고, 여기서 중요하게 봐야할 것은 frame id와 토픽들이다. amcl에서는 laser를 기반으로 위치를 추정하므로 scan의 토픽으로 laser 값을 받아서 사용한다.

AMCL의 경우 /scan을 가지고 /map -> /odom을 연산하게 되므로, /odom에 대한 정보가 필요한데, 이는 시뮬레이션 상에서 gazebo가 발행해준다. 또한 로봇의 base가 되는 base_footprint를 사용하여 base_frame을 나타낸다. slam에서 본 urdf의 tf 구조에서 scan 토픽을 발행하는 base_scan -> base_link -> base_footprint의 구조가 정확하게 되어야만 정상적으로 동작가능하다.

 

2-2) move_base.launch파일

<launch>
  // 실제 로봇이 지령을 받을 cmd_vel_topic을 지정하고
  // 로봇의 이동을 확인할 odom topic을 지정한다.
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

 // 아래의 parameter가 매우 많은 것을 볼 수 있는데, move base는 costmap을 기준으로 해당 값이 높으면
 // 자유영역으로 판단하고, 값이 낮으면 벽이 있는 공간으로 파악하여 local_plan을 수행하는데 
 // 여기서는 dwa 방법을 사용하였다. dwa는 dynamic approch window로 
 // 각 속도와 선속도를 사용하여 장애물이 나타나면 가장 최적의 속도값을 찾아서 이동하는 local plan방법이다.
  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
  </node>
</launch>

위의 move_base.launch 파일을 보면 move_base를 실행시키기 위해서 매우 많은 parameter 파일이 동작하는 것을 알 수 있다. cost_map에 대한 parameter 와 실제 plan을 수행하기 위한 paramer에 대한 파일들이다.

 

- costmap_common_params_waffle.yaml

// 장애물에 대한 range 설정
obstacle_range: 3.0
raytrace_range: 3.5

footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]
#robot_radius: 0.17

// 아래의 inflation_radius가 너무 클 경우 로봇이 경로를 계획하지 못하고 계속
// recovery 모드가 수행된다.
inflation_radius: 1.0
cost_scaling_factor: 3.0

map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

-local_costmap_params.yaml

// local의 경우 global_frame이 odom 이고 base_frame이 base_footprint
local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5  

  static_map: false  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

-global_costmap_params.yaml

// global costmap의 경우 global_frame이 map이고 base_frame이 baes_footprint이다.
global_costmap:
  global_frame: map
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5

  static_map: true

- move_base_params.yaml

shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2

- dwa_local_planner_params_waffle.yaml

-> 여기서는 base_local_planner로 dwa를 선택하였다. dwa는 위에서 언급한 것 처럼 선속도와 각속도가 최대가 되는 방향으로 이동하는 local_planning 중의 하나이다.

-> 로봇의 형태와 local planner에 따라 자동차 모델의 경우 tep_local_planner를 사용하기도 한다.

DWAPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.22
  min_vel_x: -0.22

  max_vel_y: 0.0
  min_vel_y: 0.0

# The velocity when robot is moving in a straight line
  max_vel_trans:  0.22
  min_vel_trans:  0.11

  max_vel_theta: 2.75
  min_vel_theta: 1.37

  acc_lim_x: 2.5
  acc_lim_y: 0.0
  acc_lim_theta: 3.2 

# Goal Tolerance Parametes
  xy_goal_tolerance: 0.05
  yaw_goal_tolerance: 0.17
  latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 1.5
  vx_samples: 20
  vy_samples: 0
  vth_samples: 40
  controller_frequency: 10.0

# Trajectory Scoring Parameters
  path_distance_bias: 32.0
  goal_distance_bias: 20.0
  occdist_scale: 0.02
  forward_point_distance: 0.325
  stop_time_buffer: 0.2
  scaling_speed: 0.25
  max_scaling_factor: 0.2

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true

3) 실행화면

-> 아래에서 보면 목적지 설정 시 global plan의 길이 보이게 되며, 로봇의 위치에 따라 localization을 하면서 local plan이 동작하여 장애물을 회피하면서 목적지 까지 이동하게 된다.

navigation 실행화면

실행 화면과 아래의 tf_tree / graph를 통해서 현재 동작하고 있는 좌표계와 topic / node들이 어떻게 작용하는 지 확인할 수 있다.

rqt_tf_tree

rqt_graph