🐷

Nav2 Waypoint Followerをノードから複数Waypointを指定する

2022/11/04に公開

ROS2

tech

まえがき

ROS2 galacticで独立差動二輪ロボットを自作しています。今回はNav2で実装されています複数のWaypointを順番にたどっていくWaypoint Followerをノードから呼び出してみました。

環境

自作Intel PC（Ubuntu 20.04LTS) & ROS2 galactic(インストール済みの前提)
Turtlebot3シミュレーションが動かせること
これについては、porizou1さんがご説明して頂いてるのでこれを参照してください。

調査

Nav2のWaypoint Followerのドキュメントには以下のような記載がありました（日本語訳はGoogle翻訳）

The Waypoint Follower module implements a way of doing waypoint following using the NavigateToPose action server.
Waypoint Follower モジュールは、NavigateToPose アクションサーバーを使用してウェイポイントフォローイングを実行する方法を実装します。

つまり、Nav2のconfigでwaypoint followerを実行していればアクションサーバが立ち上がっているはず、と推測できます。

まず、起動しているノードを見ました。

$ ros2 node list
WARNING: Be aware that are nodes in the graph that share an exact name, this can have unintended side effects.
/amcl
/amcl_rclcpp_node
/bt_navigator
/bt_navigatornavigate_through_poses_rclcpp_node
/bt_navigatornavigate_to_pose_rclcpp_node
/controller_server
/controller_server_rclcpp_node
/gazebo
/global_costmap/global_costmap
/global_costmap/global_costmap_rclcpp_node
/global_costmap_client
/lifecycle_manager_localization
/lifecycle_manager_navigation
/local_costmap/local_costmap
/local_costmap/local_costmap_rclcpp_node
/local_costmap_client
/map_server
/planner_server
/planner_server_rclcpp_node
/recoveries_server
/recoveries_server_rclcpp_node
/robot_state_publisher
/rviz2
/rviz2
/rviz2
/rviz2
/transform_listener_impl_557a379e3e40
/transform_listener_impl_5590feb0f730
/transform_listener_impl_55d3ac96bff0
/transform_listener_impl_564b1a226f70
/transform_listener_impl_7fd23000bdd0
/turtlebot3_diff_drive
/turtlebot3_imu
/turtlebot3_joint_state
/turtlebot3_laserscan
/waypoint_follower

一番下に/waypoint_followerというのがありましたので、これの詳細を見ます。

$ ros2 node info /waypoint_follower 
/waypoint_follower
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
  Publishers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /rosout: rcl_interfaces/msg/Log
    /waypoint_follower/transition_event: lifecycle_msgs/msg/TransitionEvent
  Service Servers:
    /waypoint_follower/change_state: lifecycle_msgs/srv/ChangeState
    /waypoint_follower/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /waypoint_follower/get_available_states: lifecycle_msgs/srv/GetAvailableStates
    /waypoint_follower/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions
    /waypoint_follower/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /waypoint_follower/get_parameters: rcl_interfaces/srv/GetParameters
    /waypoint_follower/get_state: lifecycle_msgs/srv/GetState
    /waypoint_follower/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions
    /waypoint_follower/list_parameters: rcl_interfaces/srv/ListParameters
    /waypoint_follower/set_parameters: rcl_interfaces/srv/SetParameters
    /waypoint_follower/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:
    /follow_waypoints: nav2_msgs/action/FollowWaypoints
  Action Clients:
    /navigate_to_pose: nav2_msgs/action/NavigateToPose

立ち上がっているアクションサーバは/follow_waypointsでした。このaction型について確認しましょう。

$ ros2 interface show nav2_msgs/action/FollowWaypoints 
#goal definition
geometry_msgs/PoseStamped[] poses
	std_msgs/Header header
		builtin_interfaces/Time stamp
			int32 sec
			uint32 nanosec
		string frame_id
	Pose pose
		Point position
			float64 x
			float64 y
			float64 z
		Quaternion orientation
			float64 x 0
			float64 y 0
			float64 z 0
			float64 w 1
---
#result definition
int32[] missed_waypoints
---
#feedback
uint32 current_waypoint

ということで、/follow_waypointsアクションサーバに対し、geometry_msgs/PoseStampedの配列をゴールとして投げると動くんじゃねーの？と推測しました。

単一のゴールを投げるやり方は、やはりporizou1さんのQiita記事にありましたのでそれを参考にプログラムを作成しました。

サンプルプログラム

waypointは４つ作りました。

#include <memory>
#include <chrono>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <nav2_msgs/action/follow_waypoints.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rclcpp/time.hpp>

using namespace std::chrono_literals;

using FollowWaypoints = nav2_msgs::action::FollowWaypoints;
using GoalHandleFollowWaypoints = rclcpp_action::ClientGoalHandle<FollowWaypoints>;

class WaypointFollowerClient : public rclcpp::Node
{
public:
  WaypointFollowerClient(rclcpp::NodeOptions options = rclcpp::NodeOptions());
  ~WaypointFollowerClient();
    
private:
  void execute();
  void onGoalResponseReceived(std::shared_future<GoalHandleFollowWaypoints::SharedPtr> future);
  void onFeedbackReceived(
			  GoalHandleFollowWaypoints::SharedPtr,
			  const std::shared_ptr<const FollowWaypoints::Feedback> feedback);
  void onResultReceived(const GoalHandleFollowWaypoints::WrappedResult & result);
    
  rclcpp_action::Client<FollowWaypoints>::SharedPtr client_ptr_;
  rclcpp_action::ResultCode result_code_;
};

