[ROS2] Navigation2 분석(3) - DWB Controller

DWB Controller

ros1에서 dwa controller가 좀 더 발전한 형태이며, path를 선정할때 cost를 주는 critic을 plugin으로 변경하여 좀다 다양한 const factor를 만들 수 있다.

참고 page

https://github.com/ros-planning/navigation2/tree/foxy-devel/nav2_dwb_controller

GitHub - ros-planning/navigation2: ROS2 Navigation Framework and System

Parmeters

DWB의 paramter들은 ros1의 dwa와 대부분 같아서 다른 것만 나열하면 다음과 같다.

ros1 dwa parameter

https://coding-robot.tistory.com/42

[ROS] Move Base (6) Local Planner 개념

• yaw_goal_tolerance (삭제 / goal checker에게 권한이 넘어감)
• backup_vel (삭제 / recovery에게 권한이 넘어감)
• escape_vel (삭제 / recovery에게 권한이 넘어감)
• y_vels (삭제 / progress checker에게 권한이 넘어감)
• latch_xy_goal_tolerance (삭제)
• sim_granularity → linear_granularity (변경)
• angular_sim_granularity → angular_granularity (변경)
• meter_scoring (삭제 / 기본으로 meter 사용)
• pdist_scale (삭제 / critic plugin에게 권한 넘어감)
• gdist_scale (삭제 / critic plugin에게 권한 넘어감)
• occdist_scale (삭제 / critic plugin에게 권한 넘어감)
• heading_lookahead (삭제 / critic plugin에게 권한 넘어감)
• heading_scoring (삭제 / critic plugin에게 권한 넘어감)
• heading_scoring_timestep (삭제 / critic plugin에게 권한 넘어감)
• dwa (에초에 dwb plugin이다... 사용 안할거면 plugin을 빼면 된다.)
• publish_cost_grid_pc → debug_trajectory_details (변경 및 기능 추가)
• oscillation_reset_dist (삭제 / progress checker에게 권한이 넘어감)
• prune_plan (삭제)
  • navigation2를 자세히 보면 주기적으로 global path를 다시 세우는게 일반적이라 굳이 path를 지울 필요가 없다.
• critics (신규)

Critics paramter

dwa에서 path를 선정하는 cost function에서 아래 식이 있었다.

$$ G(v,w) = \sigma (\alpha * heading(v, w) + \beta * dist(v,w) + \gamma * velocity(v,w)) $$

이 식에서 dwb는 다음과 같이 plugin들로 각 cost factor들이 변경되었다.

$$ G(v,w) = \sigma \sum_{i=0}^{n} (CiriticFactor_{i}) $$

여기서 각각의 Critic Factor들은 각각의 가중치를 부여하는 방식이 있고, 기본적으로 scale을 설정할 수 있게 된다.

Critics의 interface는 다음에서 확인 할 수 있다. (이 인터페이스로 plugin을 만들면 custom으로 만들어 쓸 수 있다.)

https://github.com/ros-planning/navigation2/blob/foxy-devel/nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_critic.hpp

GitHub - ros-planning/navigation2: ROS2 Navigation Framework and System

Nav2에서 제공되는 Critic는 다음과 같다. (foxy 기준 이후 version에선 더 추가 돼 있다.)

!!! 공통 속성

scale : 가중치

• RotateToGoal
  • goal 근처에서만 로봇이 goal 방향으로 heading하게 해준다.
• Oscillation
  • 로봇이 앞 또는 뒤로만 움직이게 되는걸 막아준다.
• BaseObstacle
  • local path가 inflation 된 영역을 지나면 좋지 않은 걸로 판단.
• GoalAlign
  • local path가 goal에 도착했을 때의 robot pose와 잘 맞을 수록 좋은 path로 판단.
• PathAlign
  • local path가 global path와 일치 할 수록 좋은 path로 판단.
• PathDist
  • local path를 따라 갔을 때 global path의 끝 지점과 가까워질수록 좋은 path로 판단.
• GoalDist
  • local path를 따라 갔을 때 최종 goal과 가까워질수록 좋은 path로 판단.

paramter 예시

# DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      # Add high threshold velocity for turtlebot 3 issue.
      # <https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75>
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0