0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@mhieda(稗田 睦生)

ROS2のAction Clientを理解しながらNav2のNavigateToPoseで単一ゴールを送信するノードを作成

0
Last updated at Posted at 2026-02-08

はじめに

今までROS2で簡単なノードを実装する際は，Topic通信のみで事足りていたが，この度Nav2による自律移動を実装するにあたって，Action Clientの実装が必要になった．

Nav2でNavigationを実行している際，Rviz2のGoal Pose ボタンから，目標位置姿勢を手で矢印によって与えることで，自律移動をすることができる．しかし巡回タスクといった実用上では，人がいちいち手で目標を与えていられず，自動で目標位置まで移動し，決まったタスクを実行するといった実装が必要となる．ここで，この Goal Pose ボタン自体は内部でNavigateToPoseアクションにgoalを送信するように実装されているため，同じことを行う自作のAction Clientを実装すればよいということがわかる．今回はこのノードを作成する．

スクリーンショット 2026-02-08 183259.png
▲ GoalPoseを配置する様子

読者層

  • ROS 2初心者
  • Topic通信は実装したことがあるが，Action通信からは避けてきた人（自分）
  • Nav2初心者

この記事でわかること

  • ROS 2 Action通信のClientの実装の基礎（Python）
  • Nav2のNavigateToPoseを用いて単一ゴールを送信するノードを自作する方法

NavigateToPoseにはBehavior Treeの設定項目が存在するが，今回は混乱を避けるためこの設定には触れない．Behavior Treeを指定しない場合には，Nav2によりデフォルトの設定が使用される．

Action通信とNav2

ROS2におけるAction通信はサービス通信と似ており，実行を要求するクライアントと要求を受けて実行するサーバの関係である．サービス通信と違う点はサーバの実行時にクライアントにフィードバックを返すという点である．Action通信は実行に時間がかかる処理に対して実装され，実行中のロボットの状態や目的地までの残り時間といった情報をフィードバックする．さらに，Action通信では，中断処理も実装できる．

Action-SingleActionClient.gif
▲ Action通信の概要図（引用：https://docs.ros.org/en/foxy/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html ）

Nav2で実装する自律移動においても，長い時間のかかるタスクであるため，一般的にAction通信で実装される．Nav2のActionサーバと通信するためにnav2_msgsと呼ばれるNav2特有の .action 型が存在する． .action はアクション通信におけるGoal, Result, Feedbackの型を決めたものであり，トピック通信でのメッセージと同じ扱いである．

NavigateToPoseについて

NavigateToPose とはnav2_msgsの中のアクション通信の型の一つであり，ロボットを単一のゴールポイントへと移動させる場合に用いられる．このAction型でClientが要求を投げれば，それに従って実行しフィードバックを受けることができる．

NavigateToPose は以下のように定義されている．

NavigateToPose.action

# Error codes
# Note: The expected priority order of the errors should match the message order
uint16 NONE=0

#goal definition
geometry_msgs/PoseStamped pose
string behavior_tree
---
#result definition
uint16 error_code
---
#feedback definition
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining

実装

まず今回 NavigateToPose 型のAction Clientを実装した例を示す．
pose_navigator.py

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node

from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose

def make_goal_pose(node: Node):
    now = node.get_clock().now().to_msg()
    goal_pose = PoseStamped()

    def euler_to_quaternion(roll=0.0, pitch=0.0, yaw=0.0):
        import math
        from geometry_msgs.msg import Quaternion
        q = Quaternion()
        q.x = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        q.y = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
        q.z = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
        q.w = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        return q

    def pose(x, y, yaw):
        pose = PoseStamped()
        pose.header.stamp = now
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation = euler_to_quaternion(0.0, 0.0, float(yaw))
        return pose

    goal_pose = pose(1.0, 0.0, 0.0)
    return goal_pose

