def test_sim(self):
print("Initialising rotation of ", self.angle*180/pi, "degres")
app = AppAreneThread(self.arene)
s = Simulation(strategies=[self.strat])
v = self.strat.robot.direction.clone()
s.start()
app.start()
self.strat.robot.set_wheels_rotation(1, 40)
self.strat.robot.set_wheels_rotation(2, 60)
sens = self.strat.sens
precision = max(self.strat.robot.get_wheels_rotations(1), self.strat.robot.get_wheels_rotations(2)) * (
pi / 180) * PAS_TEMPS * (max(self.strat.robot.rd.diametre,
self.strat.robot.rg.diametre) / self.strat.robot.dist_wheels)
while not s.stop:
pass
app.stop()
if sens > 0:
angle = abs((2 * pi) - v.diff_angle(self.strat.robot.direction, 1))
else:
angle = abs(v.diff_angle(self.strat.robot.direction, 1))
print("Error: ", abs(abs(self.angle) - angle)*(180/pi), " degres")
print("Maximal error expected: ", precision*180/pi, "degres")
self.assertLess(abs(abs(self.angle) - angle), precision)
def test_diametre(self):
"""
Initialise une trajectoire circulaire, et calcule le diamètre de celle-ci
"""
print("Testing diametre trajectory...")
app = AppAreneThread(self.arene)
sim = Simulation(strategies=[self.strat], acceleration_factor=5)
min_x, min_y = sim.strategie.robot.centre.x, sim.strategie.robot.centre.y
max_x, max_y = sim.strategie.robot.centre.x, sim.strategie.robot.centre.y
sim.start()
while not sim.stop:
sleep(PAS_TEMPS/5)
if sim.strategie.robot.centre.x < min_x:
min_x = sim.strategie.robot.centre.x
if sim.strategie.robot.centre.x > max_x:
max_x = sim.strategie.robot.centre.x
if sim.strategie.robot.centre.y < min_y:
min_y = sim.strategie.robot.centre.y
if sim.strategie.robot.centre.y > max_y:
max_y = sim.strategie.robot.centre.y
pass
self.assertLess((max_y-min_y)-self.diametre_cercle, 0.0001)
print("Done")
def test_vis(self):
td = Thread(target=self.strat.start_3D)
sim = Simulation(strategies=[self.strat])
td.start()
sim.start()
sleep(15)
self.strat.stop_3D()
def test_visualisation_2D(self):
"""
Visualise le déplacement circulaire en 2D
"""
print("Testing 2D...")
app = AppAreneThread(self.arene)
sim = Simulation(strategies=[self.strat], tic=2, tic_display=[self.strat.robot.tete.sensors["acc"]], final_actions=[app.stop])
sim.start()
app.start()
def test_target_circle_trajectectory(self):
self.strat = DroitVersBaliseVision(RobotMotorise(forme=Pave(1,1,1,self.p0.clone()), direction=self.v2.clone()), AreneFermee(10,10,10))
self.strat2 = DeplacementCercle(robot=self.target,vitesse=30, diametre=3, angle_max=360)
td = Thread(target=self.strat.start_3D)
sim = Simulation(strategies=[self.strat2, self.strat], tmax=15, tic=1,tic_display=[self.strat], final_actions=[self.strat.stop_3D])
td.start()
while not self.strat.robot.tete.sensors["cam"].is_set:
pass
sim.start()
while not sim.stop:
pass
self.assertNotEqual(self.strat.last_detected, None)
def test_target_straith_trajectory(self):
self.strat2 = DeplacementDroit(robot=self.target,vitesse= 30, distance_max=2)
td = Thread(target=self.strat.start_3D)
sim = Simulation(strategies=[self.strat2, self.strat], tmax=15, tic=3,
tic_display=[self.strat],
final_actions=[self.strat.stop_3D])
td.start()
while not self.strat.robot.tete.sensors["cam"].is_set:
pass
sim.start()
while not sim.stop:
pass
def test_vis(self):
sim = Simulation(strategies=[self.strat],
tmax=10,
final_actions=[self.strat.stop_3D],
tic=2,
tic_display=[self.strat])
thd = Thread(target=self.strat.start_3D)
thd.start()
while not self.strat.robot.tete.sensors["cam"].is_set:
pass
sim.start()
def test_sim_2D(self):
"""
Teste la distance parcourue lors d'un déplacement droit
"""
p = self.strat.robot.centre.clone()
sim = Simulation(strategies=[self.strat],
tic=2,
tic_display=[self.strat.robot.tete.sensors["acc"]])
app = AppAreneThread(self.arene)
sim.start()
app.start()
dps_wheels = self.strat.robot.get_wheels_rotations(3)
while not sim.stop:
pass
dist = (self.strat.robot.centre - p).to_vect().get_mag()
# On s'assure qu'on a la précision souhaitée
self.assertLess(
abs(dist - self.distance),
abs(dps_wheels[0]) * (pi / 180) * PAS_TEMPS *
self.strat.robot.rd.diametre / 2)
def test_detection_2D(self):
print("Testing direct detection with infrared ray...")
td = Thread(target=self.strat.robot.tete.sensors["cam"].run)
app = AppAreneThread(self.arene)
sim = Simulation(strategies=[self.strat],
final_actions=[app.stop],
tic=2,
tic_display=[
(self.strat.robot.tete.sensors["ir"].arena_v,
"-v")
])
self.strat.init_movement(3, 60)
sim.start()
#self.strat.start()
app.start()
while not sim.stop:
pass
#self.strat.robot.tete.sensors["cam"].stop()
# On s'assure que le pavé est bien entré dans le champ d'alerte
dist = (self.p.centre - self.strat.robot.centre
).to_vect().get_mag() - self.p.length / 2
self.assertLess(dist, self.strat.proximite_max,
"Object detected too close")
# On s'assure qu'on a détecté la limite avec une assez bonne précision
diff = abs(self.strat.last_detected - dist)
# err_max n'est valide que si le pavé est droit par rapport à la direction du robot
err_max = max(
self.strat.robot.get_wheels_rotations(1) * (pi / 180) *
self.strat.robot.rd.diametre * PAS_TEMPS, PAS_IR)
self.assertLess(diff, err_max, "Maximal detection error exceeded")
print("Done")
def __init__(self, arene):
Thread.__init__(self)
self.arene = arene
self.window = None
def run(self):
self.window = AppArene(self.arene)
self.window.mainloop()
def stop(self):
self.window.quit()
if __name__ == '__main__':
from gl_lib.sim.robot.strategy.deplacement import DeplacementDroitAmeliore
from gl_lib.sim.robot import RobotMotorise
from gl_lib.sim import Simulation
from gl_lib.sim.geometry import Vecteur
a = Arene()
strat = DeplacementDroitAmeliore(
robot=RobotMotorise(direction=Vecteur(1, 1, 0).norm()),
distance_max=1,
arene=a)
a.add(strat.robot)
sim = Simulation(strategies=[strat])
app = AppAreneThread(strat.arene)
app.start()
sim.start()
p.vertices[l_ln[l][3]].y,
p.vertices[l_ln[l][3]].z,
)
lvertices.append(vertices)
return lvertices
if __name__ == '__main__':
from gl_lib.sim import Simulation
from gl_lib.sim.robot.strategy.deplacement.balise import DroitVersBaliseVision
from gl_lib.sim.robot.sensor.camera import Balise
import os
from math import pi
a = AreneFermee(15, 15, 15)
p1 = Pave(1, 1, 1)
w, l, h = a.width, a.length, a.height
p_w2, p_l2, p_h2 = p1.width / 2, p1.length / 2, p1.height / 2
v = Vecteur(1, 1, 0).norm() * -1
r = RobotTarget(direction=v.clone(), balise=Balise())
r2 = RobotTarget(pave=Pave(1, 1, 1, (v * 5).to_point()),
direction=v.clone())
r.set_wheels_rotation(3, 0)
r2.set_wheels_rotation(0, 0)
print(os.getcwd())
#a.add(r2)
s = Simulation(DroitVersBaliseVision(r, a))
s.start()