[ROS] Move Base (3) Move Base Code 분석

여기서 분석하는 코드는 다음 github에서 확인 할 수 있다.

https://github.com/ros-planning/navigation

GitHub - ros-planning/navigation: ROS Navigation stack. Code for finding where the robot is and how it can get somewhere else.

Move Base Node

move base node 코드에서는 다음과 같이 단순히 TF buffer를 MoveBase에 넘겨주는 역할만 하는 걸 볼 수 있다.

#include <move_base/move_base.h>
#include <tf2_ros/transform_listener.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "move_base_node");
  tf2_ros::Buffer buffer(ros::Duration(10));
  tf2_ros::TransformListener tf(buffer);

  move_base::MoveBase move_base( buffer );

  //ros::MultiThreadedSpinner s;
  ros::spin();

  return(0);
}

Move Base

MoveBase class 정의에서 함수 부분만 보면 다음과 같다.

  class MoveBase {
    public:
      MoveBase(tf2_ros::Buffer& tf);
      virtual ~MoveBase();
      bool executeCycle(geometry_msgs::PoseStamped& goal);

    private:
      bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
      bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
      bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
      bool loadRecoveryBehaviors(ros::NodeHandle node);
      void loadDefaultRecoveryBehaviors();
      void clearCostmapWindows(double size_x, double size_y);
      void publishZeroVelocity();
      void resetState();
      void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
      void planThread();
      void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
      bool isQuaternionValid(const geometry_msgs::Quaternion& q);
      bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
      double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
      geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
      void wakePlanner(const ros::TimerEvent& event);
      void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
  };
};

여기서 핵심적인 함수들만 constructor부터 따라가면서 해석해 보자.

MoveBase::MoveBase(tf2_ros::Buffer& tf)

기본적으로 tf2_ros::Buffer를 참조하는 생성자이다.(global planner , local planner, costmap 등에서 tf를 사용하기 때문)

• node handle로 부터 parameter를 받아와서 publisher , subscriber 등을 세팅 한 후 plugin까지 load 해준다.

void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal)

PoseStamped로 publish된 Goal을 MoveBaseAction을 호출하기 위한 topic으로 republishe해주는 역할을 한다. (이 Subcriber 덕분에 /goal 로 publish해도 MoveBaseAction이 호출 된다.)

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)

MoveBaseAction을 처리하는 함수이며, 요청에 대한 Validataion을 수행하고, 실질적인 executeCycle과 planThread를 실행하는 함수이다.

• action flow
  1. isQuaternionValid(move_base_goal->target_pose.pose.orientation)
     quaternion 확인을 통해 요청한 goal이 유효한지 확인한다.
  2. goalToGlobalFrame(move_base_goal->target_pose)
     요청된 goal을 global frame에서의 goal로 변환한다.
  3. publishZeroVelocity()
     일단 로봇을 멈춘다.
  4. planner_cond_.notify_one()
     condiontion을 날려 planner thread를 작동시킨다.
  5. current_goal_pub_.publish(goal)
     global frame기준의 goal을 퍼블리시 해준다.(feedback 용)
  6. if(shutdown_costmaps_)
     costmap이 작동하고 있는지 확인 후 작동 하지 않는다면 작동하게 한다.
  7. if(c_freq_change_)
     control frequency를 변경해야 하면 변경한다.
  8. if(as_->isPreemptRequested())
     action의 요청이 새로 들어온 건지 검사한다. (기존의 요청 무시하고 선점 해야하는지 검사)
     • if(as_->isNewGoalAvailable())
       새로 들어온 Goal이 유효한지 검사한다.
     • new_goal = *as_->acceptNewGoal();
       Goal을 바꾸고 다시 세팅힌다.
  9. if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID())
     다시 한번 frame Id를 확인하고, Preempted면 goal을 재설정 해준다.
  10. bool done = executeCycle(goal)
      실제 navigation이 작동하는걸 execute한다.
  11. if(done)
      로봇이 Goal에 도착했으면 action을 끝낸다.
  12. 8~11반복
  13. node가 죽으면 action을 Abort를 날리고 끝낸다.

bool MoveBase::executeCycle(geometry_msgs::PoseStamped &goal)

