Multi cycle control switch

[jordan-palacios]

Currently, the control switch performed in the ControlManager update cycle is considered to be an atomic operation, i.e., all necessary tasks needed to do a control switch are performed in a single call.

This is the code excerpt from the update() function that handles the switch:

  // there are controllers to start/stop
  if (please_switch_)
  {
    // switch hardware interfaces (if any)
    robot_hw_->doSwitch(switch_start_list_, switch_stop_list_);

    // stop controllers
    for (unsigned int i=0; i<stop_request_.size(); i++)
      if (!stop_request_[i]->stopRequest(time))
        ROS_FATAL("Failed to stop controller in realtime loop. This should never happen.");

    // start controllers
    for (unsigned int i=0; i<start_request_.size(); i++)
      if (!start_request_[i]->startRequest(time))
        ROS_FATAL("Failed to start controller in realtime loop. This should never happen.");

    please_switch_ = false;
  }

It can be found here: https://github.com/pal-robotics-forks/ros_control2/blob/melodic-devel/controller_manager/src/controller_manager.cpp#L95.

Unfortunately, more often than not, a control switch can take more than one control cycle. It is common case for started controllers to start sending commands to actuators that are still transitioning to the new control mode. Ultimately, this can lead to undesired behaviour.

Real-time constraints on doSwitch(...) means it can't block while waiting for a switch to finish. I propose we add a new function switchState() to RobotHW's API. This method will report the current state of the switch: Done, Error, Ongoing. We can use it for waiting for switching to finish before starting the new set of controllers across multiple update() calls. Default implementation will always return Done and will allow for backwards compatibility.

[bmagyar]

Can the current canSwitch()/doSwitch() mechanism be retained and deprecated or should it directly be removed? Will switchState() operate on a set of joint-modes which then allow for implementing the queue of controllers waiting to be loaded/started?

switchState is probably a name that's overly general, switchMode may be better fitted but this is a minor detail.

[jordan-palacios]

Can the current canSwitch()/doSwitch() mechanism be retained and deprecated or should it directly be removed?

I was thinking in keeping them and try to not break backwards compatibility. At least from a functional point of view.

Will switchState() operate on a set of joint-modes which then allow for implementing the queue of controllers waiting to be loaded/started?

My initial approach would consist of relaying its implementation to RobotHW, with an ideal default implementation. Something like:

enum SwitchStates
{
  DONE,
  ONGOING,
  ERROR
}

virtual void switchState()
{
  return DONE;
}

This would mean that current implementations that relay on doSwitch() to be mono cyclic won't break. Provided they keep the default implementation of switchState() in the RobotHW of course.