Added optional per-controller update period

[meyerj]

This pull request adds an optional per-controller update period to ros_control.

The default update period for a controller is zero and the controller is updated on each update of the controller manager. This should not break existing code.

If the controller sets an individual update period by calling the new ControllerInterface::setUpdatePeriod() method, e.g. in its overridden init() method, the controller manager will only invoke update if the desired period has elapsed since the last update and also adapts the period passed to the controller accordingly.

I am using ros_control for a cascaded controller where the outer loops should run slower than the inner loops. Of course it would be possible to implement this throttling in the outer controllers itself, but this solution is more general. Another use case is the JointStateController in ros_controllers, where a similar solution is already implemented for the publish rate.

There are also some checks in ControllerInterface::updateRequest() that make sure that the controller (hopefully) handles stops and restarts at a later point of time (or a time reset when running in Gazebo) correctly without passing bogus periods to the controller's update() method.

[wmeeusse]

Thanks for providing a patch to ros_control! Here are a few comments:

• Ros control is designed to trigger controllers periodically. But, you don't get control over what that update period is for each controller, the update period is set for all controller in ros control. So a controller can only decide to run every n update cycles. When you set an update period as a duration, your controller might update at some irregular pattern, e.g. first skip 2 cycles, then skip 1 cycle, then 2 again, etc. to approximate the desired update period. I think it would be better to specify that the controller should run every n cycles instead of an update period.
• Querying ros time in the hard realtime loop won't work. The update request for a controller receives the system time estimate (estimated in the realtime loop), and the duration according to the monotonic realtime clock. But as stated above, it would be better to tell the controller to run every n cycles instead, so the controller update period is constant all the time.
• The controller manager is already reading a number of controller-specific parameters (e.g. "type"), so it would make sense to add an optional parameter to the controller namespace that will be read by the controller manager at init time. That will allow us to not modify the controller base class.

[meyerj]

Ros control is designed to trigger controllers periodically. But, you don't get control over what that update period is for each controller, the update period is set for all controller in ros control. So a controller can only decide to run every n update cycles. When you set an update period as a duration, your controller might update at some irregular pattern, e.g. first skip 2 cycles, then skip 1 cycle, then 2 again, etc. to approximate the desired update period. I think it would be better to specify that the controller should run every n cycles instead of an update period.

I also thought about that, but there are use cases where you want a controller to update at a fixed time interval, independently of how fast the controller manager runs (like in the JointStateController example). Is there a possibility to get the controller manager update rate from the controller's init() function? Otherwise it would still be impossible to configure the skip cycles correctly. Of course there might also be use-cases where you want to configure the update interval in cycles, but right now I can't think of one.

In fact the number of cycles is predictable if you know the manager's update rate and the controller's update rate (given that the time argument to updateRequest(time, period) is always incremented in steps of period). For example, if the controller_manager runs with a period of 0.2s and a controller has an update period of 0.5s, it will be updated every 3rd cycle with an actual update rate of 0.6s.

Querying ros time in the hard realtime loop won't work. The update request for a controller receives the system time estimate (estimated in the realtime loop), and the duration according to the monotonic realtime clock. But as stated above, it would be better to tell the controller to run every n cycles instead, so the controller update period is constant all the time.

There is no system time querying involved here. This patch only uses the information it gets from the controller_manager as an argument to updateRequest(). ros::Time and ros::Duration are implemented with integer arithmetic internally, to there won't be floating point problems or unpredictable results with update rates like 3 Hz.

The controller manager is already reading a number of controller-specific parameters (e.g. "type"), so it would make sense to add an optional parameter to the controller namespace that will be read by the controller manager at init time. That will allow us to not modify the controller base class.

What exactly do you mean?

1. The controller manager could read a "frequency" (or "update_period", "skip_cycle",...) parameter if present and configures the controller itself. In this case controllers would not need to implement their own logic in init() to set the update rate. I did not want to add something like this as there might already be other conflicting usages of that parameter, but I would like to add that functionality if we agree on a parameter name.
2. The controller manager could keep the parameter in its ControllerSpec or hardware_interface::ControllerInfo structure and handle the individual throttling itself. In this case there would be no way for the controller itself to request or influence its desired update rate itself (thinking of dynamic update rates...).

[wmeeusse]

Is there a possibility to get the controller manager update rate from the controller's init() function? Otherwise it would still be impossible to configure the skip cycles correctly. Of course there might also be use-cases where you want to configure the update interval in cycles, but right now I can't think of one.

Currently the best way to get the update rate is to look at the duration passed into the update call.

In fact the number of cycles is predictable if you know the manager's update rate and the controller's update rate (given that the time argument to updateRequest(time, period) is always incremented in steps of period).

Yeah, that's right, so you don't actually get the desired update rate. Even your average update rate will be off. This makes a parameter to skip n cycles more appropriate.

There is no system time querying involved here. This patch only uses the information it gets from the controller_manager as an argument to updateRequest()

That's awesome, my bad!

The controller manager could read a "frequency" (or "update_period", "skip_cycle",...) parameter if present and configures the controller itself. In this case controllers would not need to implement their own logic in init() to set the update rate.

Yes, I think I'd prefer this implementation.

I did not want to add something like this as there might already be other conflicting usages of that parameter, but I would like to add that functionality if we agree on a parameter name.

There's definitely a risk to break an existing controller, but we could choose a name that's less likely to be already in use. How about something explicit like 'run_every_n_cycles'?

In this case there would be no way for the controller itself to request or influence its desired update rate itself (thinking of dynamic update rates...).

True, but it seems okay to expect a controller to handle more advanced cases itself.

Thoughts?

[meyerj]

I updated my pull request according to your suggestions:

• Check parameters update_every_n_cycles, min_update_period and max_update_frequency in the controller's namespace and store these in the ControllerSpec
• ControllerManager::update() now checks itself if the update conditions have met.
• The only things that remain in ControllerBase are the state variable last_update_time_ and skipped_update_cycles_. These are required there to be able to update them in real-time and to make sure that they are reset properly when the controller is started. ControllerBase::getLastUpdateTime() might be useful for the controller implementation, too.
• I did not remove the time-based parameters as I think they are useful applications despite of the drawbacks you mentioned. The parameter naming min_update_period and max_update_frequency should make clear that this period/frequency is not guaranteed.

[wmeeusse]

I'm a bit worried about trying to create features for too many usecases, and ending up with a messy interface:

• The latest proposal has three different parameters to change the controller rate, but you can only use one of the three at a time, otherwise you can end up with conflicting specifications.
• The controller can change the configured value on the fly, so even if you set a certain parameter, the controller might be doing something completely different.

I'd love to hear other people chime in too, but I'd vote for only adding the "run_every_n_cycles" parameter, and not allowing the controller to change this on the fly. I do realize there are valid usecases where you want a more complex behavior, but I think those can be covered inside specific controllers. In any case, @meyerj thanks for your patience in working through this pull request!

[sachinchitta]

I agree with Wim about keeping things simpler in the base classes and controller manager. The update every n cycles seems to be a reasonable compromise and more complex behaviors could happen in the individual controllers themselves. The intention for ros_control was to support the most common use cases well while leaving flexibility for more complex things to happen.

[meyerj]

Can you give me hint on how to implement the desired behavior (minimum update period/maximum update frequency) with run_every_n_cycles alone? I think there is no reliable way to do that without assuming a certain update rate of the controller manager. If I use the gazebo_ros_control package, there is no way to know the controller manager's update period from the controller's implementation or configuration as it directly depends on the world update rate.

I you prefer, I will remove the min_update_period and max_update_frequency parameters from the pull request before merging. But at least I would not use the run_every_n_cycles parameter then (and instead fall back to an internal timing helper class to control the update period in the controllers itself). If nobody would use it, there is no good reason to add it and I would also prefer to keep ros_control clean instead of adding a feature with limited use.

[wmeeusse]

@meyerj You're right that using the 'run_every_n_cycles' parameter does not give you direct control over the update frequency, as it it tied to the base update rate of the controller manager. But it is a reasonable assumption that you know the base controller rate when configuring the controllers of your robot. Even your controller gains will make an assumption about the controller rate (even when you take the controller period into account inside the controller, the bandwith of your update loop will still affect the gains). With that in mind, the 'run_every_n_cycles' parameter allows you to precisely set the update rates for each controller.

The joint_state_publisher controller has been a bit of an exception, where we've allowed people to set an actual frequency, because that controller is not actually controlling anything, and the exact update rate does not matter in that case. But for actual controllers I'd argue that you really do care about the exact update rate, and you don't want to change the trigger rate of the controller manager without updating the parameters of your controllers.

What's the usecase you're developing this for?

[meyerj]

I am in the progress of rewriting our controller structure for hector_quadrotor using ros_control and gazebo_ros_control. You can check the current state in the hector_quadrotor_controller package in the ros_control branch of the source repository, if you want.

For the real system, there is a whole bunch of controllers involved which realize a cascaded control loop from pose down to motor voltages, where there are different "branches" for different control inputs like horizontal position, height and heading. The inner control loops run on a microcontroller and communicate with the onboard PC via an Ethernet link. The outer loops are currently realized in an Orocos RTT component. For the Gazebo simulation we currently just "bypass" the inner loops by applying torques and forces directly instead of controlling the motor voltages and integrate the Orocos controllers using our gazebo_rtt_plugin.

My idea was to rewrite all the control logic using ros_control and instantiate and update the controllers either from an Orocos component in real-time or run them in Gazebo using gazebo_ros_control in simulation, without affecting the current timing too much. My QuadrotorInterface class serves as common HardwareInterface for all type of controllers and each controller registers its inputs and outputs during the init() call. The QuadrotorInterface takes care of which controller should see which other controller's output at its input ports based on the names and types. Additionally, some controllers have subscribers and react on user input as long as no upstream controller is running.

Some of the controllers would only run in Gazebo, as I don't want to touch the microcontroller firmware. Others won't control anything but implement a pre-filter or even parts of the simulation. One example is the current aerodynamics plugin, which takes the commanded motor voltages and simulates the resulting force and torque based on a empirically parameterized motor and propeller model. This "controller" would run as the inner-loop with the highest possible update rate and its output is what is finally applied to the Gazebo model. Although this is not a controller in the common sense, I would like to integrate it in the ros_control structure to avoid publishing and subscribing stuff within the same Gazebo process to overcome plugin boundaries.

To come back to the original problem: I am not sure yet if ros_control is the right solution to solve that kind of control problem. Especially there is no out-of-the-box support for cascaded controllers yet and I am not sure how to ensure that the controllers are updated in the right order if I would stop and restart one of them. In any case I must be able to control the update period for each controller individually to simulate the real system as close as possible. I am aware that the update period slightly influences the effective gains, but I did not experience any problems until now if I define all the control parameters as for the time-continuous case and use the given period for the calculation of integrated and differential errors.

I will remove the min_update_period and max_update_frequency parameters from the pull request tomorrow and try to switch to run_every_n_cycles. Gazebo will probably always run at 1000 Hz if the user does not change the physics properties and for the real system I can choose the control period freely anyway.

[meyerj]

I updated the pull request and removed the min_update_period and max_update_frequency parameters. I could not remove ControllerBase::getLastUpdateTime() because it is needed to calculate the correct period in ControllerManager::update() if the run_every_n_cycles parameter is set.

I also have pushed a rebased branch without the reverts and two stashed commits at https://github.com/meyerj/ros_control/tree/added-per-controller-update-period-rebased.

[adolfo-rt]

ros_control in its current state does not support well multiple update rates. The two existing solutions mentioned previously in the thread can work for specific usecases, but have their drawbacks:

• run_every_n_cycles guarantees periodicity of the updates, but depends on the controller_manager update frequency, so it's not self-contained. Still, it's the best solution when you're actually sending commands to hardware.
• Specifying explicitly the update frequency (or min/max bounds) is self-contained, but will in general not guarantee periodic execution. This solution is good enough for low-frequency feedback topics published by controllers (read-only controllers like the joint state publisher being a particular case).

Since these are more workarounds than solutions, my position so far in this matter has been to not touch the core of ros_control, but rather select the best strategy for a specific usecase and implement it inside a controller. Still, if there's demand for the run_every_n_cycles, I don't oppose it, as it would be an optional feature.

Finally, it's good to know that usecases for multiple controller update rates are starting to appear.

[wmeeusse]

What happens if the parameter is negative? Could you add a check for that and return false in that case?

[meyerj]

It would have no effect as it is checked in ControllerManager::update(), but I will add an error message in ControllerManager::loadController(). However, 0 and 1 are valid parameter values with the only difference that update_every_n_cycles = 0 would simply pass the period parameter of ControllerManager::update() to the controllers while update_every_n_cycles = 1 would recalculate the period based on getLastUpdateTime() if available. I am not sure if there are cases where these two are not equivalent.

[wmeeusse]

Good point! Also, running every 0 cycles does not make much sense, so I'd make the check here that the parameter has to be >= 1, and set the default value to 1.

[meyerj]

I am not sure what you mean. With update_every_n_cycles = 1 as default and minimum value the period would always be recalculated from the time argument of ControllerManager::update(time, period) ignoring the period argument except for the first invocation after the start. Is this what you want?

I would suggest to additionally change the check in controller_manager.cpp:97 and only throttle and recalculate if spec.update_every_n_cycles > 1.

[wmeeusse]

The (only) correct way to compute the period over multiple cycles is to add up the periods of the individual cycles. The 'period' values come from the monotonic clock, so they are the only correct way to measure the total period over multiple cycles. The 'time' is the best approximation of the system time, and is not monotonic; it can be adjusted at runtime to stay synchronized with the actual world time.

If you modify the implementation to create a sum of periods (which is the only correct way to implement this), there's no special case needed for update_every_n_cycles == 1. So you just keep adding the period to the sum of periods, until the number of periods equals update_every_n_cycles.

[wmeeusse]

@meyerj Thanks for the comments and the update to the PR. I added a few comments, but after updating those, this PR looks good to go. As for your usecase, ros control indeed does not have support for cascading controllers. I believe right now the controllers get executed in the order that they are loaded, so you'll need to spawn them in sequence to get a reliable execution order.

[meyerj]

Updated with "add up periods" patch. There still is a clean rebased and squashed branch available at https://github.com/meyerj/ros_control/tree/added-per-controller-update-period-rebased, if you prefer.

I also made the skipped_update_cycles_, total_update_period_ and last_update_time_ parameters private in the ControllerBase class so that they cannot be used directly in controller implementations.

[wmeeusse]

Awesome, this is really getting somewhere! Thanks for all the work! One last comment here, now that we're adding up the durations, can we get rid of last_update_time_ and get the patch nice and compact?

[adolfo-rt]

Hey,

I'm picking this one up. I'll write some tests for this feature, which is what's been preventing the merge.

[meyerj]

Closing this in favor of #207.