[ROS2] Navigation2 분석(2) - Controller Server

Controller Sever

• controller plugin, goal checker plugin과 progress checker를 load한다.
• 요청에 따라 그에 맞는 controller와 goal checker를 호출하는 역할을 담당한다.
• progress checker로 현재 로봇이 잘 control 되고 있는지 확인해 준다.

Controller Server의 대략적인 흐름도

parameter

• controller_frequency
  • controller가 robot을 control하게 될 주기이기다. 일반적으로 20 ~ 30hz를 사용한다.
• progress_checker_plugin
  • parameter를 적는 형식은 다음과 같다.

  progress_checker_plugin: {PROGRESS_CHECKER_NAME}
      {PROGRESS_CHECKER_NAME}:
        plugin: {PROGRESS_CHECKER_PUGIN}
        {PROGRESS_CHECKER_PARAM_1}: 0.5
        {PROGRESS_CHECKER_PARAM_2}: 10.0
  

  • controller server가 controller에 의해 robot이 잘 작동 되는지 확인하는 용도이다.
  • nav2_core::GoalChecker를 따르며 기본 제공되는 progress checker에는 nav2_controller::SimpleProgressChecker가 있다.
    • simple progress checker는 controller을 할때 로봇이 움직이는 걸 확인한다.(일정 거리 혹은 각도 이상 움직이지 않으면 Navigation이 실패했다고 판단한다.)
  • custom controller를 만들고 싶으면 nav2_core/controller의 인터페이스를 따라 만들고, plugin을 등록하면 사용 가능하다.
  https://github.com/ros-planning/navigation2/blob/galactic/nav2_core/include/nav2_core/controller.hpp
• goal_checker_plugin
  • foxy까지는 goal checker가 하나만 존재할 수 있었지만 galactic에서는 controller처럼 여러개가 존재 할 수 있다. (물론 foxy에서도 되게 할 수 있다. 고치는거 어렵지 않다.)
  • paramter를 적는 형식은 다음과 같다.
    • foxy

    goal_checker_plugin: {GOAL_CHECKER_NAME}
    {GOAL_CHECKER_NAME}:
      plugin: {GOAL_CHECKER_PLUGIN}
      {GOAL_CHECKER_PARAM_1}: 0.25
      {GOAL_CHECKER_PARAM_2}: 0.2
    

    • galactic

    goal_checker_plugin: [{GOAL_CHECKER_NAME1}, {GOAL_CHECKER_NAME2}]
    {GOAL_CHECKER_NAME1}:
      plugin: {GOAL_CHECKER_PLUGIN1}
      {GOAL_CHECKER_PARAM_1}: 0.25
      {GOAL_CHECKER_PARAM_2}: 0.25
      
    {GOAL_CHECKER_NAME2}:
      plugin: {GOAL_CHECKER_PLUGIN2}
      {GOAL_CHECKER_PARAM_3}: 0.25
      {GOAL_CHECKER_PARAM_4}: 0.25
    

  • nav2_controller::SimpleGoalChecker가 제공되며, 단순히 goal 지점과 현재 robot의 Euclidean distance와 yaw error 만 가지고 판단한다.
  • custom goal checker를 만들고 싶다면 nav2_core/goal_checker 인터페이스를 따르고 plugin을 등록하면 된다.
  https://github.com/ros-planning/navigation2/blob/galactic/nav2_core/include/nav2_core/goal_checker.hpp
• controller_plugins
  • path를 제공받아 실제 robot을 control하는 역할 들이다.
  • parameter를 적는 형식은 다음과 같다.

  controller_plugins: [{CONTROLLER_PLUGIN_NAME1}, {CONTROLLER_PLUGIN_NAME2}]
  
  {CONTROLLER_PLUGIN_NAME1}:
  	plugin: {CONTROLLER_PLUGIN1}
  	{CONTROLLER_PLUGIN_PRAMA1}: 10.0
  
  {CONTROLLER_PLUGIN_NAME2}:
  	plugin: {CONTROLLER_PLUGIN2}
  	{CONTROLLER_PLUGIN_PRAMA2}: 3.0
  

  • dwb_core::DWBLocalPlanner가 제공 되며, 이 move_base의 dwa_controller에서 업그레이드 된 형태이다. 이에 대한건 다음 포스팅에서... dwa 논문 리뷰도 적어야 하는데...
  • custom controller를 만들고 싶다면 nav2_core/controller 인터페이스를 만족시키고 plugin을 등록하면 된다.
  https://github.com/ros-planning/navigation2/blob/galactic/nav2_core/include/nav2_core/controller.hpp
