def draw_sonars(self, canvas): # Draw just the sonars
canvas.delete(self.tags + 'sonars') # Deleting a nonexistent tag is safe, so always delete the sonar lines
if not self._sonars:
self.calc_sonars()
for dist, pose in zip(self._sonars, self.sonar_poses):
origin = pose.transform(self.pose).rotate(self.pose.point(), self.pose[2])
fill = 'firebrick2' if dist > self.SONAR_MAX else 'gray'
sonar_ray = Ray(origin, dist, tags=self.tags+'sonars', fill=fill, width=1)
sonar_ray.draw(canvas)
def get_distance_left_and_angle(self):
""" Get the perpendicular distance and angle to a surface on the left.
Returns:
`(d, a)` where `d` is the perpendicular distance to a surface on the left, assuming it is linear, and `a`
is the angle to that surface.
"""
# Build a list of the points the sonars hit, or None if the distance was greater than the sonar max
endpoints = [Ray(origin, d, dummy=True).p2 if d else None for (origin, d) in zip(self.sonar_poses, self.sonars)]
return self.dist_and_angle(endpoints[0], endpoints[1])
def calc_sonars(self): # Calculate the actual sonar ranges. Called once per simulated timestep
self._sonars = [0]*len(self.sonar_poses)
for i in range(len(self.sonar_poses)):
# We take each sonar and build a ray longer than the world's max diagonal
origin = self.sonar_poses[i]
# Translate and turn by the robot's pose, then rotate about its center
origin = origin.transform(self.pose).rotate(self.pose.point(), self.pose[2])
sonar_ray = Ray(origin, 1.5*max(self.world.dimensions), dummy=True)
# Find all collisions with objects that aren't the robot itself
# Sonars only accurate to the millimeter, so let epsilon be 0.001 meters
collisions = self.world.find_all_collisions(sonar_ray, condition=lambda obj: obj is not self, eps=1e-3)
if collisions: # Should always be True since the world has boundaries
# Sort the collisions by distance to origin
distances = [origin.distance(p) for _, p in collisions]
distances.sort()
self._sonars[i] = distances[0] # Sonar reading is the distance to the nearest collision