ROS講座116 navigationを実装する

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |
| Gazebo | 7.14.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
move_baseはパラメーター設定だけで簡単に使えてお手軽ですが、どうしても細部の動作をカスタムしたくなります。
Navigationの動作はアプリケーションによるので、plannerの入れ替えやパラメーターの変更だけでは対応しきれません。
そこでここではmove_base相当のプログラムを実際に実装してみましょう。move_baseはコードが1000行程度と長く複雑なことをしていそうなのですが、プラグインの処理やrecover動作を抜いた一通り動くだけの部分なら以下のように100行程度で実装できてしまいます。
plannerの難しいアルゴリズムはうまくクラスに押し込まれているので、ただ賢く考えて答えを返してくれる関数程度の扱いですみます。

# ソースコード
* move_baseと同様にglobal_costmap、local_costmap、global_planner、local_plannerを持ちますが、recovery_behaviorは持ちません。
* 外部からPoseStamped型のトピックを受けると、現在位置からゴールのglobal_planを一回だけ解き、そのあと5Hzでlocal_planを解いていきます。異常などの処理はありません。
* move_baseのデフォルトに則りglobal_plannerはnavfn、local_plannerはbase_local_plannerを使います。

```c++:nav_lecture/src/move_navigation.cpp
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>
#include <navfn/navfn_ros.h>

enum class NavState{
  STANDBY,
  WAIT_PLAN,
  MOVING
};

class MoveNavigation {
public:
  MoveNavigation() : global_costmap_("global_costmap", tf_), local_costmap_("local_costmap", tf_) {
    twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/dtw_robot1/diff_drive_controller/cmd_vel", 10);
    goal_sub_ = nh_.subscribe("/move_base_simple/goal", 10, &MoveNavigation::goalCallback, this);
    global_planner_.initialize("global_planner", &global_costmap_);
    local_planner_.initialize("local_planner", &tf_, &local_costmap_);
    nav_state_ = NavState::STANDBY;
    timer_ = nh_.createTimer(ros::Duration(0.2), &MoveNavigation::timerCallback, this);
  }

  void goalCallback(const geometry_msgs::PoseStamped msg){
    ROS_INFO("recieve");
    last_pose_ = msg;
    nav_state_ = NavState::WAIT_PLAN;
  }

  void timerCallback(const ros::TimerEvent& e){
    if (nav_state_ == NavState::WAIT_PLAN){
      ROS_INFO("PLAN");

      geometry_msgs::PoseStamped source_pose;
      source_pose.header.frame_id="dtw_robot1/base_link";
      source_pose.header.stamp=ros::Time(0);
      source_pose.pose.orientation.w=1.0;

      geometry_msgs::PoseStamped target_pose;
      std::string target_frame="dtw_robot1/map";
      try{
        tf_.waitForTransform(source_pose.header.frame_id, target_frame, ros::Time(0), ros::Duration(1.0));
        tf_.transformPose(target_frame, source_pose, target_pose);
        ROS_INFO("x:%+5.2f, y:%+5.2f,z:%+5.2f",target_pose.pose.position.x,target_pose.pose.position.y,target_pose.pose.position.z);
      }
      catch(...){
        ROS_INFO("tf error");
      }
      geometry_msgs::PoseStamped start = target_pose;
    
      if (!global_planner_.makePlan(start, last_pose_, last_global_plan_)){
        ROS_WARN("global plan fail");
        nav_state_ = NavState::STANDBY;
        return;
      }
      local_planner_.setPlan(last_global_plan_);
      geometry_msgs::Twist cmd_vel;
      if (local_planner_.isGoalReached()){
        ROS_INFO("reach");
        twist_pub_.publish(cmd_vel);
        nav_state_ = NavState::STANDBY;
        return;
      }
      local_planner_.computeVelocityCommands(cmd_vel);
      twist_pub_.publish(cmd_vel);
      nav_state_ = NavState::MOVING;
    }
    else if(nav_state_ == NavState::MOVING){
      ROS_INFO_THROTTLE(2.0, "MOVING");
      geometry_msgs::Twist cmd_vel;
      if (local_planner_.isGoalReached()){
        ROS_INFO("reach");
        twist_pub_.publish(cmd_vel);
        nav_state_ = NavState::STANDBY;
        return;
      }
      local_planner_.computeVelocityCommands(cmd_vel);
      twist_pub_.publish(cmd_vel);
    }
  }
  ros::NodeHandle nh_;
  tf::TransformListener tf_;
  ros::Publisher twist_pub_;
  ros::Subscriber goal_sub_;
  ros::Timer timer_;

  NavState nav_state_;
  geometry_msgs::PoseStamped last_pose_;
  std::vector<geometry_msgs::PoseStamped> last_global_plan_;

  costmap_2d::Costmap2DROS global_costmap_;
  costmap_2d::Costmap2DROS local_costmap_;
  navfn::NavfnROS global_planner_;
  base_local_planner::TrajectoryPlannerROS local_planner_;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "move_navigation");
  MoveNavigation move_navigatoion;
  ros::spin();
  return 0;
}
```

* ノードの動きを`NavState`で管理しています。
    * 起動直後は`STANDBY`
    * goalがセットされると`WAIT_PLAN`
    * 上記のちに最初のtimer_loopでglobal_planが行われ、local_planの実行中は`MOVING`
    * local_planが終わると`STANDBY`に戻る
* global_planでは開始点と終了点が必要です。終了点は外部から与えられますが、開始点は自分で決める必要があります。大体は今の現在位置を入れればよいでしょう。
    * `makePlan()`で開始・終了点をつなぐ点列を得ます。
* local_planでは最初にglobal planの出力の点列をセットして、その後は一定周期で関数を呼び速度指示値を得ます。
    * `setPlan()`でglobal planの出力をセットします。
    * `computeVelocityCommands()`で現在の速度指値を得ます。
    * `isGoalReached()`でゴールに到着したかを判定します。

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行

```shell:実行
roslaunch nav_lecture move_navigation.launch 
```

![move_navigation.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/d8f30496-13e8-818d-1276-3f91693b7aeb.gif)

Rvizの2DNavGoalを使ってPoseStampedトピックを投げるとそこへ移動します。

# 参考
[move_base](https://github.com/ros-planning/navigation/blob/melodic-devel/move_base/src/move_base.cpp)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)