ROS actionlib的使用

最新推荐文章于 2021-05-10 21:36:12 发布

学之之博未若知之之要知之之要未若行之之实

最新推荐文章于 2021-05-10 21:36:12 发布

阅读量3.7k

点赞数 4

转载：https://www.cnblogs.com/21207-iHome/p/8297099.html

actionlib是ROS中一个很重要的功能包集合，尽管在ROS中已经提供了srevice机制来满足请求—响应式的使用场景，但是假如某个请求执行时间很长，在此期间用户想查看执行的进度或者取消这个请求的话，service机制就不能满足了，但是actionlib可满足用户这种需求。例如，控制机器人运动到地图中某一目标位置，这个过程可能复杂而漫长，执行过程中还可能强制中断或反馈信息，这时actionlib就能大展伸手了。

actionlib使用client-server工作模式，ActionClient 和ActionServer通过"ROS Action Protocol"进行通信，"ROS Action Protocol"以ROS消息方式进行传输。此外ActionClient 和ActionServer给用户提供了一些简单的接口，用户使用这些接口可以完成goal请求（client-side）和goal执行（server-side）。

　　ActionClient 和ActionServer之间使用action protocol通信，action protocol就是预定义的一组ROS message，这些message被放到ROS topic上在 ActionClient 和ActionServer之间进行传实现二者的沟通。

　　ROS Messages：

goal - Used to send new goals to servers. 代表一个任务，可以被ActionClient发送到ActionServer。比如在MoveBase中，它的类型是PoseStamped，包含了机器人运动目标位置的信息。
cancel - Used to send cancel requests to servers
status - Used to notify clients on the current state of every goal in the system
feedback - Used to send clients periodic auxiliary information for a goal. 服务端定期告知Client当前Goal执行过程中的情况。在Move Base案例中，它表示机器人当前姿态。
result - Used to send clients one-time auxiliary information upon completion of a goal

　　ROS系统在action文件（文件名后缀为.action）中定义了上述goal、result、feedback等消息。The .action file has the goal definition, followed by the result definition, followed by the feedback definition, with each section separated by 3 hyphens (---).

　　下面是一个示意的例子，在./action/DoDishes.action文件中对洗碗这一任务进行定义：goal为使用某一洗碗机洗碗，result为总共洗好的碗数目，feedback为洗碗进度。

复制代码

# Define the goal
uint32 dishwasher_id  # Specify which dishwasher we want to use
---
# Define the result
uint32 total_dishes_cleaned
---
# Define a feedback message
float32 percent_complete

复制代码

　　下面在catkin_ws/src目录下创建一个测试package：

catkin_create_pkg actionlib_test roscpp std_msgs actionlib actionlib_msgs message_generation rospy

　　在package的CMakeLists.txt文件中加入下面这几行：

#find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)
add_action_files(DIRECTORY action FILES DoDishes.action)
generate_messages(DEPENDENCIES actionlib_msgs)

　　注意如果使用catkin_create_pkg创建包时没有添加actionlib相关的依赖项，要将上面CMakeLists中第一行的注释去掉，另外还要在package.xml文件中加入下面几行。因为我们在创建包时就添加好了相关依赖，所以这一步骤可以省略。

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend>
<run_depend>actionlib_msgs</run_depend>

　　使用catkin_make编译即可查看生成的消息文件，这些消息之后将会用于ActionClient 和 ActionServer间的通信。

　　另外可以看到，在devel/include/actionlib_test/中生成了相关的头文件：

C++ SimpleActionClient

　　client示例代码client.cpp如下，它会等待Server连接，发送Goal，获取状态。SimpleActionClient完整的API可以参考C++ SimpleActionClient

复制代码

#include <actionlib_test/DoDishesAction.h> 
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<actionlib_test::DoDishesAction> Client;

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

  Client client("do_dishes", true); // true -> don't need ros::spin()
  client.waitForServer(); // Waits for the ActionServer to connect to this client
  actionlib_test::DoDishesGoal goal;
  // Fill in goal here
  client.sendGoal(goal); // Sends a goal to the ActionServer
  client.waitForResult(ros::Duration(5.0)); // Blocks until this goal finishes
  if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    printf("Yay! The dishes are now clean\n");
  printf("Current State: %s\n", client.getState().toString().c_str());
  return 0;
}

复制代码

C++ SimpleActionServer

　　服务端代码server.cpp如下，SimpleActionServert完整的API可以参考 C++ SimpleActionServer

复制代码

#include <actionlib_test/DoDishesAction.h> 
#include <actionlib/server/simple_action_server.h>

typedef actionlib::SimpleActionServer<actionlib_test::DoDishesAction> Server;

void execute(const actionlib_test::DoDishesGoalConstPtr& goal, Server* as) 
{
  // Do lots of awesome groundbreaking robot stuff here
  as->setSucceeded();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "do_dishes_server");
  ros::NodeHandle n;
  Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
  server.start();
  ros::spin();
  return 0;
}

复制代码

　　在CMakeLists文件中加入下面这几行：

add_executable(client   src/client.cpp)
add_executable(server   src/server.cpp)
target_link_libraries( client ${catkin_LIBRARIES})
target_link_libraries( server ${catkin_LIBRARIES})

　　使用catkin_make进行编译完成后输入指令rosrun actionlib_test server 运行server，通过rostopic list查看系统中的话题如下：

