ROS moveit plan in callback function blocking

[jacknlliu]

moveit using the same callback queue with the main thread, this makes blocking when it is in a callback function, because the new callback from the moveit can't enter when the current callback function does not exit.

Solution: using a separated callback queue for service inner callback. We can use multiple threads async spinner with the number of threads larger than 1, or using a specific callback queue for this service and another callback queue for others(can also used for the service inner callback event).

Method 1

Add async spinner with N threads where N > 1 in main function

ros::AsyncSpinner async_spinner(2);
async_spinner.start();

Method 2

We need corresponding option object to set callback queue for subscribe(), advertise(), advertiseService(), etc. For example, option object ros::AdvertiseServiceOptions for advertiseService .

NOTE: We also should add an another spinner for other callback queues, or callback functions in the main process can't work!

Using service callback In main function

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/advertise_service_options.h>

   // define a new callback queue for service callback
    ros::CallbackQueue connect_srv_cb_queue;

    ros::AdvertiseServiceOptions ops = 
    ros::AdvertiseServiceOptions::create<iiwa_lfd_tools::ConnectState>(robot_connect_state_srv_name, 
                                        cb_function, ros::VoidPtr(),
                                        &connect_srv_cb_queue);

    ros::ServiceServer robot_connect_state_srv = nh.advertiseService(ops);

    ros::AsyncSpinner async_spinner(2, &connect_srv_cb_queue);
    async_spinner.start();

    ros::AsyncSpinner async_spinner1(1);
    async_spinner1.start();

Using service callback In a Class:

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/advertise_service_options.h>

class CommandRobotServer
{
    ros::AsyncSpinner async_spinner_;

    // define callback queue
    ros::CallbackQueue connect_srv_cb_queue_;

        ros::NodeHandle nh_;
}

CommandRobotServer::CommandRobotServer(ros::NodeHandle &nh, std::string connect_state_srv_name): nh_(nh),
                                        robot_connect_state_srv_name_(connect_state_srv_name),
                                        async_spinner_(2, &this->connect_srv_cb_queue_)
{
    ros::AdvertiseServiceOptions ops = ros::AdvertiseServiceOptions::create<iiwa_lfd_tools::ConnectState>(robot_connect_state_srv_name_, 
                                        boost::bind(&CommandRobotServer::get_connect_state,this, _1, _2), ros::VoidPtr(),
                                        &this->connect_srv_cb_queue_);

    robot_connect_state_srv_ = nh_.advertiseService(ops);

    async_spinner_.start();
}

Reference

• MoveGroup API gets stuck in plan(...)
• Using Different Callback Queues - ROS wiki
• multiple callback queues
• MoveIt! planning not continue after successful planning?
• ros::NodeHandle::advertiseService
• AdvertiseServiceOptions
• advertiseService
• multi_thread_node
• CommanderROS
• client callback_queue

[jacknlliu]

There are inner callback queues in moveit, and it uses the global callback queue. When calling the service server, the callback queue can't response to the coming moveit inner callback event(current callback queue is busy because current callback is not exit), so it's blocking. If assign more spinner, when main thread callback queue is busy, another spinner thread will process other things.