ROS2で道案内ロボットをつくる〜ナビゲーション編（後編）〜

# はじめに
　本記事では、移動型ロボットが**事前に登録されたパスをたどりつつ**も**後方から追随するユーザーを置いてけぼりにしない**ような「道案内ロボット」の実装について紹介しています。
　後半では、得られたデータをサブスクライブし、ゴールを更新したりキャンセルしたりする方法について紹介します。
　なお前半の記事はこちら

https://qiita.com/sfc_nakanishi_lab/private/a134b6540276aed9a760


# 1.指標物体の情報をサブスクライブする
　前編で紹介した独自のメッセージを立てるので、ワークスペース直下に`imgProc_msgs`ディレクトリを用意し、ビルドしたあとで反映されるようにしましょう。
　今回は半径の大きさのみをもって、認識されているかいないかの判定を行うのでコールバック関数ではピンポン玉の半径を取得します。
```callback.func
//抜粋
void topic_callback(const imgproc_msgs::msg::Imgproc::SharedPtr msg) const
{
  ballRadius = msg->radius;
  RCLCPP_INFO(this->get_logger(), "radius: '%f'", ballRadius);
}
```
　C++で書く際にコールバック関数の属性がconstになっているため、外部で宣言した変数への代入ができないです。これを解決するのに、メンバ変数の`ballRadius`の属性をmutableにしています。

# 2.決められたパスをたどる
　ノードの中でActionクライアントを立てて、逐一ゴールを`PoseStamped`型で送信しています。今回は便宜的に座標だけを引数に指定して、回転は定数にしています。設定してあるパスは以下のような形でVectorで定義しています。
```Path
//座標地点用の構造体
struct Vec2
{
    float x;
    float y;

    Vec2()
        : x(0.0)
        , y(0.0) {}

    Vec2(float _x, float _y)
        : x(_x)
        , y(_y) {}
};

//ゴールとなるポイントを設定したVector配列
std::vector<Vec2> wayPoints;
wayPoints.push_back(Vec2(2.0, 2.0));
wayPoints.push_back(Vec2(2.0, 0.0));
wayPoints.push_back(Vec2(0.0, 0.0));
```
　地図によって適宜目標地点を設定してください。

