2017/12/11

ROS ナビゲーションメタパッケージのリカバリ動作を自作する

ROS のナビゲーションメタパッケージ（以下、ナビゲーションと呼びます）はロボットが自律移動するためのパッケージです。
ナビゲーションは自己位置推定がうまくいかないときに、リカバリ動作を行います。
このページでは、リカバリ動作を自作する方法を紹介します。

環境

• Ubuntu 16.04
• ROS Kinetic

手順

1, ROSパッケージを作る

ナビゲーションはメタパッケージなので複数のROSパッケージから構成されています。
リカバリ動作もまたROSパッケージ単位で管理されることがほとんどです。
なので、まずこの章で自作するリカバリ動作用のROSパッケージを作成します。
今回は、go_forward_recovery という名前でロボットを直進させるリカバリ動作を作っていきます。

では、早速作っていきましょう。

$ roscd
$ cd ../src/navigation

依存関係にはとりあえず roscpp を入れておきます。

$ catkin_create_pkg go_forward_recovery roscpp

はじめに、CMakeLists.txt を書き変えていきましょう。

$ cd go_forward_recovery
$ vim CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(go_forward_recovery)

find_package(catkin REQUIRED
        COMPONENTS
            cmake_modules
            roscpp
            tf
            costmap_2d
            nav_core
            pluginlib
            base_local_planner
        )

find_package(Eigen REQUIRED)
include_directories(
    include
    ${catkin_INCLUDE_DIRS}
    ${EIGEN_INCLUDE_DIRS}
)
add_definitions(${EIGEN_DEFINITIONS})

catkin_package(
    INCLUDE_DIRS include
    LIBRARIES go_forward_recovery
    CATKIN_DEPENDS
        roscpp
        pluginlib
)

add_library(go_forward_recovery src/go_forward_recovery.cpp)
target_link_libraries(go_forward_recovery ${catkin_LIBRARIES})

install(TARGETS go_forward_recovery
       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
       )

install(FILES recovery_plugin.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

次に package.xml を書き換えましょう。

$ vim package.xml

<package>
    <name>go_forward_recovery</name>
    <version>1.12.13</version>
    <description>

       *The description of recovery behavior. 

    </description>
    <author>Eitan Marder-Eppstein</author>
    <author>contradict@gmail.com</author>
    <author>*your name</author>
    <maintainer email="*your e-mail address">*your name</maintainer>
    <license>MIT</license>
    <url></url>

    <buildtool_depend>catkin</buildtool_depend>

    <build_depend>cmake_modules</build_depend>
    <build_depend>roscpp</build_depend>
    <build_depend>tf</build_depend>
    <build_depend>costmap_2d</build_depend>
    <build_depend>nav_core</build_depend>
    <build_depend>pluginlib</build_depend>
    <build_depend>eigen</build_depend>
    <build_depend>base_local_planner</build_depend>

    <run_depend>roscpp</run_depend>
    <run_depend>tf</run_depend>
    <run_depend>costmap_2d</run_depend>
    <run_depend>nav_core</run_depend>
    <run_depend>pluginlib</run_depend>
    <run_depend>eigen</run_depend>


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

</package>

*印がついているものはそれぞれ書き入れてみて下さい。
上からパッケージの概要, 著者の名前, 管理者のメールアドレスと名前を意味しています。

2, recovery_plugin.xml を書く

ナビゲーションではリカバリ動作をプラグインとして管理しています。
ここでは新たにリカバリ動作のプラグインを設定するための recovery_plugin.xml を書きます。

$ vim recovery_plugin.xml

<library path="lib/libgo_forward_recovery">
  <class name="go_forward_recovery/GoForwardRecovery" type="go_forward_recovery::GoForwardRecovery" base_class_type="nav_core::RecoveryBehavior">
    <description>
      A recovery behavior that go straight along the wall.
    </description>
  </class>
</library>

このファイルにもリカバリ動作の概要を書く部分があるのですが、ここは書きました。自分で作りたいリカバリ動作に合わせて書き換えて下さい。
あと、少しだけ補足をしておくと、move_base では nav_core でリカバリ動作のプラグインを管理しているため、上記のようにクラスを定義します。

3, リカバリ動作のコードを書く

ここでやっとリカバリ動作についてコードを書きます！
今回は C++ で書いていきます。
まずはヘッダーファイルを書いていきましょう。

以下のようにディレクトリを作り、その中にヘッダーファイルを書きます。

$ mkdir -p include/go_forward_recovery
$ vim include/go_forward_recovery/go_forward_recovery.h

#ifndef GO_FORWARD_RECOVERY_H_
#define GO_FORWARD_RECOVERY_H_
#include <nav_core/recovery_behavior.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf/transform_listener.h>
#include <ros/ros.h>
#include <base_local_planner/costmap_model.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <angles/angles.h>
#include <sensor_msgs/LaserScan.h>

#define RANGE_MAX 5.6

namespace go_forward_recovery{
  class GoForwardRecovery : public nav_core::RecoveryBehavior {
    public:
      GoForwardRecovery();

      void initialize(std::string name, tf::TransformListener* tf, 
          costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap);

      void runBehavior();

      ~GoForwardRecovery();

    private:
      costmap_2d::Costmap2DROS* global_costmap_, *local_costmap_;
      costmap_2d::Costmap2D costmap_;
      std::string name_;
      tf::TransformListener* tf_;
      ros::NodeHandle n;
      ros::Publisher vel_pub;
      ros::Subscriber scan_sub;
      geometry_msgs::Twist cmd_vel;
      bool initialized_;
      base_local_planner::CostmapModel* world_model_;
      double null_check(double target);
      void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
      int sub_n, sub_flag;
  };
};
#endif

次にいよいよメインのコードを書いていきます。
ヘッダーファイルを書いた時に気がついたかもしれませんが、今回は実用的なパッケージの作成を目指しサブスクライバとパブリッシャをリカバリ動作に仕込みます。

先ほどと同様に、以下のようにディレクトリを作り、その中にコードを書きます。

$ mkdir src
$ vim src/go_forward_recovery.cpp

#include <go_forward_recovery/go_forward_recovery.h>
#include <pluginlib/class_list_macros.h>

//register this planner as a RecoveryBehavior plugin
PLUGINLIB_DECLARE_CLASS(go_forward_recovery, GoForwardRecovery, go_forward_recovery::GoForwardRecovery, nav_core::RecoveryBehavior)

namespace go_forward_recovery {
GoForwardRecovery::GoForwardRecovery(): global_costmap_(NULL), local_costmap_(NULL), 
  tf_(NULL), initialized_(false), world_model_(NULL) {} 

void GoForwardRecovery::initialize(std::string name, tf::TransformListener* tf,
    costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
  if(!initialized_){
    name_ = name;
    tf_ = tf;
    global_costmap_ = global_costmap;
    local_costmap_ = local_costmap;
    world_model_ = new base_local_planner::CostmapModel(*local_costmap_->getCostmap());
    initialized_ = true;
  }else{
    ROS_ERROR("You should not call initialize twice on this object, doing nothing");
  }
}

GoForwardRecovery::~GoForwardRecovery(){
  delete world_model_;
}

double GoForwardRecovery::null_check(double target){
  if(!(target > 0)){
    target = (double)RANGE_MAX;
    //ROS_WARN("RANGE OVER");
  }
  return target;
}

void GoForwardRecovery::scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
  double center_number = (-msg->angle_min)/msg->angle_increment;
  double center = msg->ranges[center_number];
  double left = msg->ranges[center_number+128];
  double right = msg->ranges[center_number-128];

  center = null_check(center);
  left = null_check(left);
  right = null_check(right);

  //ROS_INFO("center: [%lf], left: [%lf], right: [%lf]", center, left, right);
  //ROS_INFO("center number: [%lf]", (-msg->angle_min)/msg->angle_increment);

  if(center < 0.5){
    ROS_WARN("center warning!!");
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 1.0;
  }
  if(left < 0.4){
    ROS_WARN("left warning!!");
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = -1.0;
  }
  if(right < 0.4){
    ROS_WARN("right warning!!");
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 1.0;
  }
  if(center >=0.5 && left >= 0.4 && right >= 0.4){
    cmd_vel.linear.x = 0.2;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
  }

  //ROS_INFO("x: %lf, y: %lf, z: %lf", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
  vel_pub.publish(cmd_vel);
  ROS_INFO("sub_n: %d", sub_n);
  sub_n++;
  if(sub_n > 100){
    scan_sub.shutdown();
    sub_flag = 0;
    return;
  }
}

void GoForwardRecovery::runBehavior(){
  if(!initialized_){
    ROS_ERROR("This object must be initialized before runBehavior is called");
    return;
  }

  ROS_WARN("Go forward recovery behavior started.");
  sub_n = 0;
  sub_flag = 1;
  scan_sub = n.subscribe("scan", 10, &GoForwardRecovery::scanCallback, this);
  vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
  while(n.ok()){
    if(sub_flag == 0){
      return;
    }
  }
}
};

