How to do it…
- Create an actionlib_tutorials package with the following dependencies or download it from the GitHub repository (https://github.com/kbipin/Robot-Operating-System-Cookbook):
$ cd <workspace>/src $ catkin_create_pkg actionlib_tutorials actionlib message_generation roscpp rospy std_msgs actionlib_msgs
- First, define the action which consists of goal, result, and feedback messages. These are generated automatically from the .action file.
- Create an actionlib_tutorials/action/Fibonacci.action file with the following contents:
#goal definition int32 order --- #result definition int32[] sequence --- #feedback int32[] sequence
- To automatically generate the message files during the creation process, the following elements need to be added to CMakeLists.txt:
find_package(catkin REQUIRED COMPONENTS
actionlib
actionlib_msgs
message_generation
roscpp
rospy
std_msgs
)
add_action_files(
DIRECTORY action
FILES Fibonacci.action
)
generate_messages(
DEPENDENCIES actionlib_msgs std_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES actionlib_tutorials
CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs
DEPENDS system_lib
)
add_executable(fibonacci_server src/fibonacci_server.cpp)
target_link_libraries(
fibonacci_server
${catkin_LIBRARIES}
)
add_dependencies(
fibonacci_server
${actionlib_tutorials_EXPORTED_TARGETS}
)
add_executable(fibonacci_client src/fibonacci_client.cpp)
target_link_libraries(
fibonacci_client
${catkin_LIBRARIES}
)
add_dependencies(
fibonacci_client
${actionlib_tutorials_EXPORTED_TARGETS}
)
The catkin_make command will automatically generate the required messages and header files, as shown in the following screenshot:
Action messages and file generation
The following code snippet of the action server in the actionlib_tutorials/src/fibonacci_server.cpp file is self-explanatory:
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
class FibonacciAction
{
protected:
ros::NodeHandle nh_;
/* NodeHandle instance must be created before this line. Otherwise strange error occurs.*/
actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_;
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)
{
ros::Rate r(1);
bool success = true;
/* the seeds for the fibonacci sequence */
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
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, however, 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 server");
FibonacciAction fibonacci("fibonacci");
ros::spin();
return 0;
}
Now, let's discuss the important parts of this code in detail:
FibonacciAction(std::string name) :
as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
action_name_(name)
{
as_.start();
}
In the constructor of the FibonacciAction class, an action server is initialized and started, which takes a node handle, the name of the action, and optionally, a callback as arguments. In this example, the action server is created with executeCB being used as a callback function:
/* 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, however, the sequence is computed at 1 Hz for demonstration purposes */
r.sleep();
}
One of the most important features of the action server is the ability to allow an action client to preempt the current goal execution. When a client requests the preemption of the current goal, the action server will cancel the goal execution, perform essential clean-up, and call the setPreempted() function, which informs the ROS framework that the action has been preempted by a client request. The rate at which the action server checks for preemption requests and provides the feedback information is implementation-dependent:
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
/* set the action state to succeeded */
as_.setSucceeded(result_);
}
Once the action server has finished executing its current goal, it informs the action client about completion by calling the setSucceeded() function. From now on, the action server is running and waiting to receive its next set of goals.
The following code snippet of the action client's actionlib_tutorials/src/fibonacci_client.cpp file is self-explanatory:
#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, "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.");
/* will be waiting for infinite time */
ac.waitForServer();
ROS_INFO("Action server started, sending goal.");
actionlib_tutorials::FibonacciGoal goal;
goal.order = 20;
ac.sendGoal(goal);
/* waiting 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 does not finish before the time out.");
return 0;
}
Now, let's discuss the key elements of the code in detail:
// 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
Here, the action client is constructed with the server name and the auto spin option set to true which takes two arguments, the action server name and a boolean option to automatically spin a thread (actionlib is used to do the 'thread magic' behind the scenes). It also specifies message types to communicate with the action server.
In the following line, the action client will wait for the action server to start before continuing:
// 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.");
Here, the action client creates the goal message and sends it to the action server. It now waits for the goal to finish before continuing. The timeout on the wait is set to 30 seconds, which means that the function will return with false if the goal has not finished and the user is notified that the goal did not finish in the allotted time. If the goal finishes before a timeout, the goal status is reported and normal execution continues.
- To run the following commands in the top level directory of our workspace to compile the package, , execute the following command:
$ catkin_make
- To set up the system environment, let's execute the following command:
$ source devel/setup.bash
After compiling the executable successfully, we will start the action client and server in a separate terminal, assuming that roscore is running:
$ rosrun actionlib_tutorials fibonacci_server
The output is shown in the following screenshot:
Action server outputs
Next, we would start the action client using following command:
$ rosrun actionlib_tutorials fibonacci_client
The output is shown in the following screenshot:
Action client outputs
- To visualize the action server and client execution graphically, execute the following command:
$ rqt_graph
The output is shown in the following screenshot:
Action client computation graph
The preceding screenshot shows that the action server is publishing the feedback, status, and result channels and is subscribed to the goal and cancel channels. Similarly, the action client is subscribing to the feedback, status, and result channels and is publishing to the goal and cancel channels. Both the server and client are up and running as expected.
To see feedback from the action server while it's acting on its goal, run the following command:
$ rostopic echo /fibonacci/feedback
The output is shown in the following screenshot:
Feedback from the action server
Similarly, in a new terminal, we can observe the feedback channel to see the result from the action server after the goal has been completed:
Results from the action server