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