ROS講座95 actionlibを使う

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

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ROSの基本的な通信であるpub、subは一方通行の通信です。それとは別に相手に命令を送ってその返信をもらうという方式としてrosserviceがありますが、これはrostopic ehcoができなかったり、client側がブロッキング処理になったりと使い勝手が悪くあまり推奨されません。
この「命令を送信して返信をもらう」という通信を実現するものとして「actionlib」があります。

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

![adv_actionlib_diagram1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/e10614bc-ce35-d9ba-76ff-37f0a5ecbf9b.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メッセージの定義と同じです。

```text: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
```

ビルドの設定を書きます。

```cmake: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

```c++: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

```c++: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

```cmake: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

```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関連の依存を追加します。

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell:1つめのターミナル
roscore
```

```shell:2つめのターミナル
rosrun adv_lecture adv_action_client 
```

```shell:3つめのターミナル
rosrun adv_lecture adv_action_server 
```

2つ目のターミナルでキー入力をするとactionlibのやり取りをします。

# 参考
[actionlibの遷移について](http://wiki.ros.org/actionlib/DetailedDescription)
[actionlib API](http://docs.ros.org/melodic/api/actionlib/html/index.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)