# 3.パスをキャンセルする
　実行中のゴールをキャンセルするには以下のメソッドから実行可能です
```
client_ptr_->async_cancel_all_goals();
```
# 4.コード
　なお、Navigation2に対してゴールを送信するのに[こちら](https://qiita.com/porizou1/items/cb9382bb2955c144d168)の記事にあるコードを参考にしました。基本的には、ボールが検出されている（＝半径が０以上）場合にはゴールを送ることができ、されていない場合にはゴールをキャンセルします。また、ボールが検出されていて、ゴールに一定距離近づいたら次のゴールを送信します。

```navigation_manager.cpp
#include <vector>  
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "rclcpp/time.hpp"

#include "imgproc_msgs/msg/imgproc.hpp"

using namespace std::chrono_literals;
using std::placeholders::_1;
using std::placeholders::_2;
using std::placeholders::_3;

struct Vec2
{
    float x;
    float y;

    Vec2()
        : x(0.0)
        , y(0.0) {}

    Vec2(float _x, float _y)
        : x(_x)
        , y(_y) {}
};

class NavManagerClass : public rclcpp::Node
{
public:
  using NavigateToPose = nav2_msgs::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
  rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_;

  int counter = 0;
  float dist = 1.0;
  std::vector<Vec2> wayPoints;
  mutable std::vector<float> radiusBuffer;
  mutable int max_buffer_size = 10;
  mutable float radius_avg = 0.0;

  bool isAborted = false;
  bool isCanceled = false;
  bool moveFlag = false;
  mutable float ballRadius = 0.0;
  explicit NavManagerClass(): Node("navigation_manager")
  {
    //waypoints
    /*
    wayPoints.push_back(Vec2(2.0, 2.0));
    wayPoints.push_back(Vec2(2.0, 0.0));
    wayPoints.push_back(Vec2(4.0, 2.5));
    wayPoints.push_back(Vec2(-2.0, 2.0));
    wayPoints.push_back(Vec2(0.0, 0.0));
    */

    wayPoints.push_back(Vec2(0.0, 2.0));
    wayPoints.push_back(Vec2(2.0, 0.0));
    wayPoints.push_back(Vec2(0.0, -2.0));
    wayPoints.push_back(Vec2(-2.0, 0.0));

    //create action client
    this->client_ptr_  = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
    //timer call back
    timer_ = this->create_wall_timer(2000ms, std::bind(&NavManagerClass::timer_callback, this));
    //subscriber
    subscription_ = this->create_subscription<imgproc_msgs::msg::Imgproc>("img_data", 10, std::bind(&NavManagerClass::topic_callback, this, _1));

  }

  void sendGoal(float x, float y) {
    int counter= 0;
    bool entryFlag = false;

    while (!entryFlag)
    {
      counter ++;
      if(this->client_ptr_->wait_for_action_server()) 
      {
        entryFlag = true;
      }
      else
      {
        if(counter > 100000)
        {
          RCLCPP_INFO(get_logger(), "No connection to server");
          exit(0);
        } 
      }
    }
    
    auto goal_msg = NavigateToPose::Goal();
    goal_msg.pose.header.stamp = this->now();
    goal_msg.pose.header.frame_id = "map";

    goal_msg.pose.pose.position.x = x;
    goal_msg.pose.pose.position.y = y;
    goal_msg.pose.pose.orientation.x = 0.0;
    goal_msg.pose.pose.orientation.y = 0.0;
    goal_msg.pose.pose.orientation.w = 1.0;
    goal_msg.pose.pose.orientation.z = 0.0;

    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    send_goal_options.feedback_callback = std::bind(&NavManagerClass::feedbackCallback, this, _1, _2);
    send_goal_options.result_callback = std::bind(&NavManagerClass::resultCallback, this, _1);
    
    client_ptr_->async_send_goal(goal_msg, send_goal_options);
    
  }
  void feedbackCallback(GoalHandleNavigateToPose::SharedPtr,const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    RCLCPP_INFO(get_logger(), "Distance remaininf = %f", feedback->distance_remaining);

    dist = feedback->distance_remaining;
  
  }
  void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_ERROR(get_logger(), "Success!!!");
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(get_logger(), "Goal was aborted");
        isAborted = true;
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(get_logger(), "Goal was canceled");
        isCanceled = true;
        return;
      default:
        RCLCPP_ERROR(get_logger(), "Unknown result code");
        return;
    }
  }

  private:
    void timer_callback()
    {
      
      RCLCPP_INFO(this->get_logger(), "TIMERCALLBACK");

      if(ballRadius > 0)
      {
        moveFlag = true;
      }
      else
      {
        moveFlag = false;
      }

      if(moveFlag)
      {
        if(dist < 0.35)
        {
            counter ++;
            sendGoal(wayPoints[counter%wayPoints.size()].x, wayPoints[counter%wayPoints.size()].y);  
        }

        if(isAborted || isCanceled)
        {
          sendGoal(wayPoints[counter%wayPoints.size()].x, wayPoints[counter%wayPoints.size()].y);  
          isAborted = false;
          isCanceled = false;
        }
      }
      else
      {
         client_ptr_->async_cancel_all_goals();
      }      
    }

    void topic_callback(const imgproc_msgs::msg::Imgproc::SharedPtr msg) const
    {
      ballRadius = msg->radius;
      RCLCPP_INFO(this->get_logger(), "radius: '%f'", ballRadius);

      
    }
    
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<imgproc_msgs::msg::Imgproc>::SharedPtr subscription_;
    
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<NavManagerClass>();
  node->sendGoal(-0.70, 0.0);
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}


```

なお、画像データをサブスクライブしてナビゲーションのアクションサーバへ送るノードが含まれるパッケージは以下のリポジトリに投稿しています。よろしければご参照ください。

https://github.com/Takagi-Waluigi/guide_operation

# 5.Gazeboで試す
  ターミナルの画面を5つ立てて実行します。
　まずは、Gazebo
```
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```
　次に、Nav2のブリングアップ
```
ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=/opt/ros/foxy/share/nav2_bringup/maps/turtlebot3_world.yaml default_bt_xml_filename:=$HOME/btdata/sample.xml param_file:=$HOME/my_param/test_param01.yaml
```
　そして、Rviz
```
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz
```
　ここまで出来たら初期位置をRviz上で設定しておきましょう。

　画像認識ノードを立てます。
```
cd ~/(your_ws)
. install/setup.bash
ros2 run image_processor colball_detection 
```
　ナビゲーション管理のノードを立てます。
```
cd ~/(your_ws)
. install/setup.bash
ros2 run guide_operation nav_manager 
```

　この挙動をRviz上で試したものです。カメラはPCの備え付けのウェブカメラを使用してボールを見せたり隠したりしています。

<iframe width="560" height="315" src="https://www.youtube.com/embed/hiX9rYuKY60" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

# 6.実機によるデモ
　実機のブリングアップと、画像認識ノードをrunさせた上で、Gazebo以外の先程のコマンドを叩いています。なお、実機に対するNavigation2のブリングアップ等については、以前投稿した[こちら](https://qiita.com/sfc_nakanishi_lab/items/028edb3a7d5ed0300e33)の記事をご参考下さい。


<br><iframe width="560" height="315" src="https://www.youtube.com/embed/glW0RIj6Wjo" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

犬の散歩のようです（笑）

# 所感・展望
　今回は指標物体としてピンポン玉を使用しましたが、Yoloなどを用いて精度のより高い人物検知やデプスカメラを用いた検知をしてみても良いかなーと少し思っています。

# 参考資料

https://qiita.com/porizou1/items/cb9382bb2955c144d168

https://docs.ros.org/en/diamondback/api/actionlib/html/classactionlib_1_1simple__action__client_1_1SimpleActionClient.html

https://qiita.com/sfc_nakanishi_lab/items/028edb3a7d5ed0300e33