LoginSignup
4
1
@ASAKA-219(おがね そん)

ROS noeticのnavigationのplanner plugin を自作する方法

Last updated at Posted at 2023-12-17

はじめに

こんにちは、オガネソンです。今回はnavigationのglobal plannerに使われるpluginを自作する方法について書きたいと思います。ROSのパッケージを作るところからビルドまで全て書いてありますので、参考になれば嬉しいです。

Navigation Stackについて

他に詳しい記事がすでにあるのでこちらを参照してもらえればいいのですが、簡単に説明します。

Navigation Stackは、以下の図のような構造をしています。

これ

(出典：Navigation Stack を理解する - 1. 導入)

move_baseパッケージが navigationで実際にホイールの回転速度の指示（/cmd_vel）を出す親分的なパッケージで、amclやmap_serverは、その速度制御の補助として自己位置推定と地図上でどの位置にあたるのかの推定を行うパッケージです。

Navigationにおいては global と local という概念があって、global は「大域的な」という意味で、主に動かない物体に対して使われます。それに対して local は「局所的な」という意味です。主に動的な障害物を避けるために使われます。

このように、/scanや/tfなどのトピックによって読み取った位置情報を元にロボットを指定した場所まで自律移動させるのがnavigation stackの役割です。

今回作るplannerもglobal plannerとlocal plannerに分かれていて、global plannerがある程度簡単なので、それを作ってみたいと思います。

planner pluginを作る

1. パッケージ作成

いよいよ本題となる、pluginの作成です。pluginは、ROSのパッケージとして作成するもので、ここではrobot_planner_wsというワークスペースに作ります。作るglobal plannerの名前はrobot_planner とします。

terminal
$ mkdir -p robot_planner_ws/src
$ cd robot_planner_ws/src
$ catkin_create_pkg robot_planner std_msgs roscpp
$ cd .. && catkin build
$ source devel/setup.bash

これで一旦 robot_planner というパッケージがROSに登録されたので、パッケージのディレクトリまで行きます。

$ roscd robot_planner/

パッケージの中身は以下のようになっていて、pluginのソースコードはC++のため、一般にはsrc/ ディレクトリに書きます。

$ ls
CMakeLists.txt  include  package.xml  src

2. ソースコードを書く

では、planner pluginのソースコードを示します。Navigationで出てくるrviz上で、2D Nav Goal をクリックして矢印で示した位置まで真っ直ぐ進むplannerになってます。

src/robot_global_planner.cpp
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <angles/angles.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

using namespace std;
using std::string; //std名前空間のstringというメソッドを使う

namespace robot_planner{ //global planner名前空間を定義

class RobotPlanner : public nav_core::BaseGlobalPlanner { //nav_coreからbgpのインタフェースを継承させる
  public:
    RobotPlanner(); //デフォルトコンストラクター
    RobotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);//引数を受け取る移譲コンストラクター（コストマップの初期化）
    ~RobotPlanner();//デストラクター（動的確保したメモリの開放をする）

     /** nav_core::BaseGlobalPlannerからインターフェースとしてオーバーライドする **/
     void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);//nav_coreから持ってきたメソッドたち　色んな変数の初期化
     bool makePlan(const geometry_msgs::PoseStamped& start,
                   const geometry_msgs::PoseStamped& goal,
                   std::vector<geometry_msgs::PoseStamped>& plan
     ); //動作を記述するメソッド

   //共通に使う処理部分をprivateにまとめている
   private:
     costmap_2d::Costmap2DROS* costmap_ros_; //二次元の障害物のある範囲を示す変数
     costmap_2d::Costmap2D* costmap_; //二次元コストマップ情報のデータが入っている変数
     double step_size_, min_dist_from_robot_; //最小の移動できる距離
     base_local_planner::WorldModel* world_model_; //ロボットのfootprint（大きさ？）を格納

     double footprintCost(double x_i, double y_i, double theta_i);
     bool initialized_;
};

