-
Notifications
You must be signed in to change notification settings - Fork 0
/
vrepGA.py
89 lines (59 loc) · 3.53 KB
/
vrepGA.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
# -*- coding: UTF-8 -*-
import numpy as np
import vrep
import sys
import time
import obstaculosVRep as oVrep
import robotFunctions as RF
# Encerra conexoes previas
vrep.simxFinish(-1)
# Faz a conexao com o Vrep
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP
# Verifica se a conexao foi efetiva
if clientID != -1:
print("Conectado ao VRep!! Obaaaaa!!!")
vrep.simxAddStatusbarMessage(clientID, "Conexao estabelecida!", operationMode=vrep.simx_opmode_oneshot)
else:
print("Nao conectado ao VRep!!!")
sys.exit("Xau!!")
#Instancia objetos no Python para os handlers
codErro, ref = vrep.simxGetObjectHandle(clientID, 'ref',
operationMode=vrep.simx_opmode_oneshot)
#Destino
codErro, target = vrep.simxGetObjectHandle(clientID, 'target', operationMode=vrep.simx_opmode_oneshot)
#Medidores de colisão
codErro, pioneerColDetect = vrep.simxGetCollisionHandle(clientID, 'pioneerColDetect',
operationMode=vrep.simx_opmode_blocking)
#Medidores de distância
codErro, roboObstDist = vrep.simxGetDistanceHandle(clientID, 'roboObstDistDetect',
operationMode=vrep.simx_opmode_blocking)
codErro, roboTargetDist = vrep.simxGetDistanceHandle(clientID, 'roboTargetDistDetect',
operationMode=vrep.simx_opmode_blocking)
codErro, obstaculos = vrep.simxGetCollectionHandle(clientID, 'obstaculos#',
operationMode=vrep.simx_opmode_oneshot)
codErro, robo = vrep.simxGetObjectHandle(clientID, 'Pionee_p3dx',
operationMode=vrep.simx_opmode_oneshot)
codErro, motorE = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_blocking)
codErro, motorD = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_blocking)
codErro, colisao = vrep.simxReadCollision(clientID, pioneerColDetect,
operationMode=vrep.simx_opmode_streaming)
posXR = vrep.simxGetFloatSignal(clientID, 'posXR', vrep.simx_opmode_streaming)
posYR = vrep.simxGetFloatSignal(clientID, 'posYR', vrep.simx_opmode_streaming)
posX1 = vrep.simxGetFloatSignal(clientID, 'posX1', vrep.simx_opmode_streaming)
posY1 = vrep.simxGetFloatSignal(clientID, 'posY1', vrep.simx_opmode_streaming)
posX2 = vrep.simxGetFloatSignal(clientID, 'posX2', vrep.simx_opmode_streaming)
posY2 = vrep.simxGetFloatSignal(clientID, 'posY2', vrep.simx_opmode_streaming)
posX3 = vrep.simxGetFloatSignal(clientID, 'posX3', vrep.simx_opmode_streaming)
posY3 = vrep.simxGetFloatSignal(clientID, 'posY3', vrep.simx_opmode_streaming)
posX4 = vrep.simxGetFloatSignal(clientID, 'posX4', vrep.simx_opmode_streaming)
posY4 = vrep.simxGetFloatSignal(clientID, 'posY4', vrep.simx_opmode_streaming)
posX5 = vrep.simxGetFloatSignal(clientID, 'posX5', vrep.simx_opmode_streaming)
posY5 = vrep.simxGetFloatSignal(clientID, 'posY5', vrep.simx_opmode_streaming)
posX6 = vrep.simxGetFloatSignal(clientID, 'posX6', vrep.simx_opmode_streaming)
posY6 = vrep.simxGetFloatSignal(clientID, 'posY6', vrep.simx_opmode_streaming)
#Grava posição dos obstaculos
mapaObstaculos = oVrep.carregaObstaculos(clientID)
posRobo = RF.getRobotPosition(clientID)
distRO = vrep.simxReadDistance(clientID, roboObstDist, operationMode=vrep.simx_opmode_blocking)
distRT = vrep.simxReadDistance(clientID, roboTargetDist, operationMode=vrep.simx_opmode_blocking)
print mapaObstaculos