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)
