Migrate to new control structure

[paulbovbel]

Add new action and service types

Break controllers into attitude, velocity, and position

Split up _controller package into _interface and _controllers

Migrate to control_toolbox's PID implementation, waiting on https://github.com/ros-controls/control_toolbox/pull/38 for antiwindup equivalent functionality

Add _action package containing pose, landing, and takeoff action servers

Update teleop to new control structure

[meyerj]

Hi Paul,

I finally had the time to release the hector packages into jade and to review your patch this weekend. It seems like a valuable contribution to hector_quadrotor and I appreciate the amount of time you spent to implement this! You told me in Hamburg that you are not working on this project anymore, but perhaps you find some time to answer some questions below.

I consider to merge this branch into jade-devel to be released into jade as soon as the anti-windup patch would be merged into control_toolbox and ros_control is released. I think for indigo the changes are too heavy and the missing anti-windup patch might be a blocker. I merged the latest patches for jade into this branch, added some patches and pushed it as clearpathrobotics-asctec-merge-2.

Some questions:

1. Do you want to maintain the new packages or can I update the maintainer tag in the package.xml files to myself?
2. I assume all your contributions are also BSD licensed and I can copy the existing license headers to the new header and source files?
3. Where does the branch name asctec-merge come from? Did you plan to use this stack to simulate an AscTec drone or are parts of the code motivated by AscTec code?

Open issues that I found so far:

• The twist command must be given in the base_stabilized frame now. I assume it is straight-forward to re-add the possibility to publish a twist in the world coordinate system. Conversion has to happen in the update() callback based on a pose returned by the quadrotor interface, because the yaw angle might change over time and the twist command is only published once.
• The cmd_vel (geometry_msgs/Twist) subscriber has been removed.
• Commanding positions with a joystick or gamepad does not work yet. The commanded position would have to be updated periodically, and not only once the callback is invoked. With my Logitech gamepad the joy node only publishes a message if I change the position of the stick, but not if I simply hold it. Obviously you started to work on this (the new autorepeat_rate parameter, but it is not yet finished).
• The auto_engage feature, which starts and stops motors automatically based on the vertical velocity command and control error, has been removed.
• The new controller implementation is different from what was in the twist_controller.cpp before. The main difference is that with the addition of the attitude controller in the loop the wrench output is calculated from Euler angle errors, while the previous implementation directly calculated it by comparison of the desired stabilized acceleration vector with the current thrust vector. With this method the effect of inaccuracies due to the non-linearity of the Euler angle conversion and the disturbing effects of higher accelerations in the z-axis on the horizontal velocity control can be reduced significantly. Is it okay if I try to reintroduce this control method in the new implementation, without removing the possibilities to command Euler angles directly?
• There should be a way to enable and disable controllers dynamically. This was already a shortcoming of the previous ros_control based controller implementation. As ros_control does not allow to stop and start controllers dynamically with preservation of the execution order, this has to be implemented within the controllers itself. For now, in order to control the quadrotor in velocity mode, it has to be spawned without the position controller.

I have not tested the interaction with hector_pose_estimation yet, if the quadrotor is spawned with use_ground_truth_for_control:=false. In this case the pose and twist is estimated from the simulated noisy IMU, magneto and GPS measurements and not taken from ground truth. Is it correct that you never tested this mode and the current PID parameters might be too tight with imperfect state information? I noticed that you even removed the hector_quadrotor_pose_estimation package in your working branch.

[paulbovbel]

@meyerj thanks for taking a look at this! I've been a little out-of-the-loop with github but if you have some time to sort through this pull and make improvements, I'd be happy to help any way I can.

I have a BSD licensed hardware driver for Asctec Pelican/Hummingbird, along with the associated URDFs for simulation, ready to go when this is merged and released. Of course we're still waiting for ros_control release into jade.

Do you see a clean way to do both the Euler-angle based approach, along with the vector difference? Perhaps it may be best just to have 4 controllers (position, velocity, attitude, and your original direct-velocity controller)

I would be happy to take maintainership of stuff I wrote independently, pass it to you, or we can share. I need to go carefully go through the headers of this pull, and the package.xml's, and make sure there's proper attribution.

With regards to execution order, it wasn't a huge deal for my application (running at 100 Hz with pretty low specification to hit), but I can see how that's an issue in more high-precision applications.

[paulbovbel]

Moved discussion to #44