• Execute Cycle flow
  1. boost::recursive_mutex::scoped_lock ecl(configuration_mutex_)
     plan을 만드는 한 cycle동안은 Configuration을 못하게 mutex를 잠근다.
  2. getRobotPose(global_pose, planner_costmap_ros_)
     현재 로봇의 Pose(위치와 각도)를 가져온다.
  3. as_->publishFeedback(feedback)
     feedback으로 로봇의 현재 위치를 알려준다.
  4. if (distance(current_position, oscillation_pose_) >= oscillation_distance_)
     • oscillation을 확인하고, 로봇이 예상 위치 밖에 있으면 recovery behavior에서 oscillation recovery를 실행한다.
     • oscillation_pose와 현재 robot의 위치의 거리값으로 판단한다.
  5. if (!controller_costmap_ros_->isCurrent())
     sensor data로 costmap이 update됐는지 확인한고, 안됐다면 로봇을 멈추고 plan을 하지 않는다.
  6. if (new_global_plan_)
     • global plan이 새로 생성 됐을 때 trigger이다.
     • 새로 받은 global plane을 local planner에게 건네주고, 건네주게 실패하면 action을 중단시킨다.
     • 성공 했다면 recovery behavior에서 re plan에 해당하는 걸 실행한다.
  7. switch (state_)
     state에 따라 어떤 컨트롤을 할지 결정한다.
     1. PLANNING
        global planner , local planer을 모두 plane을 만들게 한다.
     2. CONTROLLING
        local planner에서 현재 robot이 Goal에 도달 했는지 확인한다. 도착했으면 action에 Success를 날리고 action을 끝낸다.
        도착했으면 move base의 State를 초기화 하고 action을 마무리한다.
        oscillation을 확인하고, Timeout이 걸리면 로봇을 멈추고 recovery behavior를 실행한다.
        local planner에서 현재 Command velocity를 가져오고, publish한다.
        velocity를 생성하지 못하면 로봇을 멈추고 다시 local plan을 진행한다.
     3. CLEARING
        robot을 초기화 시키는 상태이다.
        recovery behavior가 정의 돼어 있으면, 해당하는 trigger를 찾아서 행동한다.
     4. default
        절대 도달해서는 안되는 state, move base를 초기화하고 action에 Abort를 날린다.

bool MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROS *costmap)

로봇의 Pose를 Global Pose로 transform을 해준다.

void MoveBase::resetState()

Move base State를 초기값으로 재설정 한다.

double MoveBase::distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)

두 위치의 L2 norm을 반환해 준다.

geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg)

간단하게 PoseStatmped msg를 global frame기준으로 transform하여 return해준다.

bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q)

quaternion이 valid한지 확인해 준다. (내적 사용)

void MoveBase::wakePlanner(const ros::TimerEvent &event)

주기적으로 condition을 날려 planThread를 깨워준다.

void MoveBase::planThread()

plan을 위한 기본 세팅과 plan이 일어나고의 일들을 처리하고, plan에 대한 책임은 makePlan에 위임한다.

• plan thread flow
  1. boost::unique_lockboost::recursive_mutex lock(planner_mutex_)
     planner가 시작 돼서 lock을 잠그고 시작한다.
  2. while (wait_for_wake || !runPlanner_)
     planner을 run 시키기 전까지 기다린다.
  3. planner_plan_->clear()
     plan을 초기화 한다.
  4. bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_)
     plan을 생성한다
  5. if (gotPlan)
     plan이 생성되었으면 plan을 저장하고, move base state를 CONTROLLING으로 바꾼다.
  6. else if (state_ == PLANNING)
     plan이 생성되지 못하고, move base가 plan을 필요로 하는 state면 재시도를 한다.
     그래도 생성이 안되면 recovery behavior를 작동시킨다.
  7. if (planner_frequency_ > 0)
     주기적으로 plan을 하는게 설정 되어 있다면 timer를 작동하여 plan을 주기적으로 작동시킨다.

bool MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)

실질적으로 global planner를 호출하는 곳이다.

• make plan flow
  1. boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()))
     costmap을 사용하므로 mutex를 잠근다.
  2. plan.clear()
     plan을 초기화 한다.
  3. if (planner_costmap_ros_ == NULL)
     costmap이 있는지 확인한다. (위에 mutex를 get 할때 이미 에러가 날거 같은데 왜 있는지 모르겠다.)
  4. if (!getRobotPose(global_pose, planner_costmap_ros_))
     Robot pose를 가져온다.
  5. if (!planner_->makePlan(start, goal, plan) || plan.empty())
     global planner가 plan을 만들도록 요청을 날린다.

void MoveBase::publishZeroVelocity()

robot의 속도를 0으로 만들게 모든 값을 0으로 cmd_vel을 publish해준다.