ActionServer not calling trancition callback on every goal result.
Hello. I am learning to work with "non-simple" action server/client. I used tutorial that calculates Fibonacci sequence in "actionlib_tutorials" pkg and changed SimpleActionServer to ActionServer(and client).
I tried to use servers goal callback only for receive goal, and do all work in thread. GoalHandles i am storing in some container to have access them from thread (is it correct?)
I sending goal from ActionClient
and setting received goal to setAccepted
and then to setSucceesed
on ActionServer
On client side i`m expecting transition callback calls with states in this order:
PENDING
ACTIVE
WAITING_FOR_RESULT
DONE
and its working fine until i sending many(3 in example) goals. ActionServer receives them and start thread for each. Now im expecting the same transitions order on client side but 3 times. But only one random goal reaches DONE
state, another stuck on WAITING_FOR_RESULT
example:
PENDING
PENDING
PENDING
ACTIVE
ACTIVE
ACTIVE
WAITING_FOR_RESULT
WAITING_FOR_RESULT
WAITING_FOR_RESULT
DONE
I know that ActionServer and Client communicates with each other between topics. I changing goal states on server side without any delay and maybe its very quick for client subscriber and it looses some result messages? I used rqt topic monitor and created my own subscriber, but they are both receiving all 3 message.
Why its stuck only on DONE
and work fine on another states?
Where is the problem?
Thank You.
Here is the code:
Server
#include <ros/ros.h>
#include <actionlib/server/action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
#include <actionlib/server/server_goal_handle.h>
#include <iostream>
#include <string>
#include <thread>
#include <vector>
#include <algorithm>
using namespace actionlib;
using namespace std;
typedef actionlib::ActionServer<actionlib_tutorials::FibonacciAction>::GoalHandle handle;
vector<handle> vh;
int i=0;
std::thread threads[2];
//real goal handling in this thread, just accepting, and set it succeeded.
void job(actionlib::ActionServer<actionlib_tutorials::FibonacciAction>::GoalHandle goal)
{
goal.setAccepted();
goal.setSucceeded();
}
void GoalCB(actionlib::ActionServer<actionlib_tutorials::FibonacciAction>::GoalHandle goal)
{
//only starting thread here
int id=i;
i++;
vh.push_back(goal);
threads[id]=thread(job,id,vh[id]);
threads[id].detach();
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"s");
ros::NodeHandle nh_;
actionlib::ActionServer<actionlib_tutorials::FibonacciAction> as(nh_,"fibonacci",boost::bind(GoalCB,_1),false);
as.start();
ros::spin();
}
Client:
#include <ros/ros.h>
#include <actionlib/client/action_client.h>
#include <actionlib_tutorials/FibonacciAction.h>
#include <iostream>
using namespace actionlib;
using namespace actionlib_tutorials;
using namespace std;
void TranCB(actionlib::ActionClient<actionlib_tutorials::FibonacciAction>::GoalHandle goal)
{
CommState cs=goal.getCommState();
cout<<cs.toString()<<endl;
}
void sub_cb(const actionlib_tutorials::FibonacciActionResultConstPtr& msg)
{
msgs.push_back(msg);
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"c");
ros::NodeHandle nh_;
actionlib::ActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci");
ROS_INFO("Waiting for action server to start.");
ac.waitForActionServerToStart(ros::Duration(1, 0));
ROS_INFO("Action server started, sending goal.");
FibonacciGoal goal;
actionlib::ActionClient<actionlib_tutorials::FibonacciAction>::GoalHandle gh, gh2, gh3;
gh=ac.sendGoal(goal, boost::bind(TranCB,_1));//, &doneCb, &activeCb, &feedbackCb);
cout<<"sended 1 goal"<<endl;
gh2=ac.sendGoal(goal, boost::bind(TranCB,_1));//, &doneCb, &activeCb, &feedbackCb);
cout<<"sended 2 goal"<<endl;
gh3=ac.sendGoal(goal, boost::bind ...
I would see if you are running into an issue that has already been reported. See the Actionlib issue tracker.