ROS-5-actionlib

最新推荐文章于 2022-12-12 22:45:58 发布

Summer_star_summer

最新推荐文章于 2022-12-12 22:45:58 发布

阅读量309

点赞数

本文链接：https://blog.csdn.net/Summer_star_summer/article/details/83795348

版权

Ros系统中，存在一些需要较长时间执行的任务，有时候用户希望获取任务执行的状态或者可以取消本次任务执行新任务，即任务抢占等，基于这类情况，ros 设计了actionlib package。
1.创建一个简单的Action server
Action Messages是从.action 文件中产生的，这个文件定义了goal,result,feedback这三个topic发布出的消息的类型和格式。这个action server产生一个斐波那契序列，goal:order of the sequence; feedback: the sequence as it is computed; result:final sequence
1.1首先创建一个actionlib_tutorials文件夹，输入如下代码：

$ cd %YOUR_CATKIN_WORKSPACE%/src
$ catkin_create_pkg actionlib_tutorials actionlib message_generation roscpp rospy std_msgs actionlib_msgs

之后创建actionlib_tutorials/action/Fibonacci.action文件，粘贴以下代码：

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

之后在CMakeLists.txt文件中添加以下代码：

add_action_files(
  DIRECTORY action
  FILES Fibonacci.action
)

generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs  # Orother packages containing msgs
)

catkin_package(
  CATKIN_DEPENDS actionlib_msgs  #仅需指定一个actionlib_msgs,对message_runtime的依赖是自动发生的
)

在package.xml文件中添加以下代码：

<exec_depend>message_generation</exec_depend>

之后进行编译，可以看到在工作空间的devel下面可以看到生成的msg文件以及server所需要的.h头文件

$ 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

1.2 编写server
创建 actionlib_tutorials/src/fibonacci_server.cpp文件，并粘贴以下代码：

#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 rqinstance 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) :        //建立一个有名字的Action类
    as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false), //建立一个server,使用参数为，nodeHandle,name,executeCB函数，false
    action_name_(name)  //给定action的名字，就可以开始运行server
  {
    as_.start();
  }

  ~FibonacciAction(void)
  {
  }

  void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)  //添加ConstPtr表示是boost shared pointer
  {
    // helper variables
    ros::Rate r(1);
    bool success = true;

    // push_back the seeds for the fibonacci sequence
    //设定feedback.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();  //中断当前current goal
        success = false;
        break;
      }
      //在没有其他client 来preempt的时候，运行server
      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;
}

1.3 将package进行编译
在CMakeLists.txt添加如下代码

add_executable(fibonacci_server src/fibonacci_server.cpp)

target_link_libraries(
  fibonacci_server
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_server
  ${actionlib_tutorials_EXPORTED_TARGETS}
)

1.4 运行server
输入以下命令：

$ roscore
$ rosrun actionlib_tutorials fibonacci_server

通过以下命令来查看话题：

$ rostopic list -v
$ rqt_graph

1.5 编写client
创建一个action client，并且发送一个goal到action server。
首先创建src/fibonacci_client.cpp文件，粘贴如下代码：

#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");
//创建client
  // create the action client
  // true causes the client to spin its own thread
  actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  // wait for the 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,计算20个累加的order
  goal.order = 20;
  ac.sendGoal(goal);  //发送该goal到server

  //wait for the action to return
  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.");

  //exit
  return 0;
}

在CMakeLists.txt文件添加以下代码：

add_executable(fibonacci_client src/fibonacci_client.cpp)

target_link_libraries( 
  fibonacci_client
  ${catkin_LIBRARIES}
)

add_dependencies(
  fibonacci_client
  ${actionlib_tutorials_EXPORTED_TARGETS}
)

之后编译完成即可运行：

$ rosrun actionlib_tutorials fibonacci_client

使用rxgraph来查看节点通讯状况：

$ rxgraph

$ rostopic echo /fibonacci/feedback

$ rostopic echo /fibonacci/result

2.使用Goal Callback 方法来创建simple action server
我们创建一个求平均数的action server, 通过使用action来处理从ros nodes中传递的数据，goal: 要进行平均值计算的sample的数量; feedback: sample的数量，sample data，当前的平均值和当前的标准差； result:所请求sample数的平均值和标准偏差
2.1 与之前一样，首先创建action/Averaging.action文件，确定goal,result,feedback这三个话题传输的信息的种类和格式，粘贴以下代码：

#goal definition
int32 samples
---
#result definition
float32 mean
float32 std_dev
---
#feedback
int32 sample
float32 data
float32 mean
float32 std_dev

之后输入以下代码生成头文件：

$ roscd actionlib_tutorials
$ rosrun actionlib_msgs genaction.py -o msg/ action/Averaging.action

在CMakeLists.txt文件中粘贴以下代码：

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

之后进行catkin_make初始化环境
2.2 编写server
在src/averaging_server.cpp文件中粘贴以下代码：

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/AveragingAction.h>

class AveragingAction
{
public:
    
