actionlib學習

ROS中的服務service是一問一答的形式，你來查詢了，我就返給你要的信息。
action也有服務的概念，但是它不一樣的地方是：不是一問一答，而多了一個反饋，它會不斷反饋項目進度。

如navigation下的move_base package，你設定了目標點，反饋信息可能是機器人在規划路徑上的即時位姿，

直到機器人到達目標點，返回SUCCEEDED消息。

 

上面所述的 ActionClient 和 ActionServer 通過 ROS Action Protocol （ROS Action 協議，建立在ROS messages的基礎上）來通信。

Client&Server 為用戶提供了簡單的API，通過函數的調用和回調，用來在client端request一個目標，或者，在server端來執行達成一個目標。

下圖說明這個機制如何運行：

Action Specification: Goal, Feedback, & Result

Goal：

為了用actions機制完成一個任務，我們引入了the notion of a goal, 這個goal可以被ActionClient發送到ActionServer. 比如在move base

這個案例中，它的類型是PoseStamped，包含了機器人應該到達的哪里的信息。在激光掃描雲台控制案例中，the goal會包含掃描的參數

（min angle, max angle, speed 等）

Feedback

Feedback是server用來告訴ActionClient goal執行過程中的各種情況。在moving_base案例中，它是機器人現在的位姿；

在controlling the tilting laser scanner案例中, this might be the time left until the scan completes（掃描剩余時間）.

Result

執行結果，比如在move_base中的結果和機器人pose；在雲台激光掃描中的一個請求的點雲數據。等

下面寫一個actionlib的c++例程

 

catkin_create_pkg actionlib_tutorials roscpp rospy actionlib actionlib_msgs message_generation

在包中添加action文件夾在里面新建一個 Fibonacci.action文件

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

 修改package.xml添加

  <run_depend>message_generation</run_depend>

 修改 CMakeLists.txt刪除添加；編譯cpp文件根據自己的添加

add_action_files(
DIRECTORY action
FILES Fibonacci.action
)
generate_messages(DEPENDENCIES
  actionlib_msgs
)
  CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy

 

簡單的client.cpp

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

using namespace actionlib_tutorials;
typedef actionlib::SimpleActionClient<FibonacciAction> Client;

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

  // Create the action client
  Client ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  ac.waitForServer();
  ROS_INFO("Action server started, sending goal.");

  // Send Goal
  FibonacciGoal goal;
  goal.order = 10;
  ac.sendGoal(goal);
  bool finish_before_timeout=ac.waitForResult(ros::Duration(15));
  
  if(finish_before_timeout)
  {
      actionlib::SimpleClientGoalState state = ac.getState();
      ROS_INFO("action finish : %s",state.getState().toString().c_str());
  }else
  {
      ROS_INFO("TIMEOUT");
  }

  ros::spin();
  return 0;
}

 帶回調顯示的clientprocess.cpp

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

using namespace actionlib_tutorials;
typedef actionlib::SimpleActionClient<FibonacciAction> Client;

// Called once when the goal completes
void doneCb(const actionlib::SimpleClientGoalState& state,
            const FibonacciResultConstPtr& result)
{
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  ROS_INFO("Answer: %i", result->sequence.back());
  ros::shutdown();
}

// Called once when the goal becomes active
void activeCb()
{
  ROS_INFO("Goal just went active");
}

// Called every time feedback is received for the goal
void feedbackCb(const FibonacciFeedbackConstPtr& feedback)
{
  ROS_INFO("Got Feedback of length %lu", feedback->sequence.size());
}

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

  // Create the action client
  Client ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  ac.waitForServer();
  ROS_INFO("Action server started, sending goal.");

  // Send Goal
  FibonacciGoal goal;
  goal.order = 20;
  ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

  ros::spin();
  return 0;
}

 server.cpp

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>

typedef actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> Server;

class FibonacciAction
{
protected:

  ros::NodeHandle nh_;
  Server as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  std::string action_name_;
  // create messages that are used to published feedback/result
  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);

    // publish info to the console for the user
    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]);

    // start executing the action
    for(int i=1; i<=goal->order; i++)
    {
      // check that preempt has not been requested by the client
      if (as_.isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("%s: Preempted", action_name_.c_str());
        // set the action state to preempted
        as_.setPreempted();
        success = false;
        break;
      }
      feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
      // publish the feedback
      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());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }


};


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

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

  return 0;
}

 編譯后運行

rosrun actionlib_tutorials client

rosrun actionlib_tutorials sever

 

在設置時間內服務完成則狀態輸出完成，否則反饋失敗，這種等待模式效率太低

 

 

rosrun actionlib_tutorials clientprocess

rosrun actionlib_tutorials sever

 

將動作完成過程實時的將狀態反饋回來，直到動作完成或失敗