PLUGINLIB_EXPORT_CLASS(robot_planner::RobotPlanner, nav_core::BaseGlobalPlanner)//move_baseでのnav_coreのプラグインになる

  //Default Constructor
  RobotPlanner::RobotPlanner()//コンストラクターの定義
    : costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){} //メンバー変数の初期化

  RobotPlanner::RobotPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros) //移譲コンストラクター
    : costmap_ros_(NULL), costmap_(NULL), world_model_(NULL), initialized_(false){
    initialize(name, costmap_ros);
  }

  RobotPlanner::~RobotPlanner(){ //デストラクター
    delete world_model_; //動的メモリの破棄
  }

  void RobotPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
      if(!initialized_){
          costmap_ros_ = costmap_ros; //ポインター変数
          costmap_ = costmap_ros_->getCostmap();

          ros::NodeHandle private_nh("~/" + name);
          private_nh.param("step_size", step_size_, costmap_->getResolution());
          private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
          world_model_ = new base_local_planner::CostmapModel(*costmap_); //newで動的にメモリを確保

          initialized_ = true; //boolで定義
      }
      else{ROS_WARN("This planner has already been initialized... doing nothing");}
  }
  
  double RobotPlanner::footprintCost(double x_i, double y_i, double theta_i){
    if(!initialized_){ //初期化されてなければ初期化する
        ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
        return -1.0;
    }

    std::vector<geometry_msgs::Point> footprint = costmap_ros_->getRobotFootprint(); //geometry_msgsのfootprintパラメーターを取得
    //もしfootprintが無ければ...　何もしない
    if(footprint.size() < 3)
      return -1.0;
      
    //今までの確認でfootprintが正しい値ならlocal plannerから得たfootprint_costに代入して返す
    double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint); //local_costmapの値？
    
    return footprint_cost;
  }
  
  bool RobotPlanner::makePlan(const geometry_msgs::PoseStamped& start, 
  const geometry_msgs::PoseStamped& goal,  std::vector<geometry_msgs::PoseStamped>& plan ){
    //初期位置、目的地、経路を引数として入力される
    ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);

    plan.clear();
    costmap_ = costmap_ros_->getCostmap(); //layeard_costmap_->costmap_から取得

    if(goal.header.frame_id != costmap_ros_->getGlobalFrameID()){
      ROS_ERROR("This planner as configured will only accept goals in the %s frame, but a goal was sent in the %s frame.", 
          costmap_ros_->getGlobalFrameID().c_str(), goal.header.frame_id.c_str());
      return false;
    }

    const double start_yaw = tf2::getYaw(start.pose.orientation);
    const double goal_yaw = tf2::getYaw(goal.pose.orientation);

    //正しいセルを見つけるまでロボットの自己位置と目標姿勢で作成されたベクトルに沿って後退させる
    double goal_x = goal.pose.position.x;
    double goal_y = goal.pose.position.y;
    double start_x = start.pose.position.x;
    double start_y = start.pose.position.y;

    double diff_x = goal_x - start_x; //ゴールまでの差
    double diff_y = goal_y - start_y;
    double diff_yaw = angles::normalize_angle(goal_yaw-start_yaw);

    double target_x = goal_x;
    double target_y = goal_y;
    double target_yaw = goal_yaw;

    bool done = false;
    double scale = 1.0;
    double dScale = 0.01;

    while(!done) //while Trueと同じ, 実行する部分
    {

      target_x = start_x + scale * diff_x; //ゴール座標にxがずれた座標 赤い方向に行く
      target_y = start_y + scale * diff_y;
      target_yaw = angles::normalize_angle(start_yaw + scale * diff_yaw);

      double footprint_cost = footprintCost(target_x, target_y, target_yaw); //ここでロボットのfootprintを使っている

      if(footprint_cost >= 0 ){ done = true; } //footprint costは負になっている
      
      scale -=dScale;
    }

    plan.push_back(start); //経路のベクトルが開始

    geometry_msgs::PoseStamped new_goal = goal;
    tf2::Quaternion goal_quat;//ゴールでの向きのtf
    goal_quat.setRPY(0, 0, target_yaw);

    new_goal.pose.position.x = target_x;//新しく設定したゴール地点
    new_goal.pose.position.y = target_y;

    new_goal.pose.orientation.x = goal_quat.x();//ゴールでの姿勢
    new_goal.pose.orientation.y = goal_quat.y();
    new_goal.pose.orientation.z = goal_quat.z();
    new_goal.pose.orientation.w = goal_quat.w();

    plan.push_back(new_goal); //新しいゴールを取ります

   return done;
  }
}; //global plannerのnamespace用のかっこ

これは真っ直ぐ進むプログラムなのでmakePlanメソッドで/goal_poseの位置までの差だけ動くようにしていますが、このメソッドでの動きを変えることで色々な動きをさせることができます。また、この記事の趣旨から外れますが、makePlanメソッドの部分はノードを立てて/goal_poseトピックを受け取って/cmd_velトピックを送信することでロボットを動かすプログラムを書いてnavigationと同時に起動させることで実現する方法もあります（そちらの方が実現しやすい？）。

3. CMakelists.txtを編集する

pythonではシバンを書いて実行権限を付与すればROSで実行できるのですが、C++のソースコードを実行するにはビルドしなければいけません。ROSのビルドシステムはcmakeを使っているので、CMakelists.txt を書く必要があります。
ROSのパッケージを作る際に自動で作成されますが、あくまで最低限の情報しか書かれてないので、今回のようにpluginを作るときには

CMakelists.txt
cmake_minimum_required(VERSION 3.0.2)
project(robot_planner)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  angles
  std_msgs
  base_local_planner
  costmap_2d
  nav_core
  pluginlib
  tf2
  tf2_geometry_msgs
  tf2_ros
)

add_library(${PROJECT_NAME} src/robot_global_planner.cpp)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

catkin_package()
target_link_libraries(robot_planner ${catkin_LIBRARIES})

この中で先程作成したCMakelists.txt に書き足さなければならない文は、find_package() のpluginlib とadd_library() 、target_link_libraries() です。

find_package() にはplugin関係のパッケージとして pluginlib 、global planner関係として nav_core 、costmap_2d 、base_local_planner 、tf2 etc...
を書きましょう。

C++のファイルのビルドは add_executable() を書きますが、planner pluginはライブラリなので、add_library() と書きましょう。左にパッケージ名 、右にC++ソースコードまでのパスです。

include_directories() には、pluginを書く際にヘッダーファイルにクラスや関数のプロトタイプ宣言をしている場合はinclude の部分を上のようにコメントアウトしましょう。

4. package.xmlを編集する

次に、packge.xml を書きましょう。package.xmlも先ほどの1. で作成しているのですが、plugin用の
<depend>nav_core</depend> 、<depend>base_local_planner</depend>、
<depend>costmap_2d</depend>、 <depend>pluginlib</depend>を足します。

package.xml
<?xml version="1.0"?>
<package format="2">
  <name>robot_planner</name>
  <version>0.0.0</version>
  <description>This operates wcfk2 b to plan going around the room, roomba_planner package</description>
  <maintainer email="ask-219@todo.todo">ask-219</maintainer>

<license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  
  <depend>roscpp</depend>
  <depend>std_msgs</depend>
  
  <depend>nav_core</depend>

  <depend>base_local_planner</depend>
  <depend>costmap_2d</depend>
  <depend>pluginlib</depend>
  <depend>angles</depend>

  <export>
    <nav_core plugin="${prefix}/bgp_plugin.xml"/>
  </export>
</package>

それから、<export>の中に<nav_core plugin="${prefix}/bgp_plugin.xml" />と、追加したいpluginの名前を書きます。ここでは例としてbgp_plugin.xmlとしておきます。

5. bgp_plugin.xmlを書く

ここで、4. で指定したpluginを作るための設定であるxmlファイルを書きます。

bgp_plugin.xml
<library path="lib/librobot_planner">
  <class name="robot_planner/RobotPlanner" type="robot_planner::RobotPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A simple planner that seeks to place a legal carrot 
    </description>
  </class>
</library>

library pathの " " には登録させたいpluginの名前を書きます。一般的にはlib<作りたいplanner名> と書くことが多いです。ここではlibrobot_planner とします。

最も重要なのはclassタグの中身で、

タグ 書き方
name プランナーの名前空間/プランナーのクラス名
type プランナーの名前空間：：プランナーのクラス名
base_class_type pluginの継承元の名前空間：：継承元のクラス名

となります。プランナーはnav_core::BaseGlobalPlanner クラスから継承したcostmap_2d などを使用しますから、そのクラスをbase_class_type に書かなければなりません。

6. pluginのビルド

これまでのrobot_global_planner.cpp、CMakelists.txt、package.xml、bgp_plugin.xmlを書いたらいよいよpluginを具現化できます。パッケージのワークスペースディレクトリまで戻ってcatkin_makeしましょう。catkin buildでは librobot_planner.so というpluginファイルが生成されません。

$ cd ~/robot_planner
$ catkin_make

ビルドしたら、最後の方に

####
#### Running command: "make -j8 -l8" in "/home/roboworks/robot_planner/build"
####
Scanning dependencies of target robot_planner
[ 50%] Building CXX object robot_planner/CMakeFiles/robot_planner.dir/src/robot_global_planner.cpp.o
[100%] Linking CXX shared library /home/roboworks/robot_planner/devel/lib/librobot_planner.so
[100%] Built target robot_planner

という表示になればpluginが~/robot_planner/devel/lib/librobot_planner.soという場所にできています。

最後に

$ echo `source ~/robot_planner/devel/setup.bash` >> ~/.bashrc
$ source ~/.bashrc

と、sourceを通したら終了です。では、実際にプランナーが動くかどうか確かめましょう。

実際にナビゲーションさせてみよう

実際にロボットにナビゲーションをさせるには、navigationの仕方を定義しているmove_baseパッケージのmove_base.launch（例）に登録してあげないといけません。

move_base.launch
<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <param name="base_global_planner" value="robot_planner/RobotPlanner" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
  </node>
</launch>

私はROBOTIS社製のTurtleBot3を使用するため、上記のmove_base.launchになります。

具体的に追加する文はこれだけです。
<param name="base_global_planner" value="robot_planner/RobotPlanner" />
value="プランナーの名前空間/プランナーのクラス名"　としてください。

terminal
roslaunch tb3_common turtlebot3_navigation.launch map_file:=$HOME/map.yaml

Scre
ナビゲーション結果

すると、このようにコストマップのエリアが現れるので、2d Nav Goalでゴールを選択。

Screenshot from 2023-12-17 18-32-07.png

このように赤い線の始点に向かってpathの線が引かれてロボットが動けば成功です！

おわりに

お疲れ様でした！初めてだとかなり作業量が多くて大変と思われる方もいらっしゃるでしょうが、慣れれば案外楽に感じる日が来るかもしれませんね。

では〜

4
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
4
1