Posted at

ROS講座95 actionlibを使う


環境

この記事は以下の環境で動いています。

項目

CPU
Core i5-8250U

Ubuntu
16.04

ROS
Kinetic

インストールについてはROS講座02 インストールを参照してください。

またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。


概要

ROSの基本的な通信であるpub、subは一方通行の通信です。相手に命令を送ってその返信をもらうという通信方式としてrosserviceがありますが、これはrostopic ehcoができないなどのためにデバッグ性が悪くて使用は推奨されません。

この「命令を送信して返信をもらう」という通信を実現するための方式として「actionlib」があります。

基本的な概念は以下のようです。client(最初の命令をおくるROSノード)がgoal(命令)を投げます。server(最初の命令を受けるROSノード)はそれを受けてfeedback(命令の途中経過)を返しながらresult(命令の最終結果)を返します。

実際には5つのトピックがclientとserverの間でやり取りされます。なおこれらのROSトピックはactionlibを通してpub、subするのでユーザーが直接扱うものではありません。

Topic名
方向
役割

goal
client -> server
命令

cancel
client -> server
命令のキャンセル

feedback
client <- server
命令の途中結果

result
client <- server
命令の最終結果

status
client <- server
serverの状態を常に送っている


ソースコード


Actionファイル


adv_lecture/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

上からgoal、result、feedbackです。


clientのソースコード


adv_lecture/src/adv_action_client.cpp

#include <ros/ros.h>

#include <adv_lecture/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<adv_lecture::TaskAction> Client;

int main(int argc, char** argv){
ros::init(argc, argv, "do_dishes_client");
ros::NodeHandle nh;
set_input_interactive();

Client client("do_dishes", 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;
ros::Rate loop_rate(2);
while(ros::ok()){
if(client.isServerConnected()){
char c = getchar();

if (c == 'a'){
adv_lecture::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);
}
else if (c == 's'){
adv_lecture::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);
}
else if (c == 'c'){
client.cancelGoal();
printf("publish cancel\n");
}
else if (c == 'q'){
break;
}
printf("Current State: %s\n", client.getState().toString().c_str());
}
fflush(stdout);
ros::spinOnce();
loop_rate.sleep();
}
reset_input();

return 0;
}


初めのほうはターミナルからの標準入力を受け取るためのものです。

a、sを押すとgoalを送信、cでキャンセルを送ります。


serverのソースコード


adv_lecture/src/adv_action_server.cpp

#include <ros/ros.h>

#include <adv_lecture/TaskAction.h>
#include <actionlib/server/simple_action_server.h>

typedef actionlib::SimpleActionServer<adv_lecture::TaskAction> Server;

int main(int argc, char** argv)
{
ros::init(argc, argv, "do_dishes_server");
ros::NodeHandle nh;
Server server(nh, "do_dishes", false);
server.start();

ros::Time start_time;
ros::Rate loop_rate(2);
adv_lecture::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.setAborted();
//server.setPreempted();
printf("Preempt Goal\n");
}
else{
if(start_time+ros::Duration(current_goal->duration)<ros::Time::now()){
server.setSucceeded();
printf("Active: publish result id:%i\n", current_goal->task_id);
}
else{
adv_lecture::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を送ります。


CMakeList


adv_lecture/CMakeLists.txtの一部

find_package(catkin REQUIRED COMPONENTS

genmsg
actionlib_msgs
)
add_action_files(
FILES
Task.action
)
generate_messages(
DEPENDENCIES
std_msgs
actionlib_msgs
)

上記ではactionメッセージの生成に関する部分です。


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})
add_dependencies(adv_action_server ${${PROJECT_NAME}_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


実行


1つめのターミナル

roscore



2つめのターミナル

rosrun adv_lecture adv_action_client 



3つめのターミナル

rosrun adv_lecture adv_action_server 



参考

actionlibの遷移について

actionlib API


目次ページへのリンク

ROS講座の目次へのリンク