class PoseNavigator(Node):

    def __init__(self):
        super().__init__('pose_navigator')
        self.action_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')

    def send_goal(self):
        if not self.action_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error("navigate_to_pose action server not available.")
            rclpy.shutdown()
            return

        goal_msg = NavigateToPose.Goal()
        goal_pose = make_goal_pose(self)
        goal_msg.pose = goal_pose
        
        future = self.action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback)
        future.add_done_callback(self.goal_response_callback)
        return future

    def feedback_callback(self, feedback_msg):
        fb = feedback_msg.feedback
        self.get_logger().info(f'Current pose: {fb.current_pose}')
        self.get_logger().info(f'Navigation time: {fb.navigation_time}')
        self.get_logger().info(f'Estimated time remaining: {fb.estimated_time_remaining}')
        self.get_logger().info(f'Number of recoveries: {fb.number_of_recoveries}')
        self.get_logger().info(f'Distance remaining: {fb.distance_remaining}')

    def goal_response_callback(self, future):
        goal_handle = future.result()

        if not goal_handle.accepted:
            self.get_logger().error("Goal was rejected.")
            rclpy.shutdown()
            return

        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.result_callback)

    def result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f'Goal result: {result}')
        rclpy.shutdown()

def main(args=None):
    rclpy.init(args=args)

    action_client = PoseNavigator()

    action_client.send_goal()

    rclpy.spin(action_client)

if __name__ == '__main__':
    main()

実装コードの詳細説明

全コードに説明を付しており長いので，結果のみ見たい方は目次機能等を使ってスキップしていただくのがよい．

from nav2_msgs.action import NavigateToPose

Action型 NavigateToPose.action をインポートする

def make_goal_pose(node: Node):
    now = node.get_clock().now().to_msg()
    goal_pose = PoseStamped()

    def euler_to_quaternion(roll=0.0, pitch=0.0, yaw=0.0):
        import math
        from geometry_msgs.msg import Quaternion
        q = Quaternion()
        q.x = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        q.y = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
        q.z = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
        q.w = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        return q

    def pose(x, y, yaw):
        pose = PoseStamped()
        pose.header.stamp = now
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation = euler_to_quaternion(0.0, 0.0, float(yaw))
        return pose

    goal_pose = pose(1.0, 0.0, 0.0)
    return goal_pose

ここでは，座標や姿勢を計算するための関数make_goal_pose() を作成しており，今回目標座標と姿勢は(x, y, yaw) = (1.0, 0.0, 0.0)としている．NavigateToPose でGoalとして与える座標や姿勢は，geometry_msgs/PoseStamped 型に含まれるgeometry_msgs/Pose 型であり，こちらは姿勢としてクォータニオンを与える形式になっているので，roll, pitch, yawからクォータニオンを計算する関数として，eular_to_quaternion() を実装している．

それぞれのメッセージ型の詳細

geometry_msgs/msg/PoseStamped.msg

# A Pose with reference coordinate frame and timestamp

std_msgs/Header header
Pose pose

std_msgs/msg/Header.msg

# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.

# Two-integer timestamp that is expressed as seconds and nanoseconds.

builtin_interfaces/Time stamp

# Transform frame with which this data is associated.

string frame_id

geometry_msgs/msg/Pose.msg

# A representation of pose in free space, composed of position and orientation.

Point position
Quaternion orientation

geometry_msgs/msg/Point.msg

# This contains the position of a point in free space

float64 x
float64 y
float64 z

geometry_msgs/msg/Quaternion.msg

# This represents an orientation in free space in quaternion form.

float64 x 0
float64 y 0
float64 z 0
float64 w 1

class PoseNavigator(Node):

NavigateToPose型のAction Client Nodeとして，PoseNavigatorというクラス名で定義した．

def __init__(self):
    super().__init__('pose_navigator')
    self.action_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')

クラスのinit関数では，Client Nodeを作成する．NavigateToPose はAction型の名前，/navigate_to_pose はそのActionを受け取るサーバの名前である．

def send_goal(self):

Action通信のGoalをサーバに送信する関数である．

if not self.action_client.wait_for_server(timeout_sec=5.0):
    self.get_logger().error("navigate_to_pose action server not available.")
    rclpy.shutdown()
    return

タイムアウトを設けて，時間内（ここでは5秒）にサーバとの通信ができない場合には，Clientの要求を停止する．

goal_msg = NavigateToPose.Goal()

NavigateToPose 型のGoalメッセージのインスタンスを生成している．

NavigateToPose.actionを見ると，”Goal”という文言はないが，このようにアクセスできるのは，Action通信では —- を区切りに，Goal部，Result部，Feedback部に分かれていると決まっているためである．区切りを元に，ROSIDLが自動でNavigateToPose.Goal , NavigateToPose.Result , NavigateToPose.Feedbackを作成するため，PythonからはNavigateToPose.Goal() のような形でアクセスできるようである．

goal_pose = make_goal_pose(self)
goal_msg.pose = goal_pose

前述のmake_goal_pose() により目標位置姿勢のデータを作成し，Goalメッセージのインスタンスに渡す．

future = self.action_client.send_goal_async(
    goal_msg, feedback_callback=self.feedback_callback)

Action Serverにaction_client.send_goal_async() によってGoalを非同期で送信し，その送信結果を表すfutureを受け取る．futureとは，非同期処理が将来（future）返す結果を格納するプレースホルダーである．送信したGoalが受理されたかどうかが届き次第，futureを通じて取得できる．ここで，第一引数には送信するGoalメッセージ，第二引数として，実行中にサーバから返ってくるフィードバックを受けるためのコールバック関数を登録する．（ここでは後述のfeedback_callback()）

future.add_done_callback(self.goal_response_callback)

Goalがサーバに送信され，受理されたかどうかを受けるためのコールバック関数を登録する．（ここでは後述のgoal_response_callback()）

return future

サーバにGoalを送信するsend_goal() の返り値はfutureオブジェクトである．

def feedback_callback(self, feedback_msg):
    fb = feedback_msg.feedback

サーバからのフィードバックを受ける関数feedback_callback() を定義する．フィードバックはfeedbackというフィールド名でアクセスできる．これが，NavigateToPose.Goal()のように大文字から始まらないのは，フィードバックのメッセージが，NavigateToPose.Feedback そのものではなく，それを包んだAction通信内部用のラッパーメッセージであるためである．これはその中に，goal_idと実際のNavigateToPose.Feedbackを含むfeedbackというフィールドを持つためである（ややこしい）．

self.get_logger().info(f'Current pose: {fb.current_pose}')
self.get_logger().info(f'Navigation time: {fb.navigation_time}')
self.get_logger().info(f'Estimated time remaining: {fb.estimated_time_remaining}')
self.get_logger().info(f'Number of recoveries: {fb.number_of_recoveries}')
self.get_logger().info(f'Distance remaining: {fb.distance_remaining}')

NavigateToPose.action のfeedback definitionにあった属性についてアクセスし，表示する．

def goal_response_callback(self, future):

Goalがサーバに送信され，受理されたかどうかを受けるためのコールバック関数goal_response_callback()を宣言

goal_handle = future.result()

futureの結果を取り出す．取得したGoalHandleは結果取得やキャンセルにも使用できる．

if not goal_handle.accepted:
    self.get_logger().error("Goal was rejected.")
    rclpy.shutdown()
    return

サーバがこのGoalを受理したかどうかを確認する．falseなら拒否されたことを表示してノードを終了する．

result_future = goal_handle.get_result_async()

Navigationが終了したときのResultを非同期で待つ．終了するまでresult_futureには値が返されない．

result_future.add_done_callback(self.result_callback)

Navigationが完了したタイミングで，起動するコールバックを登録する．（ここではresult_callback()）

def result_callback(self, future):

Navigationが完了したタイミングで，起動するコールバックとしてresult_callback()を宣言

result = future.result().result

future.result()はGetResultのラッパーであり，statusとresultを持つ．そして，.result でNavigationToPose.Result型で実際のResultを得られる．

self.get_logger().info(f'Goal result: {result}')
rclpy.shutdown()

Result をログ出力する．この result の中には Nav2 のエラーコードなどが入っている．

そして，ノードを終了する．

def main(args=None):
    rclpy.init(args=args)

    action_client = PoseNavigator()

    action_client.send_goal()

    rclpy.spin(action_client)

クライアントのインスタンス生成を行い，send_goal() によりサーバにgoalを送信．一連の流れが終了するまでノードをspinする．

結果

目標座標と姿勢として与えた(x, y, yaw) = (1.0, 0.0, 0.0)に向かって自律的に移動させることができた．実行中に，ロボットの状態のログが出力されているのもわかる．
output.gif

0
0
0

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
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?