Source code for sensor

# Sensors
# In the original Sim.I.Am code there is no base class, as the sensors are
# completely arbitrary objects

import random
from simobject import SimObject
from pose import Pose
from math import sin, cos, sqrt

from robot import Robot

[docs]class Sensor: """Base superclass for sensor objects""" @classmethod
[docs] def add_gauss_noise(value, sigma): """Returns the value with an added normal noise The return value is normally distributed around value with a standard deviation sigma """ return random.gauss(value,sigma)
[docs]class MountedSensor(SimObject, Sensor): """A sensor that moves together with its parent object. The sensor is assumed to be attached to *parent* at *pose* in local coordinates. """ def __init__(self,pose,parent): SimObject.__init__(self,pose) self.__frame = parent
[docs] def get_internal_pose(self): """Get the pose of the sensor in the parent (robot) coordinates.""" return SimObject.get_pose(self)
def get_pose(self): x, y, t = SimObject.get_pose(self) rx, ry, rt = self.__frame.get_pose() return Pose(rx+x*cos(rt)-y*sin(rt),ry+x*sin(rt)+y*cos(rt),t+rt)
[docs]class ProximitySensor(MountedSensor): """Create a proximity sensor mounted on robot at *pose*. The geometry is a (rmin, rmax, angle) tuple. """ def __init__(self,pose,robot,geometry): """Create a proximity sensor mounted on robot at pose. The geometry is a (rmin, rmax, angle) tuple """ MountedSensor.__init__(self,pose,robot) self.rmin, self.rmax, self.phi = geometry self.pts = self.get_cone(self.rmax) self.fullcone = [(0,0), (self.rmax*cos(self.phi/2),self.rmax*sin(self.phi/2)), (self.rmax,0), (self.rmax*cos(self.phi/2),-self.rmax*sin(self.phi/2))] self.__distance = 65536 self.set_color(0x33FF5566) def get_cone(self, distance): return [(self.rmin*cos(self.phi/2),self.rmin*sin(self.phi/2)), (distance*cos(self.phi/2),distance*sin(self.phi/2)), (distance,0), (distance*cos(self.phi/2),-distance*sin(self.phi/2)), (self.rmin*cos(self.phi/2),-self.rmin*sin(self.phi/2))]
[docs] def get_envelope(self): """Return the envelope of the sensor""" return self.fullcone
[docs] def distance_to_value(self,dst): """Returns the distance to the value using sensor calculations""" raise NotImplementedError("ProximitySensor.distance_to_value")
[docs] def distance(self): """Returns the distance instance""" return self.__distance
[docs] def reading(self): """Returns the reading value""" return self.distance_to_value(self.distance())
[docs] def update_distance(self, sim_object = None): """updates all the distances from the reading""" if sim_object is None: # reset distance to max self.__distance = 65536 self.set_color(0x33FF5566) self.pts = self.get_cone(self.rmax) return True else: distance_to_obj = self.get_distance_to(sim_object) if distance_to_obj: if self.__distance > distance_to_obj: #self.set_color(0x336655FF) self.set_color(0xCCFF5566) self.pts = self.get_cone(distance_to_obj) self.__distance = distance_to_obj return True return False
[docs] def draw(self, r): """draws the sensor simobject""" r.set_pose(self.get_pose()) r.set_brush(self.get_color()) r.draw_ellipse(0,0,min(1,self.rmin/2),min(1,self.rmin/2)) r.draw_polygon(self.pts)
[docs] def get_distance_to(self, sim_object): """Gets the distance to another simobject returns distance in meters or None if not in contact""" ox, oy, ot = self.get_pose() min_distance = None for px, py in self.get_contact_points(sim_object): distance = sqrt((px-ox)*(px-ox)+(py-oy)*(py-oy)) if min_distance is not None: if distance < min_distance: min_distance = distance else: min_distance = distance return min_distance