　　使用rqt_graph命令可以查看节点和消息的关系，可以看出server端会接收goal和cancel消息，并发出status、result、feedback消息：

https://www.cnblogs.com/21207-iHome/p/8297099.html

　　接着输入指令rosrun actionlib_test client 运行client，结果如下图所示：

Python SimpleActionClient

　　除了C++也可以使用Python实现同样的功能，client.py如下（API可以参考Python SimpleActionClient）：

复制代码

#! /usr/bin/env python

import roslib
roslib.load_manifest('actionlib_test')
import rospy
import actionlib

from actionlib_test.msg import DoDishesAction, DoDishesGoal

if __name__ == '__main__':
    rospy.init_node('do_dishes_client')
    client = actionlib.SimpleActionClient('do_dishes', DoDishesAction)
    client.wait_for_server()

    goal = DoDishesGoal()
    # Fill in the goal here
    client.send_goal(goal)https://www.cnblogs.com/21207-iHome/p/8297099.html
    client.wait_for_result(rospy.Duration.from_sec(5.0))

复制代码

Python SimpleActionServer

　　server.py程序如下（API可参考Python SimpleActionServer）：

复制代码

#! /usr/bin/env python

import roslib
roslib.load_manifest('actionlib_test')
import rospy
import actionlib

from actionlib_test.msg import DoDishesAction

class DoDishesServer:
  def __init__(self):
    self.server = actionlib.SimpleActionServer('do_dishes', DoDishesAction, self.execute, False)
    self.server.start()

  def execute(self, goal):
    # Do lots of awesome groundbreaking robot stuff here
    self.server.set_succeeded()


if __name__ == '__main__':
  rospy.init_node('do_dishes_server')
  server = DoDishesServer()
  rospy.spin()

复制代码

　　注意在运行程序前先用chmod +x命令给Python文件添加可执行权限：

　　运行client.py和server.py，注意client.py运行就会返回：

参考：

actionlib

actionlib-Tutorials

actionlib-Detailed Description

actionlib的身世之谜

ROS基础--Actionlib应用篇

ROS知识（15）----Actionlib的使用（一）

ROS actionlib的使用方法2

在ROS actionlib学习（一）中的例子展示了actionlib最基本的用法，下面我们看一个稍微实际一点的例子，用actionlib计算斐波那契数列，并发布反馈（feedback）和结果（result）。斐波那契数列指的是这样一个数列：

1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233，377，610，987，1597，2584，4181，6765，10946，17711，28657，46368........

　　这个数列从第3项开始，每一项都等于前两项之和。

　　首先在action文件中定义goal、result、feedback，其中goal是斐波那契数列的阶数，result为最终生成的数列，feedback为当前一步的中间结果（也是一个数列）。

复制代码

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

复制代码

　　然后按照教程SimpleActionServer(ExecuteCallbackMethod)中所述的步骤，修改CMakeLists.txt以及package.xml文件，编译成功后会生成相应的消息文件以及头文件。

复制代码

$ 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

复制代码

　　下面编写服务端程序，用于处理客户端发送的请求。

复制代码

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

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_;
  // 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();  // Explicitly start the action server
  }

  ~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(); // signals that the action has been preempted by user request
        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;
}

复制代码

　　客户端程序用于发送计算请求，服务器收到请求后会生成20阶的斐波那契数列。

复制代码

#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");
// the action client is constructed with the server name and the auto spin option set to true.
  actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci", true); //The action client is templated on the action definition, specifying what message types to communicate to the action server with

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start (Since the action server may not be up and running, the action client will wait for the action server to start before continuing)
  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); // the goal value is set and sent to the action serve

  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0)); // The timeout on the wait is set to 30 seconds, this means after 30 seconds the function will return with false if the goal has not finished.

  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.");

  //exit
  return 0;
}

复制代码

　　修改CMakeLists.txt后使用catkin_make编译上面两个程序。运行roscore开启ROS主节点，然后在两个新终端中分别输入下面命令分别运行server和client：

rosrun actionlib_tutorials fibonacci_server  

rosrun actionlib_tutorials fibonacci_client

　　可以通过 rostopic echo /fibonacci/feedback 和 rostopic echo /fibonacci/result 命令查看斐波那契数列计算的反馈和结果：

　　最终的计算结果result如下：

复制代码

---
header: 
  seq: 1
  stamp: 1250813759950015000
  frame_id: 
status: 
  goal_id: 
    stamp: 1250813739949752000
    id: 1250813739949752000
  status: 3
  text: 
result: 
  sequence: (0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610, 987, 1597, 2584, 4181, 6765, 10946)

复制代码

　　在一个新终端中输入下面命令，运行另一个客户端节点（重命名为client2）。如果在原fibonacci_client节点的请求还没完成时就运行client2，那么服务将会被client2抢占。

rosrun actionlib_tutorials fibonacci_client __name:=client2

　　效果如下图所示，如果前一个client发送的请求还没计算完成，新的goal就到达，server会重新开始计算数列：

　　SimpleActionServer在ActionServer类上实现了single goal policy，就是在某一时刻只能有一个goal是处于active状态，并且新的goal可以抢占先前的goal：

Only one goal can have an active status at a time
New goals preempt previous goals based on the stamp in their GoalID field (later goals preempt earlier ones)
An explicit preempt goal preempts all goals with timestamps that are less than or equal to the stamp associated with the preempt
Accepting a new goal implies successful preemption of any old goal and the status of the old goal will be changed automatically to reflect this