tambel's profile - activity

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 ...