0
$\begingroup$

Rosanswers logo

Hi all

I'm trying to get a controller to run in a ros_control controller manager and followed the general principles listed on the ros_control git wiki.

Now in the controller manager runs in a non-realtime loop .

The controller init routine runs and completes successfully, however, no further starting(), stopping(), or update routines get run().

At various times, I have adjusted the interface setup on the controller to open some custom interfaces, and usually they cause the init() to fail (but with out any warnings that it will or does)

EDIT Here is the loggers for the controller manager program when rosservice /controller_manager/list_controllers is called:

ANSWER If you aren't interested in following the comment thread attached to adolfo's answer, the problem was summed up by:

  1. Not understanding how the controller manager started and stopped controllers.

  2. Getting the services on the controller manager to work.

  3. The controller manager has no exposed start or stop routine or service. Instead the user has to use switch_controllers(startcontroller, stopcontroller, strictness). or the equivalent service. To use a controller, it must first be loaded by the controller_manager, using load_controller(controller), and then started. To perform a start of a newly loaded controller, the switch_controllers() routine is used with an empty field for the stopcontroller. Strictness has value of 1 (accepts errors) or 2 (switch must be perfect (STRICT)). The service call is:

    rosservice call /controller_manager/switch_controllers [starting_controller_name] [stopping_controller_name] 1

  4. If you find that the controller_manager is not processing the service calls but instead 'freezes' then your hardware implementation program is not effectively dealing with the callbacks of the controller_manager (and possibly your controller)

It was found that a specific ros::callbackqueue needed to be established for the implementation's nodehandle using nodehandle::setCallbackQueue(). It was found that an AsyncSpinner was required to work on that queue, using the ros::AsyncSpinner spinner(0, &my_callback_queue); This allowed the spinner to explicitly work on the callbacks passed to the nodehandle. As this nodehandle was passed to the controller manager, the controller manager service callbacks were also pushed onto this queue. This link details the different ways of dealing with callbacks and spinners. Section 4 covers the details of the method that worked.

Thanks to Adolfo for his perserverence and help in resolving this matter.


Originally posted by PeterMilani on ROS Answers with karma: 1493 on 2014-01-19

Post score: 4


Original comments

Comment by PeterMilani on 2014-01-19:
looking through controller_manager/controller_manager.cpp, I cant see where the cm is starting the controllers. in ControllerManager::loadcontroller(). The init routine is run, but the cm does not go on to start the controllers. The update() only seems to apply to those that are running.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

When the controller manager loads a controller, it is in the stopped state. You need to explicitly start it to transition to a running state. You can check a controller's state by calling /controller_manager/list_controllers

This the usual flow:

Load a controller: Use the /controller_manager/load_controller service

  • Precondition: Controller config loaded in the ROS param server.
  • Effect: Calls the controller's init(...) method.
  • Postcondition: Controller is loaded, but stopped.

Start a controller: Use the /controller_manager/switch_controller service (start_controllers field)

  • Precondition: Controller already loaded and stopped. Resources to claim are available.
  • Effect: Calls the controller's starting(...) method
  • Postcondition: Running controller.

Stop a controller: Use the /controller_manager/switch_controller service (stop_controllers field)

  • Precondition: Controller is running.
  • Effect: Calls the controller's stopping(...) method
  • Postcondition: Stopped (but still loaded) controller.

Unload a controller: Use the /controller_manager/load_controller service

  • Precondition: Controller is stopped.
  • Effect: Calls controller destructor.
  • Postcondition: Controller no longer managed by the controller_manager.

Originally posted by Adolfo Rodriguez T with karma: 3907 on 2014-01-19

This answer was ACCEPTED on the original site

Post score: 4


Original comments

Comment by PeterMilani on 2014-01-20:
I thought it was unusual that there would be no provided way for starting the controllers. the srv /../list_controllers returns the running controller. However When I apply a switch service the cm crashes. The switch is: rosservice call /controller_maner/switch_controller [] [hmmv4_controller] 2

