Workspace Setup
cd %YOUR_CATKIN_WORKSPACE%/src
catkin_create_pkg actionlib_tutorials actionlib message_generation roscpp rospy std_msgs actionlib_msgs
cd ..
catkin_make
Beginner Tutorials
This example action server generates a Fibonacci sequence, the goal is the order of the sequence, the feedback is the sequence as it is computed, and the result is the final sequence.
Writing a Simple Action Server using the Execute Callback
Creating the Action Messages
Creat …/actionlib_tutorials/action/Fibonacci.action
#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence
Edit …/actionlib_tutorials/CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(actionlib_tutorials)
find_package(catkin REQUIRED COMPONENTS roscpp actionlib actionlib_msgs)
find_package(Boost REQUIRED COMPONENTS system)
add_action_files(
DIRECTORY action
FILES Fibonacci.action
)
generate_messages(
DEPENDENCIES actionlib_msgs std_msgs
)
catkin_package(
CATKIN_DEPENDS actionlib_msgs
)
catkin_make
Writing a Simple Server
Create …/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 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;
}
Compiling
Append …/actionlib_tutorials/CMakeLists.txt
...
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
add_executable(fibonacci_server src/fibonacci_server.cpp)
target_link_libraries(
fibonacci_server
${catkin_LIBRARIES}
)
add_dependencies(
fibonacci_server
${actionlib_tutorials_EXPORTED_TARGETS}
)
catkin_make
Running the Action server
roscore
rosrun actionlib_tutorials fibonacci_server
Writing a Simple Action Client
The Code
Create …/actionlib_tutorials/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");
// 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.order = 20;
ac.sendGoal(goal);
//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;
}
Compiling
Append …/actionlib_tutorials/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}
)
catkin_make
Running the Action client
rosrun actionlib_tutorials fibonacci_client
Running an Action Client and Server
Viewing the Action Feedback
rostopic echo /fibonacci/feedback
Viewing the Action Result
rostopic echo /fibonacci/result
Intermediate Tutorials
The action server averages data from a ros node, the goal is the number of samples to average, the feedback is the sample number, the sample data, the current average, and current standard deviation, and the result is the average and standard deviation of the requested number of samples.
Writing a Simple Action Server using the Goal Callback Method
Creating the Action Messages
Create …/actionlib_tutorials/action/Averaging.action
#goal definition
int32 samples
---
#result definition
float32 mean
float32 std_dev
---
#feedback
int32 sample
float32 data
float32 mean
float32 std_dev
Append …/actionlib_tutorials/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
Writing a Simple Server
The Code
Create …/actionlib_tutorials/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),
action_name_(name)
{
//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&AveragingAction::goalCB, this));
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()
{
// 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
if (!as_.isActive())
return;
data_count_++;
feedback_.sample = data_count_;
feedback_.data = msg->data;
//compute the std_dev and mean of the data
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());
//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");
AveragingAction averaging(ros::this_node::getName());
ros::spin();
return 0;
}
Compiling
Append …/actionlib_tutorials/CMakeLists.txt
...
add_executable(averaging_server src/averaging_server.cpp)
target_link_libraries(averaging_server ${catkin_LIBRARIES})
catkin_make
Writing a Threaded Simple Action Client
The Code
Create …/actionlib_tutorials/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");
// create the action client
actionlib::SimpleActionClient<actionlib_tutorials::AveragingAction> ac("averaging");
boost::thread spin_thread(&spinThread);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer();
ROS_INFO("Action server started, sending goal.");
// send a goal to the action
actionlib_tutorials::AveragingGoal goal;
goal.samples = 100;
ac.sendGoal(goal);
//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.");
// shutdown the node and join the thread back before exiting
ros::shutdown();
spin_thread.join();
//exit
return 0;
}
Compiling
Append …/actionlib_tutorials/CMakeLists.txt
...
add_executable(averaging_client src/averaging_client.cpp)
target_link_libraries(averaging_client ${catkin_LIBRARIES})
catkin_make
Running an Action Server and Client with Other Nodes
Writing the Data Node
Create …/actionlib_tutorials/scripts/gen_numbers.py
#!/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"
chmod +x gen_numbers.py
Start the Data Node
roscore
rosrun actionlib_tutorials gen_numbers.py
rosrun actionlib_tutorials averaging_server
rosrun actionlib_tutorials averaging_client
Viewing the Action Feedback
rostopic echo /averaging/feedback
Viewing the Action Result
rostopic echo /averaging/result
Advanced Tutorials
Writing a Callback Based SimpleActionClient
Example
#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; }
But I want to use Classes!
#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; }