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?

@hiyo_control(hiyo control)

ChoreonoidでROS2の学習（Action通信）

0
Last updated at Posted at 2026-05-03

はじめに

　本記事は、著者がROS2のAction通信の勉強のため、choreonoidに自作Action通信を実装した時の知見をまとめたものです。
　本記事値が、筆者のようなROS2諸学者の学習に寄与できれば幸いです。

完成形

　本記事では、最終的にchoreonoiシミュレータ上のカートに対して、下記ができることを目指します。

  • Action clientがAction server（choreonoidコントローラ）に向けてGoal（Feedback送信回数）を送り、それと同時にカートが旋回走行すること
  • 旋回走行中、Action server（choreonoidコントローラ）がAction clientへ向け、一定周期でFeedback（片方の車輪トルク）を送ること
  • Action server（choreonoidコントローラ）がAction clientへResult（全Feedbackの配列）を送ること。それと同時にcartが停止すること

動作環境は下記です。

  • Ubuntu 24.04.3 LTS ※ Virtual Box 7.1.10上で起動
  • Choreonoid 2.3.0
  • ROS2 Jazzy

Action通信プログラム作成

事前準備

　以降の内容を進める前に、まずはChoreonoid公式から出されているROS2モバイルロボットチュートリアルを参照し、

  • choreonoidコントローラのビルド
  • choreonoidコントローラのシミュレータへの適用

が正常にできるところまで進めてください。

実装方針

　Action通信では、Goal、Result、Feedbackの3種類の通信が、Action server～Action client間でやり取りされます（下図参照）。

　本記事では、

  • Goal：Feedbackを送る回数
  • Result：全Feedbackの配列
  • Feedback：車輪（片輪）のトルク

となるよう、Action clientとAction server（choreonoidコントローラ）プログラムを作成します。本記事で作成するプログラムを図にすると以下のようになります。

Action通信プログラム

Action interface

　ROS2のAction通信では、以下のようにAction interfaceを定義する必要があります。

CartActionintarface.action
# Goal
int32 order
---
# Result
float32[] wheel_torque_vector
---
# Feedback
float32[] wheel_torque

Action intarfaceは「---」で区切ります。すると上からGoal、Result、Feedbackの順で型式と変数名が定義されます。

Action clientプログラム

　Action clientプログラム（cart_action_client.cpp）を機能ごとに抜粋して説明します。
　なお、Action clientプログラムの全体像は、Githubリンクを参照ください。
　Action clientプログラム（cart_action_client.cpp）はyoutalkさんのプログラムを参考に作成しました。

　まず、下記記述でAction clientを作成します。

cart_action_client.cpp（Action client作成）
// create Action client
this->client_ptr_ = rclcpp_action::create_client<CartAction>(
  this->get_node_base_interface(),
  this->get_node_graph_interface(),
  this->get_node_logging_interface(),
  this->get_node_waitables_interface(),
  "CartAction");

　次に、Action clientプログラムの主要な機能である、「Action server（choreonoidコントローラ）への「Goal」送信機能」を実装するため、下記を追加します。

cart_action_client.cpp（Goal送信機能）
// Send Goal to Action server (=choreonid controller)
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);

goal_msg、send_goal_optionsの中身は下記です。

  • goal_msg：Action serverへ送るGoalの中身
  • send_goal_options：Goal、Feedback、Resultのコールバック

これは下記のように記述します。

cart_action_client（goal_msgの定義）
// define goal message
auto goal_msg = CartAction::Goal();
goal_msg.order = 10; // Number of times feedback is sent
cart_action_client（send_goal_optionsの定義）
auto send_goal_options = rclcpp_action::Client<CartAction>::SendGoalOptions();
// callback for goal response
send_goal_options.goal_response_callback = [this](const GoalHandleCartAction::SharedPtr & goal_handle)
{
  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");
  }
};

// callback for feedback response
send_goal_options.feedback_callback = [this](GoalHandleCartAction::SharedPtr,const std::shared_ptr<const CartAction::Feedback> feedback)
{
  RCLCPP_INFO(this->get_logger(), "Subscribed wheel now data : %f", feedback->partial_sequence.back());
};

// callback for result response
send_goal_options.result_callback = [this](const GoalHandleCartAction::WrappedResult & result)
{
  switch (result.code) 
  {
    case rclcpp_action::ResultCode::SUCCEEDED:
      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;
  }
  std::stringstream ss;
  ss << "Result received: ";
  for (auto number : result.result->sequence)
  {
    ss << number << " ";
  }
  RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());
  rclcpp::shutdown();
};

　Action clientの実行部分は、下記です。今回はコンポーネント化して実行します。

cart_action_client.cpp（Action client実行部）
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::CartActionClient)

Action server（choreonoidコントローラ）プログラム

　Action serverプログラム（choreonoidコントローラ、MobileRobotDriveController.cpp）を機能ごとに抜粋して説明します。
　なお、Action sever（choreonoidコントローラ）プログラムの全体像は、Githubリンクを参照ください。
　プログラムはchoreonoidチュートリアルのプログラムに編集を加えました。

　まず、下記記述でAction severを作成します。

MobileRobotDriveController.cpp（Action server作成）
// create Action server
this->action_server_ = rclcpp_action::create_server<CartAction>(
  node, // node pointer
  "CartAction", // name of action
  handle_goal, // function that is ececuted when a "Goal" is received
  handle_cancel, // function that is ececuted when a "Cancel" is received
  handle_accepted // function wfunction that is ececuted when a "Goal" is accepted
  );

「handle_goal」、「handle_cancel」、「handle_accepted」は下記記述で定義します。

MobileRobotDriveController.cpp（handle_goal～handle_acceptedの定義）
// function that is ececuted when a "Goal" is received
auto handle_goal = [this](const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const CartAction::Goal> goal)
{
  (void)uuid;
  RCLCPP_INFO(node->get_logger(), "Received goal request with order %d", goal->order);
  if (goal->order > 46)
  {
    return rclcpp_action::GoalResponse::REJECT;
  }
  command.linear.x = 0.5;
  command.angular.z = 1.0;
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
};

// function that is ececuted when a "Cancel" is received
auto handle_cancel = [this](const std::shared_ptr<GoalHandleCartAction> goal_handle)
{
  RCLCPP_INFO(node->get_logger(), "Received request to cancel goal");
  (void)goal_handle;
  return rclcpp_action::CancelResponse::ACCEPT;
};

// function wfunction that is ececuted when a "Goal" is accepted
auto handle_accepted = [this](const std::shared_ptr<GoalHandleCartAction> goal_handle)
{
  auto execute_in_thread = [this, goal_handle]()
  {
    return this->execute(goal_handle);
  };
  std::thread{execute_in_thread}.detach();
};

　Action server（choreonoidコントローラ）の主要な機能である、「Action clientへのResult、Feedbackの送信」は、「handle_accepted」内にある「execute(goal_handle)」に実装します。記述は下記となります。

MobileRobodDriveController.cpp
void execute(const std::shared_ptr<GoalHandleCartAction> goal_handle)
{
  RCLCPP_INFO(node->get_logger(), "Executing goal");
  rclcpp::Rate loop_rate(1);
  const auto goal = goal_handle->get_goal();
  auto feedback   = std::make_shared<CartAction::Feedback>();
  auto & sequence = feedback->partial_sequence;
  auto result     = std::make_shared<CartAction::Result>();
  
  for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) 
  {
    // Confirm cancel request
    if (goal_handle->is_canceling()) 
  {
    result->sequence = sequence;
    goal_handle->canceled(result);
    RCLCPP_INFO(node->get_logger(), "Goal canceled");
    return;
  }
  // Send Feedback to Action sclient
  sequence.push_back( wheels[0]->u() );
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(node->get_logger(), "Published wheel now data: %f", feedback->partial_sequence.back());
  
  loop_rate.sleep();
  }
  if (rclcpp::ok()) 
  {
    // Stop cart
    command.linear.x  = 0.0;
    command.angular.z = 0.0;
  
    // Send Result to Action client
    result->sequence = sequence;
    goal_handle->succeed(result);
    RCLCPP_INFO(node->get_logger(), "Goal succeeded");
  }
}

