def _manual_run(self):
pthjoin = os.path.join
(translation_model, translation_data, rotation_model,
rotation_data, measurement_model, measurement_data, beacons) = models.load_data(pthjoin('..','Data','001'))
# Kluge fix -- need to fix the .mat file
measurement_model = measurement_model[0]
sm = Sensor.SensorManager()
# Create Beacons
# Need to add draw beacon functions
total_beacons = beacons.shape[0]
for i in range(total_beacons):
sm.add_sensor(Sensor.BeaconSensor(measurement_model[0], measurement_model[1],
[0,10000], beacons[i][0], beacons[i][1]))
sm.add_sensor(Sensor.CompassSensor(0, np.deg2rad(5), [0, np.deg2rad(359.9)]))
sm.add_sensor(Sensor.Trilateration2DSensor(None, np.eye(2)*measurement_model[1], None))
# Move Straight: Vector based on Motion Model measurements
#translation_vec = [dist_hypot, dist_hypot, translation_model[2,0]]
translation_vec = translation_model[:,0]
translation_cov = np.cov(translation_data.T)
#translation_mov = Movement.Movement(translation_vec, translation_cov)
# Turn: Vector based on Motion Model measurements
left_rotation_vec = rotation_model[:,0]
right_rotation_vec = rotation_model[:,0] * -1
# May want to change this. This is a guess to get it working.
# Playing around with cov(rotation_data) may lead to better results.
rotation_cov = np.zeros([3,3])
rotation_cov[2,2] = rotation_model[2,1]
#left_rot_mov = Movement.Movement(left_rotation_vec, rotation_cov)
#right_rot_mov = Movement.Movement(right_rotation_vec, rotation_cov)
#turn_leniency = np.deg2rad(10) # Used as an error judgement for deciding when to turn
print 'D.TF.284'
explorer_pos = self.fm.get_explorer_pos_mean()
me = MoveExplorer(explorer_pos, [12, 12, np.deg2rad(6)], debug=True,
translation_moves=[Movement(translation_vec, translation_cov),
Movement(np.array([10, 0.5, np.deg2rad(0.5)]), np.eye(3, dtype=np.float32) * 0.0001),
Movement(np.array([50, 1, np.deg2rad(0.7)]), np.eye(3, dtype=np.float32) * 0.0003)],
rotation_moves=[Movement(right_rotation_vec, rotation_cov),
Movement(left_rotation_vec, rotation_cov),
Movement(np.array([0, 0, np.deg2rad(5)]), rotation_cov),
Movement(np.array([0, 0, np.deg2rad(-5)]), rotation_cov)])
def draw_all_waypoints(cr):
#cr.select_font_face('arial')
cr.set_font_size(15)
#cr.set_font_matrix(cairo.Matrix(1, 0, 0, -1, 0, 0))
cr.new_path()
def draw_waypoints(cr, i, waypoints, rgba_color):
for pnt in waypoints:
cr.set_source_rgba(*rgba_color)
cr.arc(pnt[0], pnt[1], 6, 0, 2 * np.pi)
cr.stroke()
cr.move_to(pnt[0], pnt[1])
#cr.show_text(str(i))
#i += 1
waypoints = me.get_old_waypoints()[-2:]
draw_waypoints(cr, -2, waypoints, (0.5, 0, 0, 0.8))
waypoints = me.get_current_waypoints()
draw_waypoints(cr, 1, waypoints[0:1], (0.4, 0.9, 0.8, 0.8))
if len(waypoints) > 1:
draw_waypoints(cr, 2, waypoints[1:], (0, 0, 0.5, 0.8))
cr.new_path()
#=======================================================================
# Add Drawing Methods
#=======================================================================
self.tg.add_draw_method(draw_all_waypoints)
for beacon in sm.sensors_by_type['Beacon']:
self.tg.add_draw_method(beacon.draw)
accumulate_distance = np.array([0, 0, 0], np.float32)
while not self.quit:
# Poll for any new user input.
waypoints = tg.get_click_positions()
for pnt in waypoints:
me.move_to(pnt[0], pnt[1])
transition_mov = me.get_next_move(explorer_pos)
if transition_mov is not None:
print 'D.TF.337'
explorer_pos = self.fm.get_explorer_pos_mean()
self.fm.move(transition_mov.vec, transition_mov.cov)
accumulate_distance += np.abs(transition_mov.vec)
time.sleep(0.5)
# Only perform filter observation when enough translation has occurred.
# Observations must be independent.
if accumulate_distance[0] >= 20:
accumulate_distance[0] = 0
# Testing Trilateration
beacon_sensors = sm.sensors_by_type['Beacon']
print 'D.TF.351'
prob_pos = self.fm.get_explorer_pos_mean()
for j in range(total_beacons):
#prob_pos = self.fm.get_explorer_pos_mean()
obs_dis = np.sqrt((beacons[j][0] - prob_pos[0])**2 + (beacons[j,1] - prob_pos[1])**2) + (randn() * measurement_model[1]) + measurement_model[0]
#self.fm.observation(obs_dis, sm.sensors_by_type['Beacon'][j])
beacon_sensors[j].obs = obs_dis
self.fm.observation(beacon_sensors, sm.sensors_by_type['Trilateration2D'][0])
if accumulate_distance[2] >= np.deg2rad(10):
print 'D.TF.364'
explorer_pos = self.fm.get_explorer_pos_mean()
#self.fm.observation(explorer_pos[2] + (randn() * sm.sensors_by_type['Compass'][0].variance), sm.sensors_by_type['Compass'][0])
accumulate_distance[2] = 0
time.sleep(0.1)
def __init__(self,
serial_port,
baud_rate,
map_obj,
origin_pos,
origin_cov,
gui=None,
run_pf=True,
run_kf=True):
'''
@param serial_port:
@type serial_port:
@param baud_rate:
@type baud_rate:
@param map_obj:
@type map_obj:
@param origin_pos:
@type origin_pos:
@param origin_cov:
@type origin_cov:
@param gui:
@type gui:
@param run_pf:
@type run_pf:
@param run_kf:
@type run_kf:
'''
super(FilterSystemRunner, self).__init__()
self.quit = False
self.allow_move_timer = 0
#=======================================================================
# UART
#=======================================================================
self.cli = FilterCLI.FilterCLI()
self.ua = UARTSystem.UART(serial_port,
baud_rate,
uart_input=self.cli,
approve=True,
debug=True)
self.uin = [] # uart input from user
self.uout = [] # uart output from base/roomba
#=======================================================================
# Filter
#=======================================================================
self.fm = FilterManager.FilterManager()
if run_kf:
self.fm.add_filter(
KalmanFilter.KalmanFilter(origin_pos, origin_cov))
if run_pf:
self.fm.add_filter(
ParticleFilter.ParticleFilter(origin_pos, origin_cov))
#=======================================================================
# Data
#=======================================================================
pthjoin = os.path.join
(self.translation_model, self.translation_data, self.rotation_model,
self.rotation_data, self.measurement_model, self.measurement_data,
self.beacons) = models.load_data(pthjoin('Data', '001'))
#=======================================================================
# Sensors
#=======================================================================
# Kluge fix -- need to fix the .mat file
measurement_model = self.measurement_model[0]
self.sm = Sensor.SensorManager()
# Create Beacons
# Need to add draw beacon functions
total_beacons = self.beacons.shape[0]
for i in range(total_beacons):
self.sm.add_sensor(
Sensor.BeaconSensor(measurement_model[0], measurement_model[1],
[0, 10000], self.beacons[i][0],
self.beacons[i][1]))
# Trilateration will be used if 2 or more beacons yield readings.
self.sm.add_sensor(
Sensor.Trilateration2DSensor(measurement_model[0],
np.eye(2) * 15,
None)) #measurement_model[1], None))
#=======================================================================
# Movement
#=======================================================================
self._new_moves = []
self._last_move = None
# Move Straight: Vector based on Motion Model measurements
translation_vec = self.translation_model[:, 0]
translation_vec[:2] = np.array([100.4, 4])
translation_cov = np.cov(self.translation_data.T)
translation_mov = Movement.Movement(translation_vec,
translation_cov,
id='0 100\n')
# Bit hacky here, did not do real measurements, just based off eye measurements
translation_moves = [
translation_mov,
Movement.Movement(np.array([10.1, 0.5, np.deg2rad(0.2)]),
np.array([[0.5, 0, 0], [0, 1, 0],
[0, 0, np.deg2rad(0.6)]]),
id='0 10\n'), #np.eye(3) * .0001,
Movement.Movement(np.array([50.2, 2, np.deg2rad(0.6)]),
np.array([[0.5, 0, 0], [0, 1, 0],
[0, 0, np.deg2rad(0.6)]]),
id='0 50\n')
] #np.array([[],[],[]]))]
# Turn: Vector based on Motion Model measurements
left_rotation_vec = self.rotation_model[:, 0]
right_rotation_vec = self.rotation_model[:, 0] * -1
# May want to change this. This is a guess to get it working.
# Playing around with cov(rotation_data) may lead to better results.
rotation_cov = np.zeros([3, 3])
rotation_cov[2, 2] = self.rotation_model[2, 1]
left_rot_mov = Movement.Movement(left_rotation_vec,
rotation_cov,
id='10 0\n')
right_rot_mov = Movement.Movement(right_rotation_vec,
rotation_cov,
id='-10 0\n')
rotation_moves = [
left_rot_mov, right_rot_mov,
Movement.Movement(np.array([0, 0, np.deg2rad(-5)]),
rotation_cov,
id='-4 0\n'),
Movement.Movement(np.array([0, 0, np.deg2rad(5)]),
rotation_cov,
id='4 0\n')
]
explorer_pos = self.fm.get_explorer_pos_mean()
self.me = Movement.MoveExplorer(explorer_pos,
[11, 11, np.deg2rad(6)],
translation_moves=translation_moves,
rotation_moves=rotation_moves)
self._accumulate_distance = np.array([0, 0, 0], np.float32)
#=======================================================================
# Map
#=======================================================================
self.map_obj = map_obj
#=======================================================================
# GUI
#=======================================================================
self.rg = None
if gui is not None:
self.rg = gui
self.rg.add_filter_draw_method(self.fm.get_draw())
for beacon in self.sm.sensors_by_type['Beacon']:
self.rg.add_draw_method(beacon.draw)
self.rg.add_draw_method(self._draw_all_waypoints)
