Commit 6762f7f2 authored by Gill Lumer-Klabbers's avatar Gill Lumer-Klabbers
Browse files

fix distance calculation...

parent e7980554
......@@ -24,7 +24,7 @@ class unifmuSafetyMonitor:
def fmi2DoStep(self, current_time, step_size, no_step_prior):
x_delta = self.robotti_xpos_input - self.obs_xpos_input
y_delta = self.robotti_ypos_input - self.obs_ypos_input
self.dist = sqrt(abs(x_delta + y_delta))
self.dist = sqrt(x_delta**2 + y_delta**2)
if self.dist <= self.safety_dist:
self.flag = True
else:
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment