This article is written while I implement sample code of material the "mastering ros for robotics programming 3rd edition".
But the action client code is quite different from the material since it didn`t use class on it.
0. Generate a package and source files and action definition file.
I have generated a package its name is "mastering_ros_demo_pkg"
Then, I have two plain cpp files in the src directory: demo_service_server.cpp and demo_service_client.cpp
Then, I have generated Demo_action.action in the action directory with definition of the action as below.
1. Edit package.xml and CMakelist.txt
package.xml: snippet of dependencies is given here.
CMakelist.txt: snippet of dependencies and building are given here.
2. Generate action header file.
To activate auto completion of VS code, generate a header file of the action.
Edit demo_action_server.cpp like this :
#include <iostream>
#include "ros/ros.h"
#include <actionlib/server/simple_action_server.h>
#include "mastering_ros_demo_pkg/Demo_actionAction.h"
int main(int argc, char** argv){
return 0;
}and build it.
3. Edit source code : Action server
Edit the cpp files as I attached in this article.
First, define the wrapper class of action server.
Definition action server object needs its node handler and name of the topic to advertise(It is given when the constructor has called).
Also, the wrapper class of action server should have action server object(which is oriented from actionlib) and a result object
class Demo_actionAction
{
protected:
/*What is name of the action?*/
std::string action_name;
int progress;
ros::Rate cycle_freq;
/*Declare the action server*/
actionlib::SimpleActionServer<mastering_ros_demo_pkg::Demo_actionAction> as_;
mastering_ros_demo_pkg::Demo_actionFeedback fb_;
mastering_ros_demo_pkg::Demo_actionResult result_;
void ExecCallback(const mastering_ros_demo_pkg::Demo_actionGoalConstPtr& _goal){
/*To do when it got goal to achieve*/
}
void PreemptCallback(){
/*To do when the process is preempted(or cancelled)*/
}
public:
Demo_actionAction(const ros::NodeHandle& _nh, const std::string& _action_name, const unsigned int& _cycle_freq = 5){
/*To do such as initialization.*/
}
}In here, I gave additional option to process frequency as argument of the constructor.
Initialization list and initialization in the constructor makes some headache.
First, actionlib::SimpleActionServer<ACTION_TYPE> needs node handler, topic name to advertise, callback function object to execute and auto-start option.
To make callback function object, it needs boost::bind and boost::function.
Demo_actionAction(const ros::NodeHandle& _nh, const std::string& _action_name, const unsigned int& _cycle_freq = 5)
/*Register execution callback fnc. with initialization list.*/
:as_(_nh, _action_name, boost::bind(&Demo_actionAction::ExecCallback, this, _1), false)
/*Set name and cycle frequency of this action.*/
,action_name(_action_name), cycle_freq(_cycle_freq)
{
/*Register Preempt callback fnc..*/
as_.registerPreemptCallback(boost::bind(&Demo_actionAction::PreemptCallback, this));
}Execution callback function is called when the action server receives a goal to achieve. In this case, this action server performs count time with using internal integer: progress.
Optionally, it publishes feedback message.
In addition, I made a function to progress the action and checker function whether the action is complete.
void ExecCallback(const mastering_ros_demo_pkg::Demo_actionGoalConstPtr& _goal){
const auto goal_tmp = _goal->count;
if( (as_.isActive() == false) && (as_.isPreemptRequested() == true) ) return; /*Reject when it is not activated or preempted*/
ROS_INFO("This action trying to reach goal %d", goal_tmp);
progress = 0;
/*Iterates until the job is complete*/
while(!isJobCplt(goal_tmp)){
/*When the user input Ctrl + C to shutdown this node*/
if(!ros::ok()){
/*Report progress to final count and set this action as aborted.*/
result_.final_count = progress;
as_.setAborted(result_, "I failed!");
ROS_INFO("%s is shutted down.", action_name.c_str());
break;
}
/*Reject when it is not activated or preempted*/
if( (as_.isActive() == false) && (as_.isPreemptRequested() == true) ) return;
/*Do job something*/
job();
/*Is the job complete?*/
if(isJobCplt(goal_tmp)){
/*Report progress to final count and set this action as succeeded.*/
result_.final_count = progress;
as_.setSucceeded(result_, "I succeeded!");
ROS_INFO("%s is reached the goal %d.", action_name.c_str(), goal_tmp);
}
else{
/*Set the feedback and publish a feedback.*/
fb_.current_number = progress;
as_.publishFeedback(fb_);
ROS_INFO("%s is %d of the goal %d.", action_name.c_str(), progress, goal_tmp);
}
cycle_freq.sleep();
}
}
inline void job(){++progress;}
inline bool isJobCplt(const std::int32_t& _goal){ return static_cast<bool>(progress >= _goal);}For when the job is preempted, make a callback function and register it to the action server object.
It makes the goal preempted status.
class Demo_actionAction
{
protected:
void PreemptCallback(){
/*Report progress to final count and set this action as preempted.*/
result_.final_count = progress;
as_.setPreempted(result_,"I got Preempted");
ROS_WARN("%s got preempted!", action_name.c_str());
}
public:
Demo_actionAction(const ros::NodeHandle& _nh, const std::string& _action_name, const unsigned int& _cycle_freq = 5)
/*Register execution callback fnc. with initialization list.*/
:as_(_nh, _action_name, boost::bind(&Demo_actionAction::ExecCallback, this, _1), false)
/*Set name and cycle frequency of this action.*/
,action_name(_action_name), cycle_freq(_cycle_freq)
{
/*Register Preempt callback fnc..*/
as_.registerPreemptCallback(boost::bind(&Demo_actionAction::PreemptCallback, this));
}
};Finally, since I made it auto-start option false, a function to start the action server is needed.
void StartAction(){as_.start();}Total structure of the wrapper class and main function to generate and run the object is given as below:
#include <iostream>
#include "ros/ros.h"
#include <actionlib/server/simple_action_server.h>
#include "mastering_ros_demo_pkg/Demo_actionAction.h"
class Demo_actionAction
{
protected:
/*What is name of the action?*/
std::string action_name;
int progress;
ros::Rate cycle_freq;
/*Declare the action server*/
actionlib::SimpleActionServer<mastering_ros_demo_pkg::Demo_actionAction> as_;
mastering_ros_demo_pkg::Demo_actionFeedback fb_;
mastering_ros_demo_pkg::Demo_actionResult result_;
/*Callback fncs. for exec, preempt*/
void ExecCallback(const mastering_ros_demo_pkg::Demo_actionGoalConstPtr& _goal){
const auto goal_tmp = _goal->count;
if( (as_.isActive() == false) && (as_.isPreemptRequested() == true) ) return; /*Reject when it is not activated or preempted*/
ROS_INFO("This action trying to reach goal %d", goal_tmp);
progress = 0;
/*Iterates until the job is complete*/
while(!isJobCplt(goal_tmp)){
/*When the user input Ctrl + C to shutdown this node*/
if(!ros::ok()){
/*Report progress to final count and set this action as aborted.*/
result_.final_count = progress;
as_.setAborted(result_, "I failed!");
ROS_INFO("%s is shutted down.", action_name.c_str());
break;
}
/*Reject when it is not activated or preempted*/
if( (as_.isActive() == false) && (as_.isPreemptRequested() == true) ) return;
/*Do job something*/
job();
/*Is the job complete?*/
if(isJobCplt(goal_tmp)){
/*Report progress to final count and set this action as succeeded.*/
result_.final_count = progress;
as_.setSucceeded(result_, "I succeeded!");
ROS_INFO("%s is reached the goal %d.", action_name.c_str(), goal_tmp);
}
else{
/*Set the feedback and publish a feedback.*/
fb_.current_number = progress;
as_.publishFeedback(fb_);
ROS_INFO("%s is %d of the goal %d.", action_name.c_str(), progress, goal_tmp);
}
cycle_freq.sleep();
}
}
void PreemptCallback(){
/*Report progress to final count and set this action as preempted.*/
result_.final_count = progress;
as_.setPreempted(result_,"I got Preempted");
ROS_WARN("%s got preempted!", action_name.c_str());
}
inline void job(){++progress;}
inline bool isJobCplt(const std::int32_t& _goal){ return static_cast<bool>(progress >= _goal);}
public:
Demo_actionAction(const ros::NodeHandle& _nh, const std::string& _action_name, const unsigned int& _cycle_freq = 5)
/*Register execution callback fnc. with initialization list.*/
:as_(_nh, _action_name, boost::bind(&Demo_actionAction::ExecCallback, this, _1), false)
/*Set name and cycle frequency of this action.*/
,action_name(_action_name), cycle_freq(_cycle_freq)
{
/*Register Preempt callback fnc..*/
as_.registerPreemptCallback(boost::bind(&Demo_actionAction::PreemptCallback, this));
}
void StartAction(){as_.start();}
};
int main(int argc, char** argv){
ros::init(argc, argv, "Demo_action_server");
ros::NodeHandle nh_;
ROS_INFO("Starting Demo Action Server");
auto myaction = new Demo_actionAction(nh_, "Demo_action", 100);
myaction->StartAction();
while(ros::ok()){
ros::spinOnce();
}
return 0;
}
4. Edit source code : Action Client
The action client publishes a goal to achieve and receive its result.
So, wrapper class of the action client needs node handler and action client(oriented from actionlib) object and goal object.
class MyActionClient{
protected:
/*Actual action client*/
actionlib::SimpleActionClient<mastering_ros_demo_pkg::Demo_actionAction> ac_;
/*Goal input*/
mastering_ros_demo_pkg::Demo_actionGoal goal_;
public:
MyActionClient(ros::NodeHandle& _nh, const std::string& _to_subscribe = ""):
ac_(_nh, _to_subscribe,true) {
ac_.waitForServer();
};
};To send a goal to the server and react on it. It needs function to send a goal and callback functions which called when the goal is done, when the goal is activated, and when the server published a feedback.
If you have not to react on it, you may skip the definitions of callback functions.
Beware on the data types of the arguments of the callback functions. Otherwise, it cause compile error hard to fix.
using GOAL_STATE = actionlib::SimpleClientGoalState::StateEnum;
class MyActionClient{
protected:
void DoneCB(const actionlib::SimpleClientGoalState &state, const mastering_ros_demo_pkg::Demo_actionResultConstPtr& _result){
/*Report the status when it finished in such cases.*/
auto state_now = ac_.getState();
if(state_now == GOAL_STATE::SUCCEEDED){
ROS_INFO("The action has finished successfully.: %d", _result->final_count);
}else if(state_now == GOAL_STATE::PREEMPTED){
ROS_INFO("The action has Preempted.");
}else{
ROS_ERROR("The action has terminated unexpectly.");
}
}
void ActiveCB(){
ROS_INFO("Now the action has activated");
}
void FeedbackCB(const mastering_ros_demo_pkg::Demo_actionFeedbackConstPtr& _feedback){
/*Report when the server published feedback.*/
ROS_INFO("Feedback: Now the state is %d", _feedback->current_number);
}
public:
void SendGoal(const std::int32_t& _goal){
goal_.count = _goal;
ROS_INFO("Sending Goal [%d]",goal_.count);
ac_.sendGoal(goal_,
boost::bind(&MyActionClient::DoneCB, this, _1, _2)
,boost::bind(&MyActionClient::ActiveCB, this)
,boost::bind(&MyActionClient::FeedbackCB, this, _1) );
}
};To cancel the goal and check its status, additional simple functions are made.
void CancelGoal(){
ac_.cancelGoal();
}
const actionlib::SimpleClientGoalState GetState() const {
auto state_now = ac_.getState();
return state_now;
}Total structure of the wrapper class and main function to generate and run the object is given as below:
#include "ros/ros.h"
#include <iostream>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include "mastering_ros_demo_pkg/Demo_actionAction.h"
/*Shorten long and dirty type definition*/
using GOAL_STATE = actionlib::SimpleClientGoalState::StateEnum;
class MyActionClient{
protected:
/*Actual action client*/
actionlib::SimpleActionClient<mastering_ros_demo_pkg::Demo_actionAction> ac_;
/*Goal input*/
mastering_ros_demo_pkg::Demo_actionGoal goal_;
void DoneCB(const actionlib::SimpleClientGoalState &state, const mastering_ros_demo_pkg::Demo_actionResultConstPtr& _result){
/*Report the status when it finished in such cases.*/
auto state_now = ac_.getState();
if(state_now == GOAL_STATE::SUCCEEDED){
ROS_INFO("The action has finished successfully.: %d", _result->final_count);
}else if(state_now == GOAL_STATE::PREEMPTED){
ROS_INFO("The action has Preempted.");
}else{
ROS_ERROR("The action has terminated unexpectly.");
}
}
void ActiveCB(){
ROS_INFO("Now the action has activated");
}
void FeedbackCB(const mastering_ros_demo_pkg::Demo_actionFeedbackConstPtr& _feedback){
/*Report when the server published feedback.*/
ROS_INFO("Feedback: Now the state is %d", _feedback->current_number);
}
public:
MyActionClient(ros::NodeHandle& _nh, const std::string& _to_subscribe = ""):
ac_(_nh, _to_subscribe,true) {
ac_.waitForServer();
};
void SendGoal(const std::int32_t& _goal){
goal_.count = _goal;
ROS_INFO("Sending Goal [%d]",goal_.count);
ac_.sendGoal(goal_,
boost::bind(&MyActionClient::DoneCB, this, _1, _2)
,boost::bind(&MyActionClient::ActiveCB, this)
,boost::bind(&MyActionClient::FeedbackCB, this, _1) );
}
void CancelGoal(){
ac_.cancelGoal();
}
const actionlib::SimpleClientGoalState GetState() const {
auto state_now = ac_.getState();
return state_now;
}
};
int main (int argc, char **argv) {
ros::init(argc, argv, "demo_action_client");
ros::NodeHandle nh_;
if(argc != 3){
ROS_INFO("%d",argc);
ROS_WARN("Usage: demo_action_client <goal> <time_to_preempt_in_sec>");
return 1;
}
/*Set a goal from argument 1*/
mastering_ros_demo_pkg::Demo_actionGoal goal;
goal.count = atoi(argv[1]);
ROS_INFO("Waiting for action server to start.");
auto myActClient = new MyActionClient(nh_, "Demo_action");
ROS_INFO("Action server started, sending goal.");
myActClient->SendGoal(atoi(argv[1]));
unsigned int lmt_count, count;
count = 0;
lmt_count = atoi(argv[2]);
ros::Rate sleep_rate(1);
while(ros::ok()){
if(++count > lmt_count){
/*Cancel the goal. Then, the client will be killed in the next sentence.*/
myActClient->CancelGoal();
}
auto GOAL_STATE = myActClient->GetState();
ROS_INFO("Goal status: %s", GOAL_STATE.toString().c_str());
if((GOAL_STATE != GOAL_STATE::ACTIVE)&&(GOAL_STATE != GOAL_STATE::PENDING))
{
delete myActClient; /*Goal is deadvertised by delete*/
break;/*E.N.D*/
}
sleep_rate.sleep();
ros::spinOnce();
}
return 0;
}
5. Check the result
I send a goals possible/impossible to achieve in the limited time(which given argument of client)
Official tutorial of action client using class is given here:
References:
mastering ros for robotics programming 3rd edition
ros.org: simple action client
https://docs.ros.org/en/diamondback/api/actionlib/html/classactionlib_1_1SimpleActionClient.html
'ROS' 카테고리의 다른 글
| ros_control API를 이용한 하드웨어 제어프로그램 작성 (0) | 2022.05.21 |
|---|---|
| URDF로봇 이동 시뮬레이션 하기 (0) | 2022.05.05 |
| 간단한 urdf모델 만들고 rviz로 보기 (0) | 2022.05.04 |
| package.xml/CMakelist.txt 각 태그 내용 정리 (0) | 2022.04.03 |
| How to make workspace and package in ros (0) | 2022.03.31 |