  AveragingAction(std::string name) : 
    as_(nh_, name, false),  //建立simple action server
    action_name_(name)     
  {
    //register the goal and feeback callbacks 创建完action server之后，
    as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));注册goalcb,preempcb两个函数
    as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));

    //subscribe to the data topic of interest
    sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
    as_.start();//服务启动
  }

  ~AveragingAction(void)
  {
  }

  void goalCB()  //当该函数被调用时，action将会接收新的样本和数据
  {
    // reset helper variables
    data_count_ = 0;
    sum_ = 0;
    sum_sq_ = 0;
    // accept the new goal
    goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }

  void analysisCB(const std_msgs::Float32::ConstPtr& msg) //引用订阅的话题的信息类型
  {
    // make sure that the action hasn't been canceled   在继续处理数据之前检查action是否处在active状态
    if (!as_.isActive())
      return;
    
    data_count_++;
    feedback_.sample = data_count_;  //存储sample的数量
    feedback_.data = msg->data;  //将每个sample的数据存放在feedback中
    //compute the std_dev and mean of the data 
    sum_ += msg->data;   //计算所有sample数据之和
    feedback_.mean = sum_ / data_count_; //计算样本的平均数
    sum_sq_ += pow(msg->data, 2); //计算数据的平方之和
    feedback_.std_dev = sqrt(fabs((sum_sq_/data_count_) - pow(feedback_.mean, 2)));//计算标准偏差
    as_.publishFeedback(feedback_); //发布在feedback话题上

    if(data_count_ > goal_)   //一旦数据计算完毕，将feedback中的数据传递到result中
    {
      result_.mean = feedback_.mean;
      result_.std_dev = feedback_.std_dev;

      if(result_.mean < 5.0)
      {
        ROS_INFO("%s: Aborted", action_name_.c_str());
        //set the action state to aborted
        as_.setAborted(result_);
      }
      else 
      {
        ROS_INFO("%s: Succeeded", action_name_.c_str());
        // set the action state to succeeded
        as_.setSucceeded(result_);
      }
    } 
  }

protected:
    
  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<actionlib_tutorials::AveragingAction> as_;
  std::string action_name_;
  int data_count_, goal_;
  float sum_, sum_sq_;
  actionlib_tutorials::AveragingFeedback feedback_;
  actionlib_tutorials::AveragingResult result_;
  ros::Subscriber sub_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "averaging");//初始化server的名称为averaging
  AveragingAction averaging(ros::this_node::getName());
  ros::spin();

  return 0;
}

之后在CMakeLists.txt文件中添加以下代码：

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

编译完成之后，输入以下代码：

$ roscore

$ rosrun actionlib_tutorials averaging_server

3.编写Client
创建/src/averaging_client.cpp文件，粘贴以下代码：

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <actionlib_tutorials/AveragingAction.h>
#include <boost/thread.hpp>

void spinThread()
{
  ros::spin();
}

int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_averaging");//初始化客户端名称为test_averaging

  // create the action client
  actionlib::SimpleActionClient<actionlib_tutorials::AveragingAction> ac("averaging");   //建立简单的客户端ac，并与服务averaging连接，还可在后面添加bool类型来选择是否自动spin thread,默认是false,即必须创建一个thread
  boost::thread spin_thread(&spinThread);//创建一个thread,开始在后台spin ros node。通过这种方法可以为客户端创建多个threads

  ROS_INFO("Waiting for action server to start.");
  ac.waitForServer();  //确认server已经开始运行

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  actionlib_tutorials::AveragingGoal goal;//创建一个goal
  goal.samples = 100;
  ac.sendGoal(goal);

  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));//设置等待时间为30s，如果过了这个时间goal仍然没有完成就会设置为false

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

  // shutdown the node and join the thread back before exiting
  ros::shutdown();//关闭当前node
  spin_thread.join();//加入新的thread

  //exit
  return 0;
}

之后在CMakeLists.txt文件中粘贴以下代码：

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

编译之后运行以下代码：

$ roscore

$ rosrun actionlib_tutorials averaging_client

$ rosrun rqt_graph rqt_graph &

将其他node和创建的client,server一起运行
4.1 创建data node
首先创建actionlib_tutorials/scripts/gen_numbers.py文件，粘贴以下代码：

import rospy
from std_msgs.msg import Float32
import random
def gen_number():
    pub = rospy.Publisher('random_number', Float32)
    rospy.init_node('random_number_generator', log_level=rospy.INFO)
    rospy.loginfo("Generating random numbers")

    while not rospy.is_shutdown():
        pub.publish(Float32(random.normalvariate(5, 1)))
        rospy.sleep(0.05)

if __name__ == '__main__':
  try:
    gen_number()
  except Exception, e:
    print "done"

上面的代码生成随机数，其正态分布以5为中心，标准差为1，并在/ random_number主题上发布数字。之后，使该node能够执行：

chmod +x gen_numbers.py

4.2 运行Node,client,server
首先运行node

$ roscore

rosrun actionlib_tutorials gen_numbers.py

然后查看feedback 和 result话题

$ rostopic echo /averaging/feedback

$ rostopic echo /averaging/result

$ rosrun rqt_graph rqt_graph &

5.基于simple action client编写call back函数
有时我们希望在执行action的中间停止执行，这时我们就可以不适用waitForResult函数而使用call back函数来实现这一功能src/fibonacci_callback_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;

// 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;
}

5.1 当尝试注册class method的时候，register goal,active,feedback这些cb并不是很方便，通过使用较为复杂语法的 boost::bind,可以来使用类创建action client,代码如下：

#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;

class MyNode
{
public:
  MyNode() : ac("fibonacci", true)
  {
    ROS_INFO("Waiting for action server to start.");
    ac.waitForServer();
    ROS_INFO("Action server started, sending goal.");
  }

  void doStuff(int order)
  {
    FibonacciGoal goal;
    goal.order = order;

    // Need boost::bind to pass in the 'this' pointer
    ac.sendGoal(goal,
                boost::bind(&MyNode::doneCb, this, _1, _2),
                Client::SimpleActiveCallback(),
                Client::SimpleFeedbackCallback());

  }

  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();
  }

private:
  Client ac;
};

int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_fibonacci_class_client");
  MyNode my_node;
  my_node.doStuff(10);
  ros::spin();
  return 0;
}

我们没有注册active或feedback，所以我们改为传递空函数指针。我们也可以将这两个参数留空，因为默认情况下callback为空。