ROS actionlib-----应用实战（by goal-result-feedback callback）

于 2016-11-09 23:46:52 发布
阅读量3.2k 收藏 6
点赞数 2
分类专栏： ROS
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/x_r_su/article/details/53100128
版权


1 创建workspace

  首先创建名为actionlib_ws的workspace。然后执行如下命令创建package。

  catkin_create_pkg my_actionlib actionlib message_generation roscpp rospy std_msgs actionlib_msgs



2 创建Action Specification

  在package目录（my_actionlib）下创建Averaging.action文件，内容包括goal定义, result定义和feedback定义，它们之间用符号(---)隔离。 Averaging.action文件看上去与.srv文件非常相似。message将基于该action文件自动生成。

  

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

如果要手动基于该action文件创建message，请执行如下命令：

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

如果要在编译时自动生成message，请在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)


3 实现一个node产生数据

#!/usr/bin/env python

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"

Server订阅该node发布的topic，然后进行处理。


4 实现server端code

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <actionlib/server/simple_action_server.h> <span style="color:#3366FF;">//实现简单action需要的action库</span>
#include <my_actionlib/AveragingAction.h> <span style="color:#3366FF;">//这里是基于Averaging.action自动生成的action message，它基于<span class="http">AveragingAction.msg自动</span>创建
</span>
class AveragingAction
{
public:
  <span style="color:#3366FF;">//构造函数中创建action server，action server参数包括node handle,action name,optionally executeCB function（这里没有使用）</span>
  AveragingAction(std::string name) : 
    as_(nh_, name, false),
    action_name_(name)
  {
    <span style="color:#3366FF;">//register the goal and feeback callbacks</span>
    as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&AveragingAction::preemptCB, this));

    <span style="color:#3366FF;">//subscribe to the data topic of interest</span>
    sub_ = nh_.subscribe("/random_number", 1, &AveragingAction::analysisCB, this);
    as_.start();
  }

  ~AveragingAction(void)
  {
  }

  void goalCB()
  {
    <span style="color:#3366FF;">// reset helper variables</span>
    data_count_ = 0;
    sum_ = 0;
    sum_sq_ = 0;
    <span style="color:#3366FF;">// accept the new goal</span>
    goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    <span style="color:#3366FF;">// set the action state to preempted</span>
    as_.setPreempted();
  }

  void analysisCB(const std_msgs::Float32::ConstPtr& msg)
  {
    <span style="color:#3366FF;">//make sure that the action hasn't been canceled</span>
    if (!as_.isActive())
      return;
    
    data_count_++;
    feedback_.sample = data_count_;
    feedback_.data = msg->data;
    <span style="color:#3366FF;">//compute the std_dev and mean of the data </span>
    sum_ += msg->data;
    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_);

    if(data_count_ > goal_) 
    {
      result_.mean = feedback_.mean;
      result_.std_dev = feedback_.std_dev;

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

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

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

  AveragingAction averaging(ros::this_node::getName());
  ros::spin();

  return 0;
}


5 实现client端

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


typedef actionlib::SimpleActionClient<my_actionlib::AveragingAction> Client;

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

class MyActionClient{

public:
  MyActionClient(const std::string client_name, bool flag) : ac(client_name, flag)
  {
    
  }

  void start()
  {


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


	// send a goal to the action
	my_actionlib::AveragingGoal goal;
	goal.samples = 100;
	ac.sendGoal(goal);

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

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

  }

  void doneCb(const actionlib::SimpleClientGoalState& state,
              const my_actionlib::AveragingResultConstPtr& result)
  {
    ROS_INFO("Finished in state [%s]", state.toString().c_str());
    ROS_INFO("Result mean: %f", result->mean);
    ROS_INFO("Result std_dev: %f", result->std_dev);
  }	


	
private:
	Client ac;

};

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

  // create the action client,should be ture otherwise not work
  MyActionClient myac("averaging", true);

  //start client
  myac.start();

  // shutdown the node and join the thread back before exiting
  ros::shutdown();


  //exit
  return 0;
}

6 客制化自动创建的CMakeList.txt 

cmake_minimum_required(VERSION 2.8.3)
project(my_actionlib)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
<span style="color:#3366FF;">find_package(catkin REQUIRED COMPONENTS
  actionlib
  actionlib_msgs
  message_generation
  roscpp
  rospy
  std_msgs
)</span>

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
<span style="color:#3366FF;">add_action_files(
  DIRECTORY action
  FILES Averaging.action
)</span>
## Generate added messages and services with any dependencies listed here
<span style="color:#3366FF;">generate_messages(
  DEPENDENCIES
  actionlib_msgs std_msgs
)</span>

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES my_actionlib
#  CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(my_actionlib
#   src/${PROJECT_NAME}/my_actionlib.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(my_actionlib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
<span style="color:#3366FF;">add_executable(averaging_server src/averaging_server.cpp)
add_executable(averaging_client src/averaging_client.cpp)</span>

## Add cmake target dependencies of the executable
## same as for the library above
<span style="color:#3366FF;">add_dependencies(averaging_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(averaging_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})</span>

## Specify libraries to link a library or executable target against
<span style="color:#3366FF;">target_link_libraries(
	averaging_server
   ${catkin_LIBRARIES}
)

target_link_libraries(
	averaging_client
   ${catkin_LIBRARIES}
)</span>

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables and/or libraries for installation
# install(TARGETS my_actionlib my_actionlib_node
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_actionlib.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

7 rqt_graph如下所示


注意：以上代码部分基于参考ROS Wiki并做了定制化。



倔强不倒翁
关注
  • 2
    点赞
  • 踩
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值