Timer callback function do not working when action server be called? [closed]

edit

Timer callback function do not working when action server be called?

Hello, I find a interested problem just now. It was action server callback function may cause timer callback function do not work.

The reason why I put action-server & timer in same class is that it will be convenient to communicate between Arduino&Moveit. So timer callback function is used for reading & writing data, and action server callback function is used for getting way points. Here is a part of my code.

#ifndef LEAFROBOT_H
#define LEAFROBOT_H
#include "leaf_driver/ManipulatorProtocol.h"

class LeafRobot : public hardware_interface::RobotHW
{
public:
    LeafRobot(),
        as_(nh_, "leaf_controller/follow_joint_trajectory", 
           boost::bind(&LeafRobot::executeCB, this, _1), 
           false)
    {
       .......
       clock = 0.02;    //0.02s
       timer = nh_.createTimer(ros::Duration(clock), &LeafRobot::timerCallback, this);
       ......
       as_.start();    //action server start
       ......
    };

    virtual ~LeafRobot()
    {
       ......
    };

    bool read()
    {
       //read data and save in readList
       ......
    };

    int write()
    {
       //write data from writeList
       ......
    };

    void timerCallback(const ros::TimerEvent& e)
    {
       //Update readList and write data to Arduino
       cout << "TimerCallback\n";
       write();
       if(read())
       {
           return;
       }
       usleep(1000);
       if(read())
       {
           return;
       }
    };

    void executeCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_)
    {
       //Action server callback function
        cout << "executeCB\n";
        actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal = *(goal_.getGoal());
        goal_handle_ = goal_;
        addWaypoints(goal.trajectory);
        trackMove();

        goal_handle_.setAccepted();    
        result_.error_code = result_.SUCCESSFUL;
        goal_handle_.setSucceeded(result_);
    };

    void trackMove()
    {
       //Every points will load to writeList(a point). 
       //And If readList(a point) is equal writeList(a point), 
       //the function will load next point to writeList.
       ......
    };
    ......
    ......
private:
    ......
    ......
    ros::NodeHandle nh_;
    ros::Time time_current, time_prev;
    ros::Timer timer;
    double clock;
    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>  as_;    ![image description](http://)
    control_msgs::FollowJointTrajectoryResult result_; 
    ......
    ......
};
#endif //LEAFROBOT

The main function just have a LeafRobot Object and other simple code.

When it run, terminal just like this.

TimerCallback
TimerCallback
TimerCallback
executeCB
TrackMoving 
Arrive to 0 th point : 
Arrive to 1 th point : 
Ignore 1 th point : 
Arrive to 2 th point : 
Ignore 2 th point : 
Arrive to 3 th point : 
Ignore 3 th point : 
Arrive to 4 th point :

After TrackMoving display , it always display Arrive to X th point : or Ignore X th point : . It means, timerCallback didn't be called when executeCB running. Maybe I need to use timer in C++, or don't use ROS timer and update readList&writeList in main function , through it could not very accurate.

Welcome to take some advise to me.That all, Thanks for your attention.