def change_values(massage, pid1, pid2, pid3, pid4, pid5, pid6, clientID):
while 1:
mass = massage.get_massage()
if mass[0] == 1:
res = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
massage.send_massage(commands['sensor_ready'], 1, IP['manager'], ports['manager'])
elif mass[0] == 17:
sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot_wait)
start_sim = False
elif mass[0] == commands['stop_sim']:
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
elif mass[0] == 19:
print('seted')
k_p, k_i, k_d, k_v = mass[1::]
pid1.set_params(k_p=k_p, k_i=k_i, k_d=k_d, k_v=k_v)
elif mass[0] == 20:
k_p, k_i, k_d = mass[1::]
pid2.set_params(k_p=k_p, k_i=k_i, k_d=k_d, k_v=0)
elif mass[0] == 21:
k_p, k_i, k_d = mass[1::]
pid3.set_params(k_p=k_p, k_i=k_i, k_d=k_d, k_v=0)
elif mass[0] == 22:
k_p, k_i, k_d = mass[1::]
pid4.set_params(k_p=k_p, k_i=k_i, k_d=k_d, k_v=0)
elif mass[0] == 25:
k_p, k_i, k_d = mass[1::]
pid5.set_params(k_p=k_p, k_i=k_i, k_d=k_d, k_v=0)
elif mass[0] == 26:
k_p, k_i, k_d = mass[1::]
pid6.set_params(k_p=k_p, k_i=k_i, k_d=k_d, k_v=0)
def cleanup(self):
# stop vehicle from moving
for i in range(5):
self.setSpeed("left", 0)
self.setSpeed("right", 0)
# close all queues
for _, q in self.queues.items():
q.close()
# pause simulation and break connection
sim.simxPauseSimulation(self.clientID, sim.simx_opmode_oneshot)
sim.simxFinish(-1)
cv2.destroyAllWindows()
def pause():
global client, playing
if client == None:
return "-1"
response = sim.simxPauseSimulation(client, sim.simx_opmode_blocking)
while response == 1:
verboseResp(response, "simxPauseSimulation")
response = sim.simxPauseSimulation(client, sim.simx_opmode_blocking)
sleep(againIn)
if response == 0:
playing = False
verboseResp(response, "simxPauseSimulation")
return str(response)
def monitor_eval(client, lock):
""" Threaded function to start monitoring of simulation"""
then = time()
time_passed = 0
step_counter = 0
eval_done = False
while not eval_done:
now = time()
time_passed = now - then
if time_passed > timeout_one_gen:
lock.acquire()
print("client", client.clientID, "evaluation timed out")
lock.release()
sim.simxPauseSimulation(client.clientID, mode1)
if client.status == 1:
client.update_fitness_scores(print_report=False)
eval_done = True
else:
if step_counter % 3 == 0:
eval_done = True
client.update_status()
if client.is_online:
if client.status != 2 or time_passed < 1.0:
client.update_fitness_scores(print_report=False)
eval_done = False
else:
client.update_fitness_scores(
print_report=False) # fitness_report_please[i])
else:
if client.was_online:
lock.acquire()
print("client", client.clientID,
"went offline during evaluation.")
lock.release()
client.was_online = False
pass #client.ignore
sleep(0.3)
client.stop_fitness_streaming()
if client.is_online:
client.reset()
client.send_master_status(-1)
if verbose:
lock.acquire()
print("client", client.clientID, "evaluation finished")
lock.release()
sim.simxAddStatusbarMessage(
clientID,
'A posição em x é ' + str(xr) + ' e a posição em y é ' + str(yr),
sim.simx_opmode_oneshot_wait)
# Stop simulation:
# sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot_wait)
# Pause simulation
vref = 0
sim.simxPauseCommunication(clientID, True)
sim.simxSetJointTargetVelocity(clientID, robotRightMotor, vref,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, vref,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, robotRightMotorB, vref,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, robotLeftMotorB, vref,
sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot_wait)
# Now close the connection to V-REP:
sim.simxAddStatusbarMessage(clientID, 'Programa pausado',
sim.simx_opmode_blocking)
sim.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
def main():
print('Program started')
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart(COPPELIA_SIM_IP_ADDRESS, COPPELIA_SIM_PORT, True,
True, 2000, 5) # Connect to CoppeliaSim
# lower e upper são vetores pra detecção da cor vermelha, cada elemento do vetor é um dos canais RGB (invertido por causa do opencv).
# então o limite inferior pra detecção seria o valor 0 para a cor azul, 0 para a cor verde e 100 para a cor vermelha.
# a análise é análoga para o vetor upper.
lower = np.uint8([0, 0, 100])
upper = np.uint8([60, 60, 255])
# nós temos 8 targets, portanto para pegar as suas referências (handles), é preciso iterar sobre eles, daí vem o targetIterator.
targetIterator = 1
if (clientID != -1):
print('Connection Successful')
# pega as referências de todos os elementos a serem manipulados.
# perceba que só pega a referência do primeiro alvo porque só pegamos a referência de um alvo por vez.
err1, ddRobotHandle = sim.simxGetObjectHandle(
clientID, 'RobotFrame#', sim.simx_opmode_oneshot_wait)
err2, leftMotorHandle = sim.simxGetObjectHandle(
clientID, 'LeftMotor#', sim.simx_opmode_oneshot_wait)
err3, rightMotorHandle = sim.simxGetObjectHandle(
clientID, 'RightMotor#', sim.simx_opmode_oneshot_wait)
err4, revoluteJointHandle = sim.simxGetObjectHandle(
clientID, 'RevoluteJoint#', sim.simx_opmode_oneshot_wait)
err5, visionSensorHandle = sim.simxGetObjectHandle(
clientID, 'VisionSensor#', sim.simx_opmode_oneshot_wait)
err6, targetHandle = sim.simxGetObjectHandle(
clientID, 'Target' + str(targetIterator) + '#',
sim.simx_opmode_oneshot_wait)
targetIterator += 1
# pega a referência dos sensores ultrassônicos (usamos 7 sensores).
# atenção: sensores ultrassônicos != vision sensor
usensors = []
for i in range(1, 8):
err, usensor = sim.simxGetObjectHandle(
clientID, 'Proximity_sensor' + str(i) + '#',
sim.simx_opmode_oneshot_wait)
usensors.append(usensor)
# a leitura dos sensores não pode bloquear o loop de execução do robô (while mais abaixo), portanto o coppelia oferece o opmode streaming,
# isto é, quando você manda a leitura começar a ser feita com streaming, ele mantém-se fazendo a leitura e colocando os resultados em um buffer
# exclusivo para aquele handle (7 handles 7 buffers). Portanto só precisamos a cada iteração ler do buffer, e aí não é preciso pedir que o simulador
# faça uma nova leitura, economizando tempo e não bloqueando o loop de execução do robô.
for i in range(7):
x, a, b, c, d = sim.simxReadProximitySensor(
clientID, usensors[i], sim.simx_opmode_streaming)
# a mesma regra dos sensores ultrassônicos descrita acima se aplica ao vision sensor (e a qualquer sensor que poderia vir a ser usado).
a, b, image = sim.simxGetVisionSensorImage(clientID,
visionSensorHandle, 0,
sim.simx_opmode_streaming)
print(f'RobotFrame: {ddRobotHandle}')
print(f'LeftMotor: {leftMotorHandle}')
print(f'RightMotor: {rightMotorHandle}')
print(f'RevoluteJoint: {revoluteJointHandle}')
print(f'VisionSensor: {visionSensorHandle}')
ret = sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait)
if (ret == -1):
print('Connection Failed.')
exit(-1)
print('Simulation Started')
while (sim.simxGetConnectionId(clientID) != -1):
# posicao atual do robo
pos = getPosition(clientID, ddRobotHandle)
#print(f'posX: {pos[0]:.3f} posY: {pos[1]:.3f}', end=' ')
# target atual do robo
goal = getPosition(clientID, targetHandle)
#print(f'goalX: {goal[0]:.3f} goalY: {goal[1]:.3f}', end=' ')
# observe o simx_opmode_buffer. nós não pedimos uma nova leitura do vision sensor, nós simplesmente pegamos a leitura do buffer.
a, b, image = sim.simxGetVisionSensorImage(clientID,
visionSensorHandle, 0,
sim.simx_opmode_buffer)
# operações feitas para organizar a imagem do vision sensor.
newImage = np.uint8(image).reshape((b[0], b[0], 3))
# definitiveImage é autoexplicativa, mas atenção, pois ela se encontra com a codificação BGR.
definitiveImage = cv2.cvtColor(np.flip(newImage, 0),
cv2.COLOR_RGB2BGR)
# mask é simplesmente uma outra imagem com a mesma resolução de definitiveImage, porém só mostrando objetos vermelhos, isto é,
# valor 255 nos pixels correspondentes a pixels vermelhos na imagem original, e 0 em todos os outros pixels.
mask = cv2.inRange(definitiveImage, lower, upper)
# distancia do alvo para o robô.
dx = goal[0] - pos[0]
dy = goal[1] - pos[1]
euclid = math.sqrt(dx * dx + dy * dy)
# print(euclid)
# se o alvo está perto o suficiente faça:
if (euclid <= 0.23):
# se na mask só tem pixels diferentes de 0, portanto só pixels vermelhos faça:
if (0.0 not in mask):
# para o robô e manda a hélice girar no sentido anti-horário por aproximadamente três segundos.
phiL, phiR, phiH = 0, 0, 2
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
time.sleep(3)
setTargetSpeed(clientID, phiL, phiR, 0, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
# se não, o alvo é azul.
else:
# então para o robô e manda a hélice girar no sentido horário por aproximadamente três segundos.
phiL, phiR, phiH = 0, 0, -2
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
time.sleep(3)
setTargetSpeed(clientID, phiL, phiR, 0, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
#se ainda há alvos
if (targetIterator <= 8):
# pega a referência do próximo alvo.
err6, targetHandle = sim.simxGetObjectHandle(
clientID, 'Target' + str(targetIterator) + '#',
sim.simx_opmode_oneshot_wait)
targetIterator += 1
# se não encerra o programa e pausa a simulação.
else:
break
# se o alvo ainda está longe, continua indo até ele com a hélice parada.
else:
phiL, phiR = movementControl(pos, goal, clientID, usensors)
phiH = 0
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
# descomente estas duas linhas se você quiser salvar as imagens capturadas.
# imageItarator = 0
#cv2.imwrite('capture' + str(imageIterator) + '.png', definitiveImage)
#imageIterator += 1
setTargetSpeed(clientID, phiL, phiR, phiH, leftMotorHandle,
rightMotorHandle, revoluteJointHandle)
sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot_wait)
sim.simxFinish(clientID)
else:
print('Connection failed.')
exit(-2)
def pause(self):
s.simxPauseSimulation(self._id, self._def_op_mode)