その他プログラム

　Action clientとAction server（choreonoidコントローラ）以外にも、

  • CMakeLists.txt
  • package.xml
  • visibility_control.h

が必要となりますが、詳しくは以下をご参照ください。

動作確認

ビルド

　実行環境は、

の2つになります。これを下記の様なフォルダ構成で保存してください。

└── ros_ws <- Choreonoidチュートリアルを進める過程で作ります
    ├── build
    ├── install
    ├── log
    └── src
        ├── cart_action_com <- Githubからダウンロードします 
        |   ├── include
        │   │   └── visibility_control.h
        │   ├── meshes
        │   ├── model
        │   ├── project
        │   ├── src
        │   │   ├── cart_action_client.cpp
        │   │   ├── CMakeLists.txt
        │   │   └── MobileRobotDriveController.cpp
        │   ├── CMakeLists.txt
        │   ├── package.xml
        │   └── README.md
        └── cart_action_interface <- Githubからダウンロードします
            ├── action
            │   └── CartActioninterface.action
            ├── CMakeLists.txt
            ├── package.xml
            └── README.md

「ros2_ws」ディレクトリ上で端末を起動し、下記コマンドを実行してビルドします。

~/Document/ros2_ws$ colcon build --packages-select cart_action_interface
~/Document/ros2_ws$ colcon build --packages-select cart_action_com

ビルドに成功すると、「install」ディレクトリ内にファイルが生成されます。

└── ros_ws <- Choreonoidチュートリアルを進める過程で作ります
    ├── build
    ├── install
    |   ├── cart_action_com
    |   |   └── lib
    |   |       ├── cart_action_com
    |   |       |   └── cart_action_client <- 生成物
    |   |       └── choreonoid-2.3
    |   |           └── simplecontroller
    |   |               └── MobileRobotDriveController.so <- 生成物
    |   └── cart_action_interface <- 生成物
    ├── log
    └── src
        ├── cart_action_com <- Githubからダウンロードします 
        └── cart_action_interface <- Githubからダウンロードします

カートロボットの走行実演

　choreonoidシミュレータ上のカートが狙いの動きをするかを確認します。
　まず、「ros2_ws」上で端末を起動し、下記コマンドを実行して「Choreonoid-ROS」を起動します。

~/Document/ros2_ws/$ source ./setup.bash
~/Document/ros2_ws/$ ros2 run choreonoid_ros choreonoid

起動後、以下の場所にある「cart_action_com_simu.cnoid」を読み込ませてください。

└── ros_ws <- Choreonoidチュートリアルを進める過程で作ります
    ├── build
    ├── install
    ├── log
    └── src
        ├── cart_action_com <- Githubからダウンロードします 
        |   ├── include
        │   ├── meshes
        │   ├── model
        │   ├── project
        |   |   └── cart_action_com_simu.cnoid <- これを読み込ませる
        │   ├── src
        │   ├── CMakeLists.txt
        │   ├── package.xml
        │   └── README.md
        └── cart_action_interface <- Githubからダウンロードします

次に、別端末を起動し、下記コマンドを実行します。

~/Documents/ros2_ws/ source ./install/setup.bash
~/Documents/ros2_ws/ ros2 run cart_action_com cart_action_client

　その後、「Choreonoid-ROS」でシミュレーションを実行すると、cartがGoalのやり取りと同時に旋回走行を開始します。先ほど起動した端末を確認すると、2つの端末間でカートの片輪トルクの値がやり取り（=Feedback）されていることがわかります。
　片輪トルクのやり取り（=Feedback）が完了すると、やり取りされた全ての片輪トルクの配列が出力されます（=Result）。

以上、一連の動作確認方法は以下の動画にまとめました。

参考書籍・webサイト

製作物保存先

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?