Comment by Adolfo Rodriguez T on 2014-01-20:
Peter, 1. When list_controllers returns a running controller, did you start it with the switch_controller service or with your loadController() modification?. 2. What is the backtrace of your crash?. 3. The switch_controller call you paste above is for stopping hmmv4_controller, is this the intent?.

Comment by PeterMilani on 2014-01-21:\

  1. yes To see the controller running i need to use the modified code. when using the standard code, a call to /list_controllers freezes with no response. 2. There is no back trace just an empty frozen cursor. The controller is loaded in code with cm.loadController("hmmv4_controller"). 3.Yes

Comment by PeterMilani on 2014-01-21:
When using rqt_consol to query loggers produces the same effect. The Debug Log indicates a Connection::drop(2) after a after rosservice wants a new connection to /set_logger_levels. The TCP connection Terminates. Pubs and Subs work fine though, as srv does when a controller is running

Comment by Adolfo Rodriguez T on 2014-01-21:
Could you try starting your controller using the switch_controller service and an unmodified controller_manager?. Also, if you want to load+start a controller in a single call, consider using the spawner script of the controller_manager package.

Comment by PeterMilani on 2014-01-21:
Reverted to the original code, and displayed the log output from the controller manager (earliest last) when /list_controllers is called. The socket is recieving 0/4 bytes and is closing. Note it works when controller is already running with the modified code, so the network setup should be okay.

Comment by Adolfo Rodriguez T on 2014-01-21:
Does list_controllers work when your controller is not loaded, or when other existing controllers are loaded/running?.

Comment by PeterMilani on 2014-01-22:
With the controller not loaded, list_controllers freezes, with it loaded, list_controllers freezes, with it running, list_controllers returns the controller is running. By the way, roswtf returns no errors relating to comms. When frozen, roswtf reports everything ok, rossrv list still works.

Comment by PeterMilani on 2014-01-22:
Also as I am communicating between machines, I tried a service call to a different service server, it worked no problem. The only difference I can think is that it is an armhf machine running the controller manager. But the code was compiled from source as at late Nov 13.

Comment by Adolfo Rodriguez T on 2014-01-22:
I don't think it should be a version issue, but you could try using the binary packages or the latest sources. The controller_manager, and in particular its service API is quite stable and used a lot. I doubt the issue lies there.

Comment by Adolfo Rodriguez T on 2014-01-22:
Further, if list_controllers is not working properly in an empty setup, I would check how you're handling the spinning of ROS callbacks in your robot hardware abstraction. How do you go about it?. Maybe some piece of code is blocking and preventing your spinning mechanism from being triggered.

Comment by PeterMilani on 2014-01-22:
I think you've hit on something. There is now spin handling in the loop updating the cm. The cm now reports list_controllers in all configurations. But when I go to load, the service still hangs after all code in the init routine has been finished. I noticed in the source a reference to rt thread...

Comment by PeterMilani on 2014-01-22:
...that causes a while loop at the end of load_controllers(), that has no break statement. If i'm not using a realtime thread, what will break that loop? The last DEBUG message is above that loop, and none of the DEBUG after that loop get called. Loop starts around line 265 of controller_manager.

Comment by PeterMilani on 2014-01-22:
I commented out that loop in controller_manager.cpp, and the load_controllers() service works! The switch still hangs, following the DEBUG: It stops before completing the for loop around line 411. It finds the controller that needs starting, but never tells of the size of the start vector.

Comment by Adolfo Rodriguez T on 2014-01-22:
What kind of spinner do you have in your robot hardware abstraction?

Comment by PeterMilani on 2014-01-22:
the standard ros::spinOnce(). I've not customised any spinner. but it seems as per above the srv callbacks are being processed they are not completing for various reasons. Now spinOnce() blocks so the program should loop as fast as the callbacks can return. My looptime is not super fast 7ms.

Comment by PeterMilani on 2014-01-22:
Got it working: spinOnce in hardware update cycle didn't really catch the callbacks pertaining to the cm. Established a asynchSpinner processing a queue specific to the nodehandle: http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning section 4. I guess it drives the cm to use the nh's que

Comment by PeterMilani on 2014-01-22:
Thanks again, Adolfo, I'll eventually catch up to you guys.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.