ROS講座116 navigationを実装する

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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を使います。

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()でゴールに到着したかを判定します。

ビルド

cd ~/catkin_ws
catkin_make

実行

実行

roslaunch nav_lecture move_navigation.launch 

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

参考

move_base

目次ページへのリンク

ROS講座の目次へのリンク