- dist_left = dwleft.distance()
- dist_right = dwright.distance()
- dir = "left" if (dist_left < dist_right) else "right"
-
- diff = abs(dist_left - dist_right)
- if diff >= dist_l_r:
- # difference to high, correct to maximum
- off = diff - dist_l_r + 0.01
- if dist_left > dist_right:
- dist_left -= off/2
- dist_right += off/2
+ dist_left = dwleft.distance_valid()
+ dist_right = dwright.distance_valid()
+ x = None
+ y = None
+ if dist_left == None or dist_right == None:
+ rospy.logerr_throttle(10, "no valid sensor update %r %r" % (dist_left, dist_right))
+ # run kalman prediction only
+ x,y = pos.filter(None, None)
+ else:
+ dir = "left" if (dist_left < dist_right) else "right"
+
+ diff = abs(dist_left - dist_right)
+ if diff >= dist_l_r:
+ # difference too high, correct to maximum
+ off = diff - dist_l_r + 0.01
+ if dist_left > dist_right:
+ dist_left -= off/2
+ dist_right += off/2
+ else:
+ dist_left += off/2
+ dist_right -= off/2
+ rospy.logdebug("%.2f %.2f %.2f %.2f %s", dwleft.distance(), dwright.distance(), dist_left, dist_right, dir)
+
+ a_r = (-dist_right**2 + dist_left**2 - dist_l_r**2) / (-2*dist_l_r)
+ x = dist_l_r/2 - a_r
+ t = dist_right**2 - a_r**2
+ if t >= 0:
+ y = sqrt(t)
+ rospy.logdebug("x=%.2f, y=%.2f", x, y)
+ # Rotate 90 deg
+ x, y = (y, -x)
+
+ x, y = pos.filter(x, y)
+
+ if VISUALIZE:
+ circle_left = plt.Circle((-dist_l_r/2, 0), dwleft.distance, color='red', fill=False)
+ circle_right = plt.Circle((dist_l_r/2, 0), dwright.distance, color='green', fill=False)
+ plt.gca().add_patch(circle_left)
+ plt.gca().add_patch(circle_right)
+ plt.grid(True)
+ plt.axis('scaled')
+ plt.show()