Add List Controllers service API to controller manager

[swhart115]

ros-control allows you to make a service call to get the active controllers.

The name is: "/robot_name/controller_manager/list_controllers" and the type is controller_manager_msgs::ListControllers

It would be helpful for the ros-controllers controller manager to provide a similar functionality.

[mikeferguson]

There is a robot_controllers_msgs/QueryControllerStates action available at /query_controller_states -- this is actually able to both load/unload controllers, and returns a list of the current controller state. Does this appear to cover your needs?

Admittedly, we need to document this -- we'll try and get that done shortly.

[swhart115]

Thanks Mike,

I'm not sure this is what I want. I dont want to start or stop running controllers, i want to get the ones that are running. For Fetch, this basically means i want a list like:

[arm_controller/follow_joint_trajectory,arm_with_torso_controller/follow_joint_trajectory,/gripper_controller/gripper_action,/head_controller/follow_joint_trajectory,/torso_controller/follow_joint_trajectory]

which is what the ros_control ListControllers service would give.

I tried just sending an empty Goal topic to the action you suggested, but that doesnt seem to return useful things for my purposes.

[mikeferguson]

Hmm, an empty goal topic should return the info -- let me look at that, it sounds like it might be a bug

[swhart115]

on /query_controller_states/status, /query_controller_states/feedback, or /query_controller_states/result?

[swhart115]

For completeness,

$ rostopic pub /query_controller_states/goal robot_controllers_msgs/QueryControllerStatesActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  updates:
  - name: ''
    type: ''
    state: 0" 

Results in this on result

header: 
  seq: 892
  stamp: 
    secs: 185
    nsecs: 594000000
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 185
      nsecs: 594000000
    id: /gazebo-893-185.594000000
  status: 4
  text: No such controller to update: 
result: 
  state: []

and this on status:

---
header: 
  seq: 3774
  stamp: 
    secs: 227
    nsecs: 992000000
  frame_id: ''
status_list: 
  - 
    goal_id: 
      stamp: 
        secs: 222
        nsecs: 944000000
      id: /gazebo-1271-222.944000000
    status: 4
    text: No such controller to update: 
  - 
 ....
  - 
    goal_id: 
      stamp: 
        secs: 227
        nsecs: 894000000
      id: /gazebo-1321-227.894000000
    status: 4
    text: No such controller to update: 
  - 
    goal_id: 
      stamp: 
        secs: 227
        nsecs: 992000000
      id: /gazebo-1322-227.992000000
    status: 1
    text: This goal has been accepted by the simple action server

[mikeferguson]

Ok -- just finally got back to this -- it looks like this does work -- for example, you can do:

fergs@freight3:~/indigo/src/robot_controllers/robot_controllers_interface/scripts$ ./get_controller_state.py 
[INFO] [WallTime: 1463708196.887484] Connecting to /query_controller_states...
[INFO] [WallTime: 1463708197.250827] Requesting state of controllers...
base_controller[robot_controllers/DiffDriveBaseController]: RUNNING

Here we get that base_controller is of type robot_controllers/DiffDriveBaseController and is RUNNING.

In your example above, you were setting updates to an array containing one instance, with an empty name -- rather:

rostopic pub /query_controller_states/goal robot_controllers_msgs/QueryControllerStatesActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  updates: []" 

With an empty array of updates, works.

However, I've also now updated the behavior with https://github.com/fetchrobotics/robot_controllers/pull/20, because it really should always return the controller states, regardless of whether we succeed/fail to update them. This patch will make the next release.