ROS2 Navigation2 Action Clientを使用してゴールを送信する

# はじめに

この記事ではROS2のAction Clientを使用してROS2ノードからNavigation2にゴールを送信するサンプルプログラムについて紹介します。

この記事は以下の記事の続きです。

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

# Navigation2の構成について

以下の図はNavigation2のソフトウェアアーキテクチャの図です。
ROS1の`NavigationStack`のmove_baseでは単一プロセスのステートマシンによる制御だったシステムが機能ごとにノードに分割され、それぞれのノードはROS2のActionを使って通信をしています。

別ノードから`nav2_msgs/action/NavigateToPose`型のActionメッセージを`BT Navigator Server`に送信し、ゴールの座標を指定すると、`PlannerServer`でグローバルプランが生成され、`ControllerServer`によってグローバルプランに追従するローカルプランを生成し`Twist`型の/cmd_velトピックが出力されロボットが動くといった流れです。

`Recovery Server`は途中障害物などで有効なパスが得られない場合に復帰動作を行う、move_baseのrecovery_behaviorsに当たる機能です。

![1574227359795773.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/5cd7bb47-9cb6-160f-3715-d940872f156c.png)

図は以下より引用
https://answers.ros.org/question/337994/architecture-of-the-navigaton2-stack/

# ROS2のActionについて

ROS2のアクションについてはこの記事では省略します。
公式ドキュメントを参照してください。
基本的にはROS1のActionと同じ




https://docs.ros.org/en/foxy/Tutorials/Understanding-ROS2-Actions.html

https://docs.ros.org/en/foxy/Tutorials/Actions/Writing-a-Cpp-Action-Server-Client.html



# ROS1のmove_baseのActionとの違い


### move_baseのActionメッセージ
Goalに`geometry_msgs/PoseStamped`型の`target_pose`を指定し、Feedbackの`base_position`で現在の位置を受け取ります。

https://github.com/ros-planning/navigation_msgs/blob/ros1/move_base_msgs/action/MoveBase.action


```txt:MoveBase.action
geometry_msgs/PoseStamped target_pose
---
---
geometry_msgs/PoseStamped base_position
```

### navigation2のActionメッセージ

Goalに`geometry_msgs/PoseStamped`型の`pose`を指定。
Feedbackは現在位置 `current_pose`だけでなく、navigationの時間 `navigation_time`や、ゴールまでの距離 `distance_remaining`、実行されているRecovery動作の番号 `number_of_recoveries`
が取得できるようになっています。

https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/action/NavigateToPose.action


```txt:NavigateToPose.action
#goal definition
geometry_msgs/PoseStamped pose
string behavior_tree
---
#result definition
std_msgs/Empty result
---
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
int16 number_of_recoveries
float32 distance_remaining
```




# サンプルプログラム

以下は起動時に1度だけゴールを送信するサンプルプログラムです。


```cpp:nav2_send_goal.cpp
#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"

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

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

  explicit Nav2Client(): Node("nav2_send_goal")
  {
    //アクション Client の作成
    this->client_ptr_  = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
  }

  void sendGoal(void) {
    // アクションが提供されているまでに待つ
    while (!this->client_ptr_->wait_for_action_server()) {
      RCLCPP_INFO(get_logger(), "Waiting for action server...");
    }

    //アクション　Goalの作成
    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 = 2;
    goal_msg.pose.pose.position.y = 0;
    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;

    //進捗状況を表示するFeedbackコールバックを設定
    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    send_goal_options.feedback_callback = std::bind(&Nav2Client::feedbackCallback, this, _1, _2);
    send_goal_options.result_callback = std::bind(&Nav2Client::resultCallback, this, _1);
    //Goal をサーバーに送信
    client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }
  //feedback
  void feedbackCallback(GoalHandleNavigateToPose::SharedPtr,const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    RCLCPP_INFO(get_logger(), "Distance remaininf = %f", feedback->distance_remaining);
  }
  //result
  void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(get_logger(), "Success!!!");
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(get_logger(), "Unknown result code");
        return;
    }
  }
};

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

  rclcpp::shutdown();
  return 0;
}
```