• min_x_velocity_threshold
  • controller server가 controller에서 로봇의 현재 속도를 줄 때의 필터링 하는 임계값이다.
  • 로봇의 velocity에 noise가 있을 수 있어서 주는거 같다.
  • x_velocity는 diff type의 로봇 진행 방향으로의 속도이므로 threshold값을 작게 준다. (신뢰성)
• min_y_velocity_threshold
  • controller server가 controller에서 로봇의 현재 속도를 줄 때의 필터링 하는 임계값이다.
  • 로봇의 velocity에 noise가 있을 수 있어서 주는거 같다.
  • y_velocity는 diff type에선 있을 수 없으므로 x_velocity보단 크게 준다. (noise라 생각)
• min_theta_velocity_threshold
  • controller server가 controller에서 로봇의 현재 속도를 줄 때의 필터링 하는 임계값이다.
  • 로봇의 velocity에 noise가 있을 수 있어서 주는거 같다.

 

 

 

 

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

Controller Server Lifecycle

Controller Server는 Lifecycle을 가지고 있으며, 이는 nav2_lifecycle manger에 의해 관리된다.

Configure

paramter를 불러와서 plugin을 load하고, local costmap , velocity publisher, action server등을 만든다.

Activate

publisher와 costmap, action server 등을 activate 한다.

Deactivate

plugin을 모두 해제하고, costmap, action server등을 초기화 한다.

Shut Down

종료한다.

Controller Server Action Sequence

controller server는 /follow_path라는 이름의 action server가 돌아가게 되며, 이에 해당하는 action msg는 다음과 같다.

https://github.com/ros-planning/navigation2/blob/galactic/nav2_msgs/action/FollowPath.action

Action Flow

action flow는 다음과 함수에서 처리 된다.

void ControllerServer::computeControl()
{
  RCLCPP_INFO(get_logger(), "Received a goal, begin computing control effort.");

  try {
    std::string c_name = action_server_->get_current_goal()->controller_id;
    std::string current_controller;
    if (findControllerId(c_name, current_controller)) {
      current_controller_ = current_controller;
    } else {
      action_server_->terminate_current();
      return;
    }

    setPlannerPath(action_server_->get_current_goal()->path);
    progress_checker_->reset();

    rclcpp::WallRate loop_rate(controller_frequency_);
    while (rclcpp::ok()) {
      if (action_server_ == nullptr || !action_server_->is_server_active()) {
        RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping.");
        return;
      }

      if (action_server_->is_cancel_requested()) {
        RCLCPP_INFO(get_logger(), "Goal was canceled. Stopping the robot.");
        action_server_->terminate_all();
        publishZeroVelocity();
        return;
      }

      updateGlobalPath();

      computeAndPublishVelocity();

      if (isGoalReached()) {
        RCLCPP_INFO(get_logger(), "Reached the goal!");
        break;
      }

      if (!loop_rate.sleep()) {
        RCLCPP_WARN(
          get_logger(), "Control loop missed its desired rate of %.4fHz",
          controller_frequency_);
      }
    }
  } catch (nav2_core::PlannerException & e) {
    RCLCPP_ERROR(this->get_logger(), e.what());
    publishZeroVelocity();
    action_server_->terminate_current();
    return;
  }

  RCLCPP_DEBUG(get_logger(), "Controller succeeded, setting result");

  publishZeroVelocity();

  // TODO(orduno) #861 Handle a pending preemption and set controller name
  action_server_->succeeded_current();
}

1. action request에서 goal을 가져온다. (가장 최신에 요청된 request에서 가져온다.)
2. 요청된 controllerId에 맞는 controller plugin을 불러온다.
3. controller에게 path를 넘겨준다.
4. progress check를 reset한다.
5. action server의 상태 확인
6. action cancle 확인
7. controller path update
8. cmd_vel 계산 & publish
9. goal에 도착했는지 확인
10. 도착 안 했을 시 5 ~ 9 반복
11. 도착시 멈추는 cmd_vel publish후 종료