Support for ros_control

[mathias-luedtke]

These plugins connect to running ros_control nodes and add handles for execution.

For the sake of flexibility I have outsource the handle creation to plugins. I added a simple implementation for the joint_trajectory_controller types that uses the moveit_simple_controller_manager action handles.
That's why I had to expose its headers.

I have tested it with our https://github.com/ipa320/cob_gazebo_plugins environment.
it still lacks proper unit tests and the configuration is rather limited.

it would be great if other ros_control users would test it with theirs setups and give some feedback.

[davetcoleman]

this is exciting!

[davetcoleman]

*simply

[davetcoleman]

Great looking code, good work!

[mikeferguson]

also, according to ROS style guide, there should be a space after the if

[davetcoleman]

+1

[mathias-luedtke]

Thanks for your first review!

Personally, I dislike the roscpp style (especially the braces!), I do not consider it while binge coding.
This PR is still WIP, I will run it through a code formatter (eclipse?) before the actual merge :)

[mathias-luedtke]

Anything else that I need to change apart from the code style?

[davetcoleman]

I've compiled and used your new plugin just now and have some feedback. First, control latency is really bad. I'm doing some crazy stuff with MoveIt! where I command trajectories at ~30hz, so perhaps I'm not an ideal use case, but even so the latency difference is huge:

moveit_simple_controller_manager average: 0.00638423 s
moveit_ros_control_interface     average: 0.222877 s

That's 100x slower! I tried to disable all the service calls going on by commenting out the discover() call in getControllerState() and from my cout's there appears to be no other service calls, but I still couldn't get the latency down. I talked to @isucan about the limitation's of MoveIt!'s TrajectoryExecutionInterface and the blame is most likely because of all the checks it does when managing controllers. The simple_controller_manager, by contrast, is an unmanaged controller interface so is way faster. I'm not saying your plugin is bad, its probably MoveIt!'s fault, but I can't use it currently for my application and I'm hoping you can give some insight on this issue (I already spent an hour reading into it).

Other issues: it would be nice if we made the usability/setup of the plugin very similar to moveit_simple_controller_manager. By this I mean in the robot_controllers.yaml file, lets call it controller_manager_ns to match the simple version. I also don't think it should require a slash at the end of the namespace, since the simple plugin doesn't. To clarify, I'll provide some setup documentation that I think you should also add to the README if you don't mind:

Setup

In ROBOT_moveit_config/launch/ROBOT_moveit_controller_manager.launch.xml set the following arg:

  <arg name="moveit_controller_manager" default="moveit_ros_control_interface::MoveItControllerManager" />

In ROBOT_moveit_config/config/ROBOT_controllers.yaml make sure you have a properly set controller manager namespace and controller list. An example looks like the following:

controller_manager_ns: ROBOT
controller_list:
# Arm controller                                                                                                                                                                                                                               
  - name: /ROBOT/position_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_1
      ...

Final Thoughts

Line indentations should be 2, not 4, per ROS style, throughout the code.

Thanks for contributing this plugin!

[mathias-luedtke]

As far as I read the code for the TrajectoryExecutionInterface the overhead should be the same for all plugin. I'll guess that the handle allocation (+plugin loader) is bottleneck.
I will try to reuse them.

Can you provide a simple test-setup? How did you measeure the times?

[davetcoleman]

I have a node using MoveIt! functionality and I create a control_msgs::JointTrajectory with

header.stamp = ros::Time::now()

I send that as an action through the TrajectoryExecutionInterface and I measure the delay inside the JointTrajectoryController using the code I just created a PR for an hour ago, here

By the way, if you like that compiler flag latency measuring code, perhaps +1 it in that pull request. I feel like some developers under appreciate adding good debugging tools for other users to benefit from.

[mathias-luedtke]

I have implemented a test suite to analyse the performance drop (https://github.com/ipa-mdl/traj_ex_test).
The pre-allocation did the trick. The bottleneck was not the allocation, but the actionlib topic reconnects.

Now all controller manager plugins show the same performance. I have found a new bottleneck.. my notebook, ~350 Hz seem to be my limit ;)

At 30 Hz I measured a latency of 0.00135252 s on average for a one-joint, one-point trajectory.

By this I mean in the robot_controllers.yaml file, lets call it controller_manager_ns to match the simple version.

This parameter name is very confusing since controller_manager is ambiguous.
I chose ros_control_namespace, because it is the namespace of the ros_control node.
I am open to unambiguous alternatives..

I also don't think it should require a slash at the end of the namespace, since the simple plugin doesn't.

Sounds reasonable. I will add this and a setup section to the README.md :)

[davetcoleman]

Great, so glad you added pre-allocation! I'll test again when you tell me its ready.

The parameter name change is fine with me as long as its documented - you're right, it is currently ambiguous. But perhaps @mikeferguson has a thought.

[mikeferguson]

I think the naming is fine -- the naming in the simple_controller_manager simply tries to imitate the pr2 version (since that was the documentation most people would initially stumble upon).

+1 to merge once @davetcoleman signs off on his final testing round

[mathias-luedtke]

I'll test again when you tell me its ready.

The functionality is ready now and can be tested.
I will update the documentation in between.

[mathias-luedtke]

I have updated the documentation. I can squash it before the merge.

[davetcoleman]

needa = needs

[davetcoleman]

I've re-run my benchmark and the average latency is much much better - 0.00111334 seconds!

Is your test suite something that should be maintained going forward for testing this component of MoveIt! ? Do you think it could be converted into a real test?

Thanks for the hard work on this and sorry for my delay!

[mathias-luedtke]

Is your test suite something that should be maintained going forward for testing this component of MoveIt!? Do you think it could be converted into a real test?

I am not sure. It contains a lot of plugins for MoveIt! and ros_control that basically do nothing..

[davetcoleman]

2 things

• I just noticed this PR is targeted for indigo, can we re-open it for jade since MoveIt! is on a feature freeze for indigo?
• I was just testing this in Jade and it does not compile because of a change in controller_manager_msgs on the ControllerState - sorry about that :-/

[mikeferguson]

@davetcoleman -- we can merge this into indigo -- as this is a new plugin in a new package (thus no real API change)

[mathias-luedtke]

We still target indigo with our robots and I cannot support jade for now.
However, the change in ControllerStateshould be handled easiliy: just merge the resources from of the new list.

To ease up the migration I can squash the PR.
Do you have recommendation for a code formatter?

[davetcoleman]

I just created this repo/documentation for you, let me know if you run into problems with it:

https://github.com/davetcoleman/roscpp_code_format

[mathias-luedtke]

Great, thanks for your efforts!
I will have a look at it tomorrow :)

[davetcoleman]

fyi I added a jade-devel branch just now and made it default. as soon as moveit/moveit_ros#616 gets merged in, MoveIt! Jade will officially be incompatible with Indigo because of new xacro features used in that PR. It would be nice if you could soon add your plugin to the Jade branch, too (with the new ros_control message format).

[mathias-luedtke]

Again: I do not develop for jade. I don't even have a jade set-up.
Which new xacro features do you refer to? The jade version was backported to indigo: ros/xacro#117
(already released in v1.9.5)

[davetcoleman]

Ah, I didn't realize the jade version of xacro was back ported to indigo, I suppose its fine then.

Re: developing for jade. I understand you want to stick to indigo. I only partially switched to jade last Friday for the first time, it took about an hour. Its just that MoveIt! developers will need to start doing this since the branches are likely to diverge.

[davetcoleman]

Thanks!