def verificaConexao(self): conectado = sim.simxGetConnectionId(self.id) != -1 while not conectado: sim.simxFinish(-1) self.id = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim conectado = sim.simxGetConnectionId(self.id) != -1 return conectado
def vision(): while (sim.simxGetConnectionId(clientID) != -1): err, resolution, image = sim.simxGetVisionSensorImage( clientID, v1, 0, sim.simx_opmode_buffer) if err == sim.simx_return_ok: img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) # image was originally upside down, turn it 180 degree img180 = cv2.flip(img, 0) #convert image from bgr -> rgb imgfinal = cv2.cvtColor(img180, cv2.COLOR_RGB2BGR) # show image cv2.imshow('image', imgfinal) # press 'q' to exit if cv2.waitKey(1) & 0xFF == ord('q'): break elif err == sim.simx_return_novalue_flag: print('no image') pass else: print(err) else: print("Failed to connect to remote API Server") sim.simxFinish(clientID)
def main(): print('### Program started') print('### Number of arguments:', len(sys.argv), 'arguments.') print('### Argument List:', str(sys.argv)) sim.simxFinish(-1) # just in case, close all opened connections port = int(sys.argv[1]) clientID = sim.simxStart('127.0.0.1', port, True, True, 2000, 5) if clientID == -1: print('### Failed connecting to remote API server') else: print('### Connected to remote API server') hRobot = getRobotHandles(clientID) while sim.simxGetConnectionId(clientID) != -1: # Perception sonar = getSonar(clientID, hRobot) #print ('### s', sonar) blobs, coord = getImageBlob(clientID, hRobot) nspeed = 1.25 if blobs == 1: if coord[0] > 0.5: pd = abs(0.5 - coord[0]) / 0.5 pi = 0 else: pi = (0.5 - coord[0]) / 0.5 pd = 0 if coord[1] >= 0.6: res = 0.5 else: res = 0 print('pd= ', pd, 'pi= ', pi, 'Y= ', coord[1]) lspeed, rspeed = nspeed + (1.5 * pd) - res, nspeed + (1.5 * pi) - res else: lspeed, rspeed = avoid(sonar) #lspeed, rspeed = +1.5,+0 # Planning #lspeed, rspeed = avoid(sonar) # Action setSpeed(clientID, hRobot, lspeed, rspeed) time.sleep(0.1) print('### Finishing...') sim.simxFinish(clientID) print('### Program ended')
def streamVisionSensor(): # Mouse function def select_point(event, x, y, flags, params): global point, point_selected, old_points if event == cv2.EVENT_LBUTTONDOWN: point = (x, y) point_selected = True old_points = np.array([[x, y]], dtype=np.float32) print("TRU") cv2.namedWindow('frame') point_selected = False point = () old_points = np.array([[]]) #Get the handle of vision sensor errorCode, visionSensorHandle = sim.simxGetObjectHandle( clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait) #Get the image errorCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, sim.simx_opmode_streaming) time.sleep(0.5) errorCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, sim.simx_opmode_buffer) sensorImage = np.array(image, dtype=np.uint8) sensorImage.resize([resolution[1], resolution[0], 3]) old_image = sensorImage.copy() while (sim.simxGetConnectionId(clientID) != -1): cv2.setMouseCallback('frame', select_point) #Get the image of the vision sensor errorCode, resolution, image = sim.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, sim.simx_opmode_buffer) #Transform the image so it can be displayed using pyplot sensorImage = np.array(image, dtype=np.uint8) sensorImage.resize([resolution[1], resolution[0], 3]) displayedImage = cv2.resize(sensorImage, (480, 480)) if point_selected is True: print("coco") new_points, status, error = cv2.calcOpticalFlowPyrLK( old_image, sensorImage, old_points, None, **lk_params) old_image = sensorImage.copy() #current frame becomes previous old_points = new_points #current x,y points become previous x, y = new_points.ravel() cv2.circle(sensorImage, (x, y), 8, (0, 0, 0), -1) cv2.imshow('frame', displayedImage) if cv2.waitKey(30) & 0xFF == ord('q'): break cv2.destroyAllWindows() print('End of Simulation')
def collision(): errorCode, car_collision_handle = sim.simxGetCollisionHandle( clientID, 'Collision', sim.simx_opmode_oneshot_wait) number_collisions = 0 if errorCode != sim.simx_return_ok: print('Error: ', errorCode) while (sim.simxGetConnectionId(clientID) != -1): returnCode, collisionState = sim.simxReadCollision( clientID, car_collision_handle, sim.simx_opmode_streaming) if (collisionState != 0): number_collisions += 1 print("Collision {0} occurred".format(str(number_collisions)))
def update_status(self): """ check the status of the simulator """ self.status_has_changed = False self.is_online = False if sim.simxGetConnectionId( self.clientID) != -1 and self.clientID != -1: success, self.status = sim.simxGetIntegerSignal( self.clientID, 'Evo-Client Status', mode4) self.is_online = True if self.status != self.__old_status: self.status_has_changed = True self.__old_status = self.status # -1 = not ready/error, 0 = ready, 1 = active, 2 = done return self.status
def image_processor(clientID): pub = rospy.Publisher('offset', Float64, queue_size=10) rospy.init_node('image_processor_node') rate = rospy.Rate(10) res, handle_cam = sim.simxGetObjectHandle(clientID, "cam", sim.simx_opmode_blocking) while not rospy.is_shutdown(): err, resolution, frame = sim.simxGetVisionSensorImage( clientID, handle_cam, 0, sim.simx_opmode_streaming) if (sim.simxGetConnectionId(clientID) != -1): err, resolution, frame = sim.simxGetVisionSensorImage( clientID, handle_cam, 0, sim.simx_opmode_buffer) frame = frame[::-1] # original was bgr, I converted to rgb if err == sim.simx_return_ok: img = np.array(frame, dtype=np.uint8) img.resize([resolution[0], resolution[1], 3]) # detect red cube hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) l_b = np.array([0, 100, 100]) u_b = np.array([10, 255, 255]) mask = cv2.inRange(hsv, l_b, u_b) contours, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) if len(contours) != 0: (x, y, w, h) = cv2.boundingRect(contours[0]) x_medium = (2.0 * x + w) / 2.0 # find the center of the object object_center = ( resolution[0] ) / 2.0 - x_medium # calculate offset between object's center and the frame center pub.publish(object_center) #rospy.loginfo(res) #cv2.line(img, (int(x_medium), 0), (int(x_medium), resolution[0]), (0, 0, 255), 1) else: pub.publish(0.0) rate.sleep() #cv2.imshow('image', img) #if cv2.waitKey(1) & 0xFF == ord('q'): #break else: print("Failed to connect to the remote API server...") sim.simxFinish(clientID)
def recover(self): """ attempt to restore connection to a lost client """ if self.clientID != -1: # this check is necessary, # otherwise a client that was not online when created (ID == -1) would lead to simxFinish(-1), # which kills ALL connections sim.simxFinish(self.clientID) new_clientID = sim.simxStart(self.ip, self.port, False, True, client_connection_timeout, client_comm_cycle_time) # sim.simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, # timeOutInMs, commThreadCycleInMs) if sim.simxGetConnectionId(self.clientID) != -1 and new_clientID != -1: self.clientID = new_clientID self.health = self.health_initial self.is_online = True success = True else: success = False return success
# get vision sensor objects res, v0 = sim.simxGetObjectHandle(clientID, 'vs1', sim.simx_opmode_oneshot_wait) res, v1 = sim.simxGetObjectHandle(clientID, 'vs2', sim.simx_opmode_oneshot_wait) err, resolution, image = sim.simxGetVisionSensorImage( clientID, v0, 0, sim.simx_opmode_streaming) #get motor objects err, Ball_handle = sim.simxGetObjectHandle( clientID, 'Ball', sim.simx_opmode_oneshot_wait) err, player_x_handle = sim.simxGetObjectHandle( clientID, 'player_x_joint', sim.simx_opmode_oneshot_wait) err, player_y_handle = sim.simxGetObjectHandle( clientID, 'player_y_joint', sim.simx_opmode_oneshot_wait) #main loop while (sim.simxGetConnectionId(clientID) != -1): #get vision sensor image start = time.time() err, resolution, image = sim.simxGetVisionSensorImage( clientID, v0, 0, sim.simx_opmode_buffer) #print(len(image)) if err == sim.simx_return_ok: image_byte_array = array.array('b', image) image_buffer = I.frombuffer("RGB", (resolution[0], resolution[1]), bytes(image_byte_array), "raw", "RGB", 0, 1) img2 = numpy.asarray(image_buffer) #ColorRecognition if video_tranfer == 1: ret_yellow = color.track_yellow_object(img2)
res = vrep.simxSetObjectOrientation(clientID, handle, -1, ori0, vrep.simx_opmode_oneshot_wait) if (clientID != -1): print('Connected to remote API server') res, v0 = vrep.simxGetObjectHandle(clientID, 'Chessboard', vrep.simx_opmode_oneshot_wait) # res, vp = vrep.simxGetObjectParent(clientID, v0, vrep.simx_opmode_oneshot_wait) res, pos0 = vrep.simxGetObjectPosition(clientID, v0, -1, vrep.simx_opmode_oneshot_wait) res, ori0 = vrep.simxGetObjectOrientation(clientID, v0, -1, vrep.simx_opmode_oneshot_wait) print(pos0, ori0) while (vrep.simxGetConnectionId(clientID) != -1): #############################original############################# res = vrep.simxSetObjectPosition(clientID, v0, -1, pos0, vrep.simx_opmode_oneshot_wait) res = vrep.simxSetObjectOrientation(clientID, v0, -1, ori0, vrep.simx_opmode_oneshot_wait) time.sleep(t) rotation_combinations(v0) #############################layer 02############################# res = vrep.simxSetObjectPosition(clientID, v0, v0, (0, 0, -0.2), vrep.simx_opmode_oneshot_wait) # res = vrep.simxSetObjectOrientation(clientID, v0, -1, ori0, vrep.simx_opmode_oneshot_wait) time.sleep(t) rotation_combinations(v0)
def track_motion(self): res, resolution, frame1 = sim.simxGetVisionSensorImage( self.clientID, self.handle_cam, 0, sim.simx_opmode_streaming) # wait until frame is read correctly while res != sim.simx_return_ok: res, resolution, frame1 = sim.simxGetVisionSensorImage( self.clientID, self.handle_cam, 0, sim.simx_opmode_buffer) res, resolution, frame2 = sim.simxGetVisionSensorImage( self.clientID, self.handle_cam, 0, sim.simx_opmode_buffer) # convert from BGR to RGB, also the position of the image frame1 = frame1[::-1] frame1 = np.array(frame1, dtype=np.uint8) frame1.resize([resolution[0], resolution[1], 3]) frame2 = frame2[::-1] frame2 = np.array(frame2, dtype=np.uint8) frame2.resize([resolution[0], resolution[1], 3]) # Process the video and detect motion by subtracting subsequent frames while not rospy.is_shutdown(): if sim.simxGetConnectionId(self.clientID) != -1: diff = cv2.absdiff(frame1, frame2) gray = cv2.cvtColor(diff, cv2.COLOR_BGR2GRAY) blur = cv2.GaussianBlur(gray, (5, 5), 0) _, thresh = cv2.threshold(blur, 20, 255, cv2.THRESH_BINARY) dilated = cv2.dilate(thresh, None, iterations=3) contours, _ = cv2.findContours(dilated, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) counter = 0 centerX = 0 for contour in contours: (x, y, w, h) = cv2.boundingRect(contour) if w >= h or cv2.contourArea( contour ) < 1000: # because human's height is greater than width continue counter = counter + 1 centerX = centerX + (2.0 * x + w) / 2.0 #cv2.rectangle(frame1, (x, y), (x+w, y+h), (0, 255, 0), 1) if counter != 0: centerX = centerX / counter self.offset = (resolution[0] / 2.0) - centerX else: self.offset = 0.0 self.pub.publish( self.offset) # publish the offset from the center cv2.imshow("camera", frame1) # update variables frame1 = frame2 res, resolution, frame2 = sim.simxGetVisionSensorImage( self.clientID, self.handle_cam, 0, sim.simx_opmode_buffer) frame2 = frame2[::-1] frame2 = np.array(frame2, dtype=np.uint8) frame2.resize([resolution[0], resolution[1], 3]) if cv2.waitKey(40) == 27: break else: print("Failed to connect to the remote API server...") sim.simxFinish(self.clientID)
res_gnd, resolution_gnd, image_gnd = sim.simxGetVisionSensorImage( clientID_gnd, camera_gnd, 0, sim.simx_opmode_streaming) # Flags that allow each of the steps to excecute only when necessary isDroneCentered = False isMapReady = False isMovingToManta = False isLookingForBear = False isRescueDone = False isReadyToSearch = False isGoingBackToManta = False isReturningMapReady = False isWaitingForMap = False isGoingBackToHospital = False while (sim.simxGetConnectionId(clientID_drone) != -1) and (sim.simxGetConnectionId(clientID_gnd) != -1): # -------------------------------------- DRONE MOVES TO THE CENTRE OF THE SCENE -------------------------------------- # Moving drone to the center of the environment if (not isDroneCentered): drone_viewposition, repeatseed, isDroneCentered = fun.droneInitialMovement( clientID_drone, drone_base_handle, drone_target_handle, floor, drone_viewposition, repeatseed) # If we don't have a path if (not isMapReady) and isDroneCentered: # Give time for the drone to stabilize time.sleep(40) # Repeat until getting the instructions (commands) list
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 check_server_connection(self): return sim.simxGetConnectionId(self.clientId) != -1