では、解説をしていきます。

まずリカバリ動作の概要について説明します。
このリカバリ動作は基本は前に真っ直ぐ進みます。
その中で /scan トピックから壁などの障害物を検知し、回転し障害物がない方向を向き、直進するリカバリ動作です。(拙いコードですみません...)
また、/scan トピックは北陽電機株式会社製の URG-04LX-UG01 の使用を想定しているため、サブスクライバについては、ご自身の環境に応じて書き換えて下さい。

後日、ここにコードの解説を書きます。

またこの章では、ライセンスの記述を省いています。以下のパッケージからソースコードを参照して下さい。

• GitHub - zaki0929/navigation/go_forward_recovery/
• GitHub - ros-planning/navigation/rotate_recovery/

4, move_base 側の設定

この章では、move_base を編集していきます。
この章が終われば、作ったリカバリ動作がちゃんと組み込まれるようになるはずです。

ではまず package.xml を編集しましょう。

$ rosed move_base package.xml

build_depend と run_depend が並んで書いてあるところに以下を書き足します。
(それぞれ rotate_recovery の下辺りに書くと綺麗だと思います。)

<build_depend>go_forward_recovery</build_depend>
<run_depend>go_forward_recovery</run_depend>

次に、move_base.cpp を編集します。

$ rosed move_base move_base.cpp

このコードの void MoveBase::loadDefaultRecoveryBehaviors() 内を編集します。(いじってなければ1130行目辺りにあるはずです。)
まず、この関数のtry{}の中に以下を書き加えます。

boost::shared_ptr<nav_core::RecoveryBehavior> go_forward(recovery_loader_.createInstance("go_forward_recovery/GoForwardRecovery"));
go_forward->initialize("go_forward_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);

ここまで終わればこっちのもんです。あとは好きなようにリカバリ動作を組み合わせていきましょう。

以下の一行を、先ほど書いたものの下に書けばリカバリ動作を組み込むことができます。

recovery_behaviors_.push_back(go_forward);

デフォルトだとcons_clear, rotate, ags_clear, rotate の順番にリカバリ動作が組み込まれていると思いますが、
これを例えば、cons_clear, rotate, go_forward, ags_clear, rotate, go_forward の順番にしたいとしたら以下のように書きます。

recovery_behaviors_.push_back(cons_clear);
recovery_behaviors_.push_back(rotate);
recovery_behaviors_.push_back(go_forward);
recovery_behaviors_.push_back(ags_clear);
recovery_behaviors_.push_back(rotate);
recovery_behaviors_.push_back(go_forward);

上はあくまで例なので、ご自身の環境や要望にあったリカバリ動作の組み方をしてみてください。

最後に catkin_make をしておきます。

$ roscd
$ cd ..
$ catkin_make

• トップへもどる