Action Clientの宣言と初期化

```cpp
  using NavigateToPose = nav2_msgs::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
  rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_;

this->client_ptr_  = rclcpp_action::create_client<NavigateToPose>(shared_from_this(), "navigate_to_pose");
```

以下sendGoal関数内
Action Serverが実行されるまで待つ

```cpp
    while (!this->client_ptr_->wait_for_action_server()) {
      RCLCPP_INFO(get_logger(), "Waiting for action server...");
    }
```

ゴールメッセージの作成、ここで目標地点の位置(m)と姿勢（クオータニオン）をgeometry_msgs/PoseStamped型で作成

```cpp
    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 = 2;
    goal_msg.pose.pose.position.y = 0;
    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;
```

`feedback_callback`関数と`result_callback`関数の設定
`feedback_callback`は実行中に一定周期で実行、`result_callback`は終了時に1度だけ実行される。
また、このプログラムでは書いていませんが、ゴール送信時に1度だけ実行される`goal_response_callback`も設定できます。

```cpp
    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    send_goal_options.feedback_callback = std::bind(&Nav2Client::feedbackCallback, this, _1, _2);
    send_goal_options.result_callback = std::bind(&Nav2Client::resultCallback, this, _1);
```

Serverにゴールを送信

```cpp
 client_ptr_->async_send_goal(goal_msg, send_goal_options);
```


feedback_callback関数の中身
ゴールまでの距離を表示する処理を実行

```cpp
  void feedbackCallback(GoalHandleNavigateToPose::SharedPtr,const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    RCLCPP_INFO(get_logger(), "Distance remaininf = %f", feedback->distance_remaining);
  }
```



# 実行

Navigation2とTurtlebot3のGazeboシミュレーションを起動

```bash
$ ros2 launch nav2_bringup tb3_simulation_launch.py
```

いつも通りrviz2画面の`2D Pose Estimate`をクリックしてamclの初期位置を指定


![rviz_screenshot_2021_05_23-19_11_38.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/6f2b642e-99b2-7a4d-ed9f-ec5d2f530595.png)

サンプルプログラムの実行　

```bash
 $ ros2 run nav2_send_goal nav2_send_goal
```

Turtlebot3がプログラム中で指定したゴール地点に向かって動き出します。

ゴールに到着したら結果を表示して終了

```bash
[INFO] [1621764280.615185527] [nav2_send_goal]: Distance remaininf = 0.219025
[INFO] [1621764280.625130608] [nav2_send_goal]: Distance remaininf = 0.219016
[INFO] [1621764280.635064577] [nav2_send_goal]: Distance remaininf = 0.219016
[INFO] [1621764280.645718466] [nav2_send_goal]: Distance remaininf = 0.219016
[INFO] [1621764280.655024359] [nav2_send_goal]: Distance remaininf = 0.219010
[INFO] [1621764280.665025725] [nav2_send_goal]: Distance remaininf = 0.219010
[INFO] [1621764280.675023866] [nav2_send_goal]: Distance remaininf = 0.219010
[INFO] [1621764280.684872744] [nav2_send_goal]: Distance remaininf = 0.219010
[INFO] [1621764280.695111439] [nav2_send_goal]: Distance remaininf = 0.219002
[INFO] [1621764280.706341275] [nav2_send_goal]: Distance remaininf = 0.219002
[INFO] [1621764280.715009755] [nav2_send_goal]: Success!!!
```



# おわりに

この記事ではROS2ノードからNavigation2にゴールを送信する方法について紹介しました。
次はNavigation2で新たに追加された機能であるWaypoint Followerを使っていきます。

記事に間違いがありましたらコメントでご指摘ください。

https://github.com/porizou/nav2_send_goal_sample

# 参考



https://daily-tech.hatenablog.com/entry/2017/02/11/214336

https://gbiggs.github.io/rosjp_ros2_intro/ros2_basics.html

https://github.com/fmrico/nav2_test_utils/tree/master/src