LoginSignup
13
6

More than 1 year has passed since last update.

@porizou1

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

Last updated at Posted at 2021-06-01

はじめに

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

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

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://answers.ros.org/question/337994/architecture-of-the-navigaton2-stack/

ROS2のActionについて

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

ROS1のmove_baseのActionとの違い

move_baseのActionメッセージ

Goalにgeometry_msgs/PoseStamped型のtarget_poseを指定し、Feedbackのbase_positionで現在の位置を受け取ります。

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
が取得できるようになっています。

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度だけゴールを送信するサンプルプログラムです。

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の宣言と初期化

  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が実行されるまで待つ

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

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

    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も設定できます。

    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にゴールを送信

 client_ptr_->async_send_goal(goal_msg, send_goal_options);

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

  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シミュレーションを起動

$ ros2 launch nav2_bringup tb3_simulation_launch.py

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

rviz_screenshot_2021_05_23-19_11_38.png

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

 $ ros2 run nav2_send_goal nav2_send_goal

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

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

[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を使っていきます。

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

参考

13
6
1

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
13
6