環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
ROSの基本的な通信であるpub、subは一方通行の通信です。それとは別に相手に命令を送ってその返信をもらうという方式としてrosserviceがありますが、これはrostopic ehcoができなかったり、client側がブロッキング処理になったりと使い勝手が悪くあまり推奨されません。
この「命令を送信して返信をもらう」という通信を実現するものとして「actionlib」があります。
基本的な概念は以下のようです。client（最初の命令を送るROSノード）がgoal（命令）を投げます。server（最初の命令を受けるROSノード）はそれを受けてfeedback（命令の途中経過）を返しながら最終的にresult（命令の最終結果）を返します。
実際には5つのトピックがclientとserverの間でやり取りされます。なおこれらのROSトピックはactionlibのAPIを通してpub、subするのでユーザーが直接扱うものではありません。
| Topic名 | 方向 | 役割 |
|---|---|---|
| goal | client -> server | 命令 |
| cancel | client -> server | 命令のキャンセル |
| feedback | client <- server | 命令の途中結果 |
| result | client <- server | 命令の最終結果 |
| status | client <- server | serverの状態を常に送っている |
ソースコード
Actionのmsgの定義
msgの定義なのでこれをros_lecture_msgsの中で定義します。
上からgoal、result、feedbackの定義になります。書き方はROSメッセージの定義と同じです。
ros_lecture_msgs/action/Task.action
# Define the goal
uint32 task_id # Specify which dishwasher we want to use
float64 duration
---
# Define the result
bool done
---
# Define a feedback message
float32 rate
ビルドの設定を書きます。
ros_lecture_msgs/CMakeLists.txtに追加
find_package(catkin REQUIRED COMPONENTS
actionlib_msgs # この行を追加
)
add_action_files(
FILES
Task.action
)
generate_messages(
DEPENDENCIES
std_msgs
actionlib_msgs # この行を追加
)
client
adv_lecture/src/adv_action_client.cpp
#include <ros/ros.h>
#include <ros_lecture_msgs/TaskAction.h>
#include <actionlib/client/simple_action_client.h>
#include <stdio.h>
#include <termios.h>
#include <fcntl.h>
#include <time.h>
struct termios save_settings;
void set_input_interactive(void)
{
// set no-echo-back and non-blocking
struct termios settings;
tcgetattr(0, &save_settings);
settings = save_settings;
settings.c_lflag &= ~(ECHO | ICANON);
settings.c_cc[VTIME] = 0;
settings.c_cc[VMIN] = 1;
tcsetattr(0, TCSANOW, &settings);
fcntl(0, F_SETFL, O_NONBLOCK);
}
void reset_input(void)
{
tcsetattr(0, TCSANOW, &save_settings);
}
typedef actionlib::SimpleActionClient<ros_lecture_msgs::TaskAction> Client;
int main(int argc, char** argv)
{
ros::init(argc, argv, "task_client");
ros::NodeHandle nh;
set_input_interactive();
Client client("task", true);
printf("a: send goal (in 1s)\n");
printf("s: send goal (in 5s)\n");
printf("c: cancel goal\n");
printf("q: quit\n");
int task_id = 0;
bool initial_goal = false;
ros::Rate loop_rate(2);
while (ros::ok())
{
if (client.isServerConnected())
{
char c = getchar();
if (c == 'a')
{
ros_lecture_msgs::TaskGoal goal;
goal.task_id = task_id;
task_id++;
goal.duration = 1.0;
client.sendGoal(goal);
printf("publish goal id:%i, duration:%f\n", goal.task_id, goal.duration);
initial_goal = true;
}
else if (c == 's')
{
ros_lecture_msgs::TaskGoal goal;
goal.task_id = task_id;
task_id++;
goal.duration = 5.0;
client.sendGoal(goal);
printf("publish goal id:%i, duration:%f\n", goal.task_id, goal.duration);
initial_goal = true;
}
else if (c == 'c')
{
client.cancelGoal();
printf("publish cancel\n");
}
else if (c == 'q')
{
break;
}
if (initial_goal)
printf("Current State: %s\n", client.getState().toString().c_str());
}
fflush(stdout);
ros::spinOnce();
loop_rate.sleep();
}
reset_input();
return 0;
}
- actionlibの書き方としてcallback引数でresultやcallbackを使うこともできますが、今回はべたで書いてみます。
- 初めのほうはターミナルからの標準入力を受け取るためのものです。
- a、sを押すとgoalを送信、cでキャンセルを送ります。
-
#include <ros_lecture_msgs/TaskAction.h>の様に(ROSパッケージ)/(acitionファイル名)Action.hをincludeします。 - actionlibのクライアントは
actionlib::SimpleActionClient<(ROSパッケージ)/(acitionファイル名)Action>となります。第1引数はactionlibの通信に使うROSトピックのベース名になります。 - 送信するgoalの型は
(ROSパッケージ)/(acitionファイル名)Actionでとなります。.sendGoal関数でserverに送信します。
server
adv_lecture/src/adv_action_server.cpp
#include <ros/ros.h>
#include <ros_lecture_msgs/TaskAction.h>
#include <actionlib/server/simple_action_server.h>
typedef actionlib::SimpleActionServer<ros_lecture_msgs::TaskAction> Server;
int main(int argc, char** argv)
{
ros::init(argc, argv, "task_server");
ros::NodeHandle nh;
Server server(nh, "task", false);
server.start();
ros::Time start_time;
ros::Rate loop_rate(2);
ros_lecture_msgs::TaskGoalConstPtr current_goal;
while (ros::ok())
{
if (server.isNewGoalAvailable())
{
current_goal = server.acceptNewGoal();
start_time = ros::Time::now();
printf("Update Goal\n");
}
if (server.isActive())
{
if (server.isPreemptRequested())
{
server.setPreempted();
printf("Preempt Goal\n");
}
else
{
if (start_time + ros::Duration(current_goal->duration) < ros::Time::now())
{
server.setSucceeded();
// server.setAborted();
printf("Active: publish result id:%i\n", current_goal->task_id);
}
else
{
ros_lecture_msgs::TaskFeedback feedback;
feedback.rate = (ros::Time::now() - start_time).toSec() / current_goal->duration;
server.publishFeedback(feedback);
printf("Active: publish feedback id:%i\n", current_goal->task_id);
}
}
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
- goalを受け取るとその中のduration秒後にSucceededを送ります。
clientとserverのCMakeList
adv_lecture/CMakeLists.txtの一部
add_executable(adv_action_client src/adv_action_client.cpp)
add_executable(adv_action_server src/adv_action_server.cpp)
add_dependencies(adv_action_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(adv_action_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(adv_action_client
${catkin_LIBRARIES}
)
target_link_libraries(adv_action_server
${catkin_LIBRARIES}
)
上記ではROSノードの生成の部分です。actionメッセージへの依存が必要です。
package.xml
/adv_lecture/package.xml
<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
actionlib関連の依存を追加します。
ビルド
cd ~/catkin_ws
catkin_make
実行
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
1つめのターミナル
roscore
2つめのターミナル
rosrun adv_lecture adv_action_client
3つめのターミナル
rosrun adv_lecture adv_action_server
2つ目のターミナルでキー入力をするとactionlibのやり取りをします。
参考
actionlibの遷移について
actionlib API