You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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;
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.
The text was updated successfully, but these errors were encountered:
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;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
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.
The text was updated successfully, but these errors were encountered: