1. 程式人生 > >ROS學習筆記:actionlib

ROS學習筆記:actionlib

在任何大型的基於ROS的系統中,都有這樣的情況:有人想向某個節點發送請求,以執行某些任務,並接收對請求的應答。這可以通過ROS服務來實現。但是,在某些情況下,如果服務需要很長時間執行,使用者可能希望在執行過程中取消請求,或者得到關於請求進展情況的定期反饋。actionlib包提供了建立伺服器的工具,這些伺服器執行可被搶佔的長期目標。它還提供了一個客戶端介面,以便向伺服器傳送請求。

actionlib介面不但可以排程任務的執行,而且具備中斷任務、任務狀態跟蹤與週期性狀態反饋、執行過程中間結果反饋的能力。

ActionClient與 ActionServer的互動設計

actionlib介面定義了ActionClient(任務請求的客戶端)與ActionServer(任務排程的伺服器端)。

Goal
在移動底盤的情況下,目標將是一個包含關於機器人應該移動到世界何處的資訊的、具有固定功能的資訊。為了控制傾斜鐳射掃描器,目標將包含掃描引數(最小角度,最大角度,速度等)。

Feedback
反饋為伺服器實現者提供了一種方式來告訴一個ActionClient關於一個目標的漸進進展。對於移動底盤,這可能是機器人在路徑上的當前姿態。為了控制傾斜的鐳射掃描器,這可能是在掃描完成之前的時間。

Result
在完成目標後,將結果從ActionServer傳送到ActionClient。這與反饋不同,因為它只發送一次。當行動的目的是提供某種資訊時,這是非常有用的。對於移動底盤,其結果並不十分重要,但它可能包含機器人的最終姿態。為了控制傾斜的鐳射掃描器,結果可能包含從所請求的掃描產生的點雲。

寫一個簡單的伺服器和客戶端

.action檔案

建立名為Fibonacci.action檔案。該.action檔案具有目標定義,隨後的結果定義,隨後通過反饋定義,與由3連字號(分隔每個部分---)。這些檔案放在包的./action目錄中,看起來非常類似於服務的.srv檔案。

#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence

修改CMakeList.txt檔案

find_package(catkin REQUIRED COMPONENTS actionlib std_msgs message_generation) 
add_action_files(DIRECTORY action FILES Fibonacci.action)
generate_messages(DEPENDENCIES std_msgs actionlib_msgs)

修改package.xml檔案

<exec_depend>message_generation</exec_depend>

編譯工作空間

$ cd ../.. # Go back to the top level of your catkin workspace
$ catkin_make
$ ls devel/share/actionlib_tutorials/msg/
FibonacciActionFeedback.msg  FibonacciAction.msg        FibonacciFeedback.msg
FibonacciResult.msg          FibonacciActionGoal.msg    FibonacciActionResult.msg  FibonacciGoal.msg
$ ls devel/include/actionlib_tutorials/
FibonacciActionFeedback.h  FibonacciAction.h        FibonacciFeedback.h  FibonacciResult.h
FibonacciActionGoal.h      FibonacciActionResult.h  FibonacciGoal.h

2.建立伺服器

建立一個名為fibonacci_server的C++檔案。

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>//實現簡單行為使用的行為庫
#include <actionlib_tutorials/FibonacciAction.h>//包含了Fibonacci.action檔案中生成的行為訊息

class FibonacciAction
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  std::string action_name_;
  // 建立用作反饋/結果的訊息
  actionlib_tutorials::FibonacciFeedback feedback_;
  actionlib_tutorials::FibonacciResult result_;

public:

  FibonacciAction(std::string name) :
    as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
    action_name_(name)
  {
    as_.start();
  }

  ~FibonacciAction(void)
  {
  }

  void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
  {
    // helper variables
    ros::Rate r(1);
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    feedback_.sequence.clear();
    feedback_.sequence.push_back(0);
    feedback_.sequence.push_back(1);

    // 將資訊對映到螢幕上
    ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);

    // 開始執行行為
    for(int i=1; i<=goal->order; i++)
    {
      // 檢查客戶端是否未請求搶佔
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        //將行為狀態設定為preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
      // 釋出反饋
      as_.publishFeedback(feedback_);
      // this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
      r.sleep();
    }

    if(success)
    {
      result_.sequence = feedback_.sequence;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // 將行為狀態設定為succeeded
      as_.setSucceeded(result_);
    }
  }


};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "fibonacci");

  FibonacciAction fibonacci("fibonacci");
  ros::spin();

  return 0;
}

3.建立客戶端

建立fibonacci_client的C++檔案

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <actionlib_tutorials/FibonacciAction.h>

int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_fibonacci");

  // 建立行為客戶端
  // true會開啟客戶端建立自己的執行緒
  actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  // 等待伺服器開啟
  ac.waitForServer(); //will wait for infinite time

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  actionlib_tutorials::FibonacciGoal goal;
  goal.order = 20;
  ac.sendGoal(goal);

  // 等待行為返回
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));

  if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
  }
  else
    ROS_INFO("Action did not finish before the time out.");

  return 0;
}

啟動伺服器和客戶端節點

$ roscore
$ rosrun actionlib_tutorials fibonacci_server
$ rosrun actionlib_tutorials fibonacci_client