Go to Qiita Advent Calendar 2024 Top
LoginSignup
13
12

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@srs(project_srs)

ROS講座95 actionlibを使う

Last updated at Posted at 2019-02-16

環境

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

項目 値
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（命令の最終結果）を返します。

adv_actionlib_diagram1.png

実際には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

目次ページへのリンク

ROS講座の目次へのリンク

13
12
1

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
13
12

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?