WaypointFollowerClient::WaypointFollowerClient(rclcpp::NodeOptions options)
  : Node("waypoint_follower_client", options)
{
  client_ptr_ = rclcpp_action::create_client<FollowWaypoints>(this, "follow_waypoints");
  std::thread{&WaypointFollowerClient::execute, this}.detach();
}
  
WaypointFollowerClient::~WaypointFollowerClient()
{
}
  
void WaypointFollowerClient::execute()
{
  using std::placeholders::_1;
  using std::placeholders::_2;

  auto goal_msg = FollowWaypoints::Goal();
  goal_msg.poses.resize(4);

  goal_msg.poses[0].header.frame_id = "map";
  goal_msg.poses[0].header.stamp = this->now();
  goal_msg.poses[0].pose.position.x = 2.0;
  goal_msg.poses[0].pose.position.y = 0.0;
  goal_msg.poses[0].pose.orientation.x = 0.0;
  goal_msg.poses[0].pose.orientation.y = 0.0;
  goal_msg.poses[0].pose.orientation.z = 1.0;
  goal_msg.poses[0].pose.orientation.w = 0.0;

  goal_msg.poses[1].header.frame_id = "map";
  goal_msg.poses[1].header.stamp = this->now();
  goal_msg.poses[1].pose.position.x = 0.0;
  goal_msg.poses[1].pose.position.y = 2.0;
  goal_msg.poses[1].pose.orientation.x = 0.0;
  goal_msg.poses[1].pose.orientation.y = 0.0;
  goal_msg.poses[1].pose.orientation.z = -0.7;
  goal_msg.poses[1].pose.orientation.w = 0.7;

  goal_msg.poses[2].header.frame_id = "map";
  goal_msg.poses[2].header.stamp = this->now();
  goal_msg.poses[2].pose.position.x = -2.0;
  goal_msg.poses[2].pose.position.y = 0.0;
  goal_msg.poses[2].pose.orientation.x = 0.0;
  goal_msg.poses[2].pose.orientation.y = 0.0;
  goal_msg.poses[2].pose.orientation.z = 0.0;
  goal_msg.poses[2].pose.orientation.w = 1.0;

  goal_msg.poses[3].header.frame_id = "map";
  goal_msg.poses[3].header.stamp = this->now();
  goal_msg.poses[3].pose.position.x = 0.0;
  goal_msg.poses[3].pose.position.y = -2.0;
  goal_msg.poses[3].pose.orientation.x = 0.0;
  goal_msg.poses[3].pose.orientation.y = 0.0;
  goal_msg.poses[3].pose.orientation.z = 0.7;
  goal_msg.poses[3].pose.orientation.w = 0.7;

  RCLCPP_INFO(this->get_logger(), "Sending goal");

  auto send_goal_options = rclcpp_action::Client<FollowWaypoints>::SendGoalOptions();
  send_goal_options.goal_response_callback =
    std::bind(&WaypointFollowerClient::onGoalResponseReceived, this, _1);
  send_goal_options.feedback_callback =
    std::bind(&WaypointFollowerClient::onFeedbackReceived, this, _1, _2);
  send_goal_options.result_callback =
    std::bind(&WaypointFollowerClient::onResultReceived, this, _1);

  result_code_ = rclcpp_action::ResultCode::UNKNOWN;

  std::this_thread::sleep_for(std::chrono::seconds(1));

  auto result = client_ptr_->async_send_goal(goal_msg, send_goal_options);

  while (rclcpp::ok() && result_code_ == rclcpp_action::ResultCode::UNKNOWN) {
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
  std::this_thread::sleep_for(std::chrono::seconds(1));

  rclcpp::shutdown();
}

void WaypointFollowerClient::onGoalResponseReceived(std::shared_future<GoalHandleFollowWaypoints::SharedPtr> future)
{
  auto goal_handle = future.get();
  if (!goal_handle) {
    RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
  } else {
    RCLCPP_INFO(this->get_logger(), 
		"Goal accepted by server, waiting for result");
  }
}

void WaypointFollowerClient::onFeedbackReceived(
						GoalHandleFollowWaypoints::SharedPtr,
						const std::shared_ptr<const FollowWaypoints::Feedback> feedback)
{
  RCLCPP_INFO(this->get_logger(), "CurrentWaypoint = %d", feedback->current_waypoint);
}

void WaypointFollowerClient::onResultReceived(const GoalHandleFollowWaypoints::WrappedResult & result)
{
  result_code_ = result.code;

  switch (result.code) {
  case rclcpp_action::ResultCode::SUCCEEDED:
    RCLCPP_INFO(this->get_logger(), "Goal was succeeded");
    for (unsigned int i = 0; i < result.result->missed_waypoints.size(); i++) {
      RCLCPP_INFO(this->get_logger(), "missed waypoints: %d", result.result->missed_waypoints[i]);
    }
    break;
  case rclcpp_action::ResultCode::ABORTED:
    RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
    return;
  case rclcpp_action::ResultCode::CANCELED:
    RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
    return;
  default:
    RCLCPP_ERROR(this->get_logger(), "Unknown result code");
    return;
  }
}


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

  rclcpp::shutdown();
  return 0;
}

アクションに投げるゴールの数を増やして、それぞれのゴールに座標を設定するだけのプログラムになりました。

隘路事項

上記コードの中で、async_send_goal関数の前にスレッドで１秒待っているところがあります。
これを入れないとゴールが渡せませんでした。

  std::this_thread::sleep_for(std::chrono::seconds(1));

  auto result = client_ptr_->async_send_goal(goal_msg, send_goal_options);

理由は不明です。わかった方はぜひご連絡・コメント頂けば幸いです。

最後に

Nav2のWaypoint Followerをノードから呼び出してみました。

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

まえがき

環境

調査

サンプルプログラム

隘路事項

最後に

Discussion