Cannot always switch controllers

edit

Hello everyone,

I am on Ubuntu 16.04 with ROS Kinetic. Working on a robot with two different controllers: joint_trajectory_controller and velocity_controller, which is a custom controller written by me that uses Position Joint Interface.

Both the controllers are successfully loaded with two controller spawners: one with the arg--stopped. Then, I can switch between these controllers as follows.

// Creating the service client
ros::ServiceClient robot_switch_client;
robot_switch_client = nh.serviceClient<controller_manager_msgs::SwitchController>(robot_name + "/controller_manager/switch_controller");

// Creating and filling up the message
controller_manager_msgs::SwitchController switch_controller;

switch_controller.request.start_controllers.clear();
switch_controller.request.stop_controllers.clear();

switch_controller.request.start_controllers.push_back(start_controller);
switch_controller.request.stop_controllers.push_back(stop_controller);
switch_controller.request.strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT;

// Swithching controller by calling the service
robot_switch_client.waitForExistence(ros::Duration(5.0));
bool success = robot_switch_client.call(switch_controller);

Here the variable robot_name is previously set and start_controller and stop_controller are different and are either joint_trajectory_controller orvelocity_controller.

Now it happens, often and not always, that robot_switch_client.call(switch_controller) returns false. Sometimes this happens even if the controllers have been correctly swithed (can see them switched from rqt). This happens even if I set the strictness to STRICT.

Why is this happening? What can be the possible causes and solutions?

Thanks to everyone in advance.