Model based velocity fallback for DVL invalidity#531
Model based velocity fallback for DVL invalidity#531melihokur17 wants to merge 16 commits intomainfrom
Conversation
melihokur17
force-pushed
the
melih/dvl_validity
branch
from
fc39ff1 to
002ecb4
Compare
eminmeydanoglu
left a comment
eminmeydanoglu
left a comment
There was a problem hiding this comment.
Everything is a model!
Leaving some notes, kudos
| self.is_dvl_enabled = False | ||
|
|
||
| # Model-based velocity estimator variables | ||
| self.current_wrench = np.zeros(6) |
There was a problem hiding this comment.
If controller goes ON -> OFF, the real wrench is zero, but current_wrench will be some stale nonzero numbers because the controller node doesn't publish wrench at all if controller is OFF. Not sure any possible issues from this can emerge?
Isn't it good practice to publish zero wrench if controller is OFF. After all the wrench is not nonexistent, it is really zero.
| if is_valid_msg.data: | ||
| rotated_vector = self.transform_vector( | ||
| [velocity_msg.linear.x, velocity_msg.linear.y, velocity_msg.linear.z] | ||
| ) | ||
| velocity_msg.linear.x = rotated_vector[0] | ||
| velocity_msg.linear.y = rotated_vector[1] | ||
| velocity_msg.linear.z = rotated_vector[2] | ||
| self.last_valid_velocity = velocity_msg | ||
|
|
||
| self.update_twist_covariance(use_model_based=False) | ||
|
|
There was a problem hiding this comment.
DVL works: use it
Fails: model based estimate
okay, the model keeps its own self.estimated_velocity. but we never update it while DVL is working. So when DVL suddenly fails, will the model start from a stale value?
I guess yes. Look: only compute_model_based_velocity() writes self.estimated_velocity. And if odom is not received, it uses the old stale estimated_velocity as a starting point.
I guess the fix is simple: Just add the line
self.estimated_velocity[:3] = rotated_vector
under this if. wdy think?
| self.filter_cmd_vel() | ||
|
|
||
| if self.dynamic_model_available and dt > 0.001 and dt < 1.0: | ||
| model_vel = self.compute_model_based_velocity(dt) | ||
|
|
||
| velocity_msg.linear.x = model_vel[0] | ||
| velocity_msg.linear.y = model_vel[1] | ||
| velocity_msg.linear.z = model_vel[2] | ||
| velocity_msg.angular.x = model_vel[3] | ||
| velocity_msg.angular.y = model_vel[4] | ||
| velocity_msg.angular.z = model_vel[5] | ||
|
|
||
| self.update_twist_covariance(use_model_based=True) | ||
|
|
||
| else: | ||
| velocity_msg.linear.x = self.filtered_cmd_vel.linear.x | ||
| velocity_msg.linear.y = self.filtered_cmd_vel.linear.y | ||
| velocity_msg.linear.z = self.filtered_cmd_vel.linear.z | ||
| velocity_msg.angular.x = self.filtered_cmd_vel.angular.x | ||
| velocity_msg.angular.y = self.filtered_cmd_vel.angular.y | ||
| velocity_msg.angular.z = self.filtered_cmd_vel.angular.z | ||
|
|
||
| self.update_twist_covariance(use_model_based=True) |
There was a problem hiding this comment.
the naming use_model_based confused me here (using it for cmd_vel's too). maybe is_measured is better?
…into melih/dvl_validity
melihokur17
force-pushed
the
melih/dvl_validity
branch
from
002ecb4 to
fd8fa31
Compare
…into melih/dvl_validity
2 participants
No description provided.