def test_stopwatch(self):
sw = StopWatch()
self.assertEqual(str(sw), "StopWatch: 00:00:00.000")
sw = StopWatch(desc="test sw")
self.assertEqual(str(sw), "test sw: 00:00:00.000")
self.assertEqual(sw.is_started, False)
sw.start()
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(1500)
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 1500)
self.assertEqual(sw.value_secs, 1.5)
self.assertEqual(sw.value_hms, (0, 0, 1, 500))
self.assertEqual(sw.hms_str, "00:00:01.500")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(3000)
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 3000)
self.assertEqual(sw.value_secs, 3)
self.assertEqual(sw.value_hms, (0, 0, 3, 0))
self.assertEqual(sw.hms_str, "00:00:03.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
set_mock_ticks_ms(1000 * 60 * 75.5) #75.5 minutes
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 1000 * 60 * 75.5)
self.assertEqual(sw.value_secs, 60 * 75.5)
self.assertEqual(sw.value_hms, (1, 15, 30, 0))
self.assertEqual(sw.hms_str, "01:15:30.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
try:
# StopWatch can't be started if already running
sw.start()
self.fail()
except StopWatchAlreadyStartedException:
pass
# test reset behavior
sw.restart()
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(1000 * 60 * 75.5 + 3000)
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 3000)
self.assertEqual(sw.value_secs, 3)
self.assertEqual(sw.value_hms, (0, 0, 3, 0))
self.assertEqual(sw.hms_str, "00:00:03.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
# test stop
sw.stop()
set_mock_ticks_ms(1000 * 60 * 75.5 + 10000)
self.assertEqual(sw.is_started, False)
self.assertEqual(sw.value_ms, 3000)
self.assertEqual(sw.value_secs, 3)
self.assertEqual(sw.value_hms, (0, 0, 3, 0))
self.assertEqual(sw.hms_str, "00:00:03.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
# test reset
sw.reset()
self.assertEqual(sw.is_started, False)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(1000 * 60 * 75.5 + 6000)
self.assertEqual(sw.is_started, False)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
class navegacion(localizacion):
def __init__(self, motor_izquierdo, motor_derecho, radio_rueda,
separacion_ruedas, posicion, nav_sonar, nav_sensor_color):
localizacion.__init__(self, motor_izquierdo, motor_derecho,
radio_rueda, separacion_ruedas, posicion,
nav_sonar, nav_sensor_color)
#Fichero
self.f = None
self.escribir_fichero_activo = False
self.fin_escribir_fichero = True
self.t = StopWatch()
def coordenadas_global_a_robot(self, robot, punto_global):
R = np.array([[cos(robot[3][0]), -sin(robot[3][0]), 0.0],
[sin(robot[3][0]),
cos(robot[3][0]), 0.0], [0.0, 0.0, 1.0]])
Rt = R.transpose()
aux = -(Rt @ robot[:3])
T = np.array([[Rt[0][0], Rt[0][1], Rt[0][2], aux[0][0]],
[Rt[1][0], Rt[1][1], Rt[1][2], aux[1][0]],
[Rt[2][0], Rt[2][1], Rt[2][2], aux[2][0]], [0, 0, 0, 1]])
p = np.array([[punto_global[0][0]], [punto_global[1][0]],
[punto_global[2][0]], [1]])
resultado = T @ p
return resultado[:3]
def navegacion_planificada(self, puntos_objetivos, KW):
vector_hasta_destino = np.array([0.0, 0.0, 0.0])
for punto in puntos_objetivos:
while 1:
robot = self.odometria("RK4")
vector_hasta_destino[0] = punto[0] - robot[0][0]
vector_hasta_destino[1] = punto[1] - robot[1][0]
vector_hasta_destino[2] = punto[2] - robot[2][0]
modulo = sqrt(vector_hasta_destino @ vector_hasta_destino.T)
if (modulo <= 0.05):
sound.beep()
break
angulo_objetivo = atan2(vector_hasta_destino[1],
vector_hasta_destino[0])
if angulo_objetivo < 0:
angulo_objetivo = angulo_objetivo + 2 * pi
angulo_robot = robot[3][0]
while angulo_robot > 2 * pi:
angulo_robot = angulo_robot - 2 * pi
while angulo_robot < 0:
angulo_robot = angulo_robot + 2 * pi
angulo = angulo_objetivo - angulo_robot
if angulo < -pi:
angulo = angulo + 2 * pi
if angulo > pi:
angulo = -(2 * pi - angulo)
w = KW * angulo
if w > pi:
w = pi
if w < -pi:
w = -pi
self.correr(0.2, w)
self.parar()
def navegacion_reactiva_campos_virtuales(self, objetivo, rTs, KA, KR):
vector_resultante = np.array([0.0, 0.0, 0.0])
while 1:
vector_hasta_destino = self.coordenadas_global_a_robot(
self.odometria("RK4"), objetivo)
vector_de_repulsion = np.array([[0.0], [0.0], [0.0]])
modulo = sqrt(vector_hasta_destino[0].T @ vector_hasta_destino[0])
if (modulo <= 0.05):
break
obstaculo = rTs @ np.array([[self.s.distancia_sonar], [0.0], [0.0],
[1.0]])
modulo = sqrt(obstaculo[:3][0].T @ obstaculo[:3][0])
if modulo > 0.45:
vector_de_repulsion[0][0] = 0.0
vector_de_repulsion[1][0] = 0.0
vector_de_repulsion[2][0] = 0.0
else:
vector_de_repulsion = ((0.45 - modulo) / 0.45) * obstaculo
vector_resultante[0] = KA * vector_hasta_destino[0][
0] - KR * vector_de_repulsion[0][0]
vector_resultante[1] = KA * vector_hasta_destino[1][
0] - KR * vector_de_repulsion[1][0]
vector_resultante[2] = KA * vector_hasta_destino[2][
0] - KR * vector_de_repulsion[2][0]
v = 0.2 * vector_resultante[0]
w = 2 * vector_resultante[1]
if (v > 0.2):
v = 0.2
if (v < -0.2):
v = -0.2
if (w > pi):
w = pi
if (w < -pi):
w = -pi
self.correr(v, w)
self.parar()
#Guardar datos
def empezar_fichero(self, nombre_fichero, tiempo, *args, **kwargs):
def hilo_fichero():
self.t.start()
while self.escribir_fichero_activo:
for l in args:
if l == "tiempo":
self.f.write(str(self.t.value_ms / 1000) + " ")
elif l == "distancia_sonar":
self.f.write(str(self.s.distancia_sonar) + " ")
elif l == "otros_sensores_presentes":
self.f.write(
str(self.s.otros_sensores_presentes) + " ")
elif l == "color":
self.f.write(str(self.s.color) + " ")
elif l == "nombre_color":
self.f.write(str(self.s.nombre_color) + " ")
elif l == "ambiente":
self.f.write(str(self.s.ambiente) + " ")
elif l == "reflexion":
self.f.write(str(self.s.reflexion) + " ")
elif l == "rgb":
self.f.write(str(self.s.rgb) + " ")
elif l == "voltaje_bateria":
self.f.write(str(self.s.voltaje_bateria) + " ")
elif l == "corriente_bateria":
self.f.write(str(self.s.corriente_bateria) + " ")
elif l == "w_motor_derecho":
self.f.write(str(self.w_motor_derecho) + " ")
elif l == "w_motor_izquierdo":
self.f.write(str(self.w_motor_izquierdo) + " ")
elif l == "dc_motor_izquierdo":
self.f.write(str(self.dc_motor_izquierdo) + " ")
elif l == "dc_motor_derecho":
self.f.write(str(self.dc_motor_derecho) + " ")
elif l == "posicion_motor_derecho":
self.f.write(str(self.posicion_motor_derecho) + " ")
elif l == "velocidad_linear":
self.f.write(str(self.velocidad_linear) + " ")
elif l == "velocidad_angular":
self.f.write(str(self.velocidad_angular) + " ")
elif l == "odometria":
if "modo" in kwargs:
posicion = self.odometria(kwargs["modo"])
self.f.write(
str(posicion[0][0]) + " " +
str(posicion[1][0]) + " " +
str(posicion[2][0]) + " " +
str(posicion[3][0]) + " ")
elif l == "localizacion_probabilistica":
if ("mapa" in kwargs) and ("rox" in kwargs) and (
"roy" in kwargs) and (
"rotheta" in kwargs) and ("Rk" in kwargs):
mu, sigma = self.localizacion_probabilistica(
kwargs["mapa"], kwargs["rox"], kwargs["roy"],
kwargs["rotheta"], kwargs["Rk"])
self.f.write(
str(mu[0][0]) + " " + str(mu[1][0]) + " " +
str(mu[2][0]) + " " + str(sigma[0][0]) + " " +
str(sigma[0][1]) + " " + str(sigma[0][2]) +
" " + str(sigma[1][0]) + " " +
str(sigma[1][1]) + " " + str(sigma[1][2]) +
" " + str(sigma[2][0]) + " " +
str(sigma[2][1]) + " " + str(sigma[2][2]) +
" ")
self.f.write("\n")
sleep(tiempo)
self.t.stop()
self.fin_escribir_fichero = True
self.f = open(nombre_fichero, "w")
self.escribir_fichero_activo = True
self.fin_escribir_fichero = False
self.id_hilo_fichero = Thread(target=hilo_fichero)
self.id_hilo_fichero.start()
def parar_fichero(self):
self.escribir_fichero_activo = False
if not self.fin_escribir_fichero:
self.id_hilo_fichero.join(timeout=None)
self.f.close()
