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)