Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Handling command timeout and desired state matrix in controller #115

Open
eminmeydanoglu opened this issue Dec 30, 2024 · 0 comments
Open
Labels
bug Something isn't working

Comments

@eminmeydanoglu
Copy link
Contributor

eminmeydanoglu commented Dec 30, 2024

Current implementation of controller_ros.h has a timeout function which returns true if there are no vel and 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{};

This implementation has served its purpose effectively so far, but as we transition to a plan-and-track stack, it introduces potential for unexpected behavior. And on this pretext, maybe we can also add some new cool functionalities.

The Main Issue: Handling cmd_pose and cmd_vel Interactions

Say we had cmd_pose z = -0.5 a time ago but it hasn’t been updated for five seconds now. However, cmd_vel has continued to be published, so the controller doesn’t consider itself timed out. The result? The robot keeps aligning to z = -0.5, even though that’s no longer what we want. To make matters worse imagine we have cmd_vel.z commands too. Than the controller simultaneously tries to align to z = -0.5 and to comply with the velocity directive from cmd_vel.

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 can be times we don't care about the depth controller node but want the command's from path tracker. Or any scenario we have either cmd_pose or cmd_vel active and the other one timeouted.

And this would be simple to fix, right? Just imagine we seperated timeouted as 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 can simply mean something like

desired_state_.tail(6).setZero();

or

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

So, the robot will freeze where it is timeouted by trying to have zero velocity/ stay at current state. Or we can just go with the old implementation, which is simply publish-zero-wrench (I think freezing is cooler)

But what if we are only timeouted in one of them? This is trickier because setting "zero" as the desired state won't work neither for the position or the velocity. If you set desired state position values to zero, we will want to go to odom (while also wanting to comply with the command velocity). If you set desired velocity position values to zero, the robot will want to freeze (while also wanting to go to command pose)

Then what we want to do is to somehow deactivate the controller that is timeouted. I am not sure about the best way to do this, but setting the k's to zero if the controller is timeouted is a straight forward solution that comes to mind. This way, even though we will have a desired state position slot from an old message (we can't seem to get rid of them) we won't be using it unless we are on for example velocity control. And if we are not also on velocity control (both commands are timeouted and control is active), robot will just freeze with zero velocity.
Lastly one keyword is to "mask" the controllers. That word can guide us toward the solution.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
None yet
Development

Successfully merging a pull request may close this issue.

1 participant