move_baseの分析(1) -move_base本体-

背景

• move_baseを一通り使えるようになったが、細かいパラメータを調整する上で中身をより詳しく理解する必要が出てきたため、いい機会なのでどのように動いているのか詳細を調べる。

方針

• はじめにmove_baseが実際にどのような流れで動いているのかについてまとめる。
  • 今後は各関数の詳細やプランナーやコストマップについてまとめる予定。
  • 次回：move_baseの分析(2) -コストマップ-
• 実際のコードに記載されている関数名、変数名を使いながらまとめる。

前提条件

• ROS distroはmelodic
  • move_baseソースコード:https://github.com/ros-planning/navigation/tree/melodic-devel
• ソースコードから解析した内容を記す。

参考

• 産総研によるnavigationスタックの解説資料です。この記事を作った後に気づきました(汗)。
  • https://robo-marc.github.io/navigation_documents/

move_base基本知識

• 初期化処理後、主となる動作はgoalトピックを受信するまでは何もしない。
  • 受信後、設定されたプランナーに従ってゴールまで移動する。
• PLANNING,CONTROLLING, CLEARINGの3つの状態を持つ。
  • PLANNING
    • グローバルプラン生成中の状態
  • CONTROLLING
    • ローカルプランを生成しゴールまで移動している状態
  • CLEARING
    • 障害物クリアモード

move_baseの簡易フロー

処理のメインに当たる所のみを抜粋

1. 初期化処理
   1. MoveBaseインスタンスを生成
   2. アクションサーバー起動
   3. goalトピック受信時の設定
   4. グローバルプランナー用スレッド起動
   5. グローバルプランナーの設定
   6. ローカルプランナーの設定
   7. その他
2. goalトピック受信後
   1. goalトピックをアクションサーバーに配信
   2. プランナーに従ってゴールまで移動する

初期化処理

MoveBaseインスタンスを生成

• move_baseノードを実行するとmove_base_node.cppのメイン関数が呼び出され、MoveBaseクラスのインスタンスが生成される。
• 以降の初期化処理はMoveBaseクラスのコンストラクタでの処理になる。

アクションサーバー起動

• 最初にアクションサーバーを起動させる。
  • コールバック関数にはアクションの目標をサーバーが受信した時に起動するexecuteCb関数を登録する。

move_base.cpp

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

goalトピック受信時の設定

• goalトピックを受信したときに処理を行うコールバック関数を登録

move_base.cpp

  ros::NodeHandle simple_nh("move_base_simple");
  goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

グローバルプランナー用スレッドを起動

• グローバルプランナー用スレッドを生成する。
  • スレッドで動作させる関数にはplanThread関数を登録

move_base.cpp

  planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

• スレッド生成後は、プランナー生成の準備が整うまでスレッドをサスペンドする。
  • planThread関数内で以下の様なサスペンド処理が実行される。

move_base.cpp

    while (wait_for_wake || !runPlanner_)
    {
      // if we should not be running the planner then suspend this thread
      ROS_DEBUG_NAMED("move_base_plan_thread", "Planner thread is suspending");
      planner_cond_.wait(lock);
      wait_for_wake = false;
    }

グローバルプランナーの設定

• グローバルプランナー用コストマップとしてCostmap2DROSクラスのインスタンスを生成する。
  • 生成後、コストマップは一時停止。
• インスタンス生成後、ClassLoaderを用いてグローバルプランナーのインスタンスを生成する。
• プランナーのインスタンスを初期化する。

move_base.cpp

  planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
  planner_costmap_ros_->pause();

  // initialize the global planner
  try
  {
    planner_ = bgp_loader_.createInstance(global_planner);
    planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
  }
  catch (const pluginlib::PluginlibException &ex)
  {
    ROS_FATAL("Failed to create the %s planner, are you sure it is properly "
              "registered and that the containing library is built? Exception: %s",
              global_planner.c_str(), ex.what());
    exit(1);
  }

ローカルプランナーの設定

• ローカルプランナー用コストマップとしてCostmap2DROSクラスのインスタンスを生成する。
  • 生成後、コストマップは一時停止
• インスタンス生成後、ClassLoaderを用いてローカルプランナーのインスタンスを生成する。
• プランナーのインスタンスを初期化する。

move_base.cpp

  controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
  controller_costmap_ros_->pause();

  // create a local planner
  try
  {
    tc_ = blp_loader_.createInstance(local_planner);
    tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
  }
  catch (const pluginlib::PluginlibException &ex)
  {
    ROS_FATAL("Failed to create the %s planner, are you sure it is properly "
              "registered and that the containing library is built? Exception: %s",
              local_planner.c_str(), ex.what());
    exit(1);
  }

その他

• 各コストマップを一時停止から開始へ遷移させる。

move_base.cpp

 planner_costmap_ros_->start();
 controller_costmap_ros_->start();

• move_baseの状態をPLANNINGへ遷移

move_base.cpp

state_ = PLANNING;

• アクションサーバーの開始

move_base.cpp

as_->start();

goalトピック受信後

goalトピックをアクションサーバーに配信

• goalCBが起動し、アクションサーバーに目標を配信する処理を行う。

move_base.cpp

void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal)
{
  ROS_DEBUG_NAMED("move_base", "In ROS goal callback, wrapping the PoseStamped in the "
                               "action message and re-sending to the server.");
  move_base_msgs::MoveBaseActionGoal action_goal;
  action_goal.header.stamp = ros::Time::now();
  action_goal.goal.target_pose = *goal;

  action_goal_pub_.publish(action_goal);
}

プランナーに従って移動する処理

• アクションサーバーに目標が配信されるとexecuteCb関数が起動する。
  • この関数では、予め設定してた周期で無限ループを繰り返しながらmove_baseの状態に応じて以下のような処理を行う。
    • ローカルプランナーを生成する処理
    • ロボットへ速度コマンドを送信する処理
    • ゴール到着判定処理。
    • その他
      • リカバリー処理など
  • グローバルプランがまだ生成されていない場合、グローバルプランナー用スレッドを起動させてグローバルプランが生成されるまで待つ。
• グローバルプラン生成
  • サスペンドされていたスレッドがexecuteCb関数内で起動状態に遷移される。
  • 選択したグローバルプランナーに応じてmakePlan関数が実行されてる。
• グローバルプラン生成後
  • move_baseの状態をCONTROLLINGに遷移
    • グローバルプランナー用スレッドはサスペンド状態に戻す。
  • executeCb関数での処理
    • ローカルプランを生成する。
    • ローカルプランから最適な経路を選択し速度コマンドに変換してロボットへ送信する。
    • ゴールに到着したか判定を行う。
      • ゴールに到着していない場合はローカルプラン生成からやり直す。
      • ゴールに到着した場合、アクションサーバーにゴール到着成功を送信する。

move_base.cpp

// ゴール到着判定
if (tc_->isGoalReached())
{
  ROS_DEBUG_NAMED("move_base", "Goal reached!");
  resetState();

  // disable the planner thread
  boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
  runPlanner_ = false; // プランナースレッドのサスペンド指示
  lock.unlock();
  // アクションサーバーに目標到達を送信
  as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
  return true;
}

...

// 選択したプランナーに応じたcomputeVelocityCommands関数が実行され、ローカルプランの生成と速度コマンドの算出を行う。
if (tc_->computeVelocityCommands(cmd_vel))
{
  ROS_DEBUG_NAMED("move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
  last_valid_control_ = ros::Time::now();
  // make sure that we send the velocity command to the base
  vel_pub_.publish(cmd_vel);
  if (recovery_trigger_ == CONTROLLING_R)
    recovery_index_ = 0;
}