Handling command timeout and desired state matrix in controller

[eminmeydanoglu]

Current implementation of controller_ros.h has a timeout function which returns true if there are no vel/ pose commands for a second. And if timeouted, we send zero wrench;

      const auto wrench_msg =
          (is_control_enabled() && !(is_vel_timeouted() && is_pose_timeouted()))
              ? auv::common::conversions::convert<ControllerBase::WrenchVector,
                                                  geometry_msgs::Wrench>(
                    control_output)
              : geometry_msgs::Wrench{};

There arises a couple of problems with this implementation if we are to move to a plan-and-control stack. On this pretext, we can also add some new cool functionalities:

Problem: Desired states never reset even if timeouted.

Say we had cmd_pose z = -0.5 a time ago. But now we are not publishing it for 5 seconds. But since we had cmd_vel, controller didn’t think it is timeouted. So we continue to align with z=-0.5 even though we don’t want it anymore.

This wouldn't be a problem before times, because we always have a cmd_pose z command, even when the depth controller service is never called (in that case cmd_pose z = 0, go to odom)
But with our navigation stack, there will be times we don't want to have cmd poses, and instead we want all the control to be done by the velocity parts.

But this would be simple to fix, right? Just imagine we seperated timeouted as pose is_vel_timeouted and is_pose_timeouted.

But not so easy. The question now is what to do if pose is timeouted or vel is timeouted. If both are timeouted, but control is active, we can think about freezing the robot. That means

desired_state_.head(6) = state_.head(6);

and

desired_state_.tail(6).setZero();

So, the robot will freeze where it is timeouted for both pose and vel commands. Or we can just go with the old implementation, which is simply publish-zero-wrench (I think freezing is better)
But what if we are only timeouted in one of them? Lets start with the easy one. If cmd vel is timeouted only, we will simply

desired_state_.tail(6).setZero();

and position controller will do what it is supposed to.
But what if cmd pose is timeouted yet cmd vel is active (which is our case for plan-and-control stack)?

The problem here is that if we try to go with

desired_state_.head(6) = state_.head(6);

the robot will want to both freeze and have a nonzero velocity. We obviously don’t want that.
Then we have to either (1) remove the position desired state somehow or (2) ignore the values it has desired_state position part if there are no pose commands.

I can't think of anything that solves the problem with approach 1. I will offer one solution I thought of, but maybe a better one can be found:

One possible solution is to set gains to zero temporarly. So something like this could work:

if (is_pose_timeouted()) {
     // Update gains based on pose timeout
     update_gains_for_pose_timeout(is_pose_timeouted());

and

void load_parameters() {
    kp_ = VectorRosparamParser::parse("kp", ros::NodeHandle("~"));
    ki_ = VectorRosparamParser::parse("ki", ros::NodeHandle("~"));
    kd_ = VectorRosparamParser::parse("kd", ros::NodeHandle("~"));
    
    // Store original gains
    kp_original_ = kp_;
    ki_original_ = ki_;
    kd_original_ = kd_;
  }

  void update_gains_for_pose_timeout(bool is_pose_timeouted) {
    auto controller = dynamic_cast<auv::control::SixDOFPIDController*>(controller_.get());
    if (!controller) return;

    if (is_pose_timeouted) {
      // Zero out position control gains (first 6 elements)
      kp_.head(6).setZero();
      ki_.head(6).setZero();
      kd_.head(6).setZero();
    } else {
      // Restore original position control gains
      kp_.head(6) = kp_original_.head(6);
      ki_.head(6) = ki_original_.head(6);
      kd_.head(6) = kd_original_.head(6);
    }

    controller->set_kp(kp_);
    controller->set_ki(ki_);
    controller->set_kd(kd_);
  }

This way, even though we will have a desired state slot from an old message (we can't seem to get rid of them) we won't be using it unless we are on velocity control. And if we are not also on velocity control, robot will just freeze (